DuongTD/custom_planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

optimal

Browse Source
This commit is contained in:
duongtd
2026-01-30 10:56:41 +07:00
parent 49afcce5b2
commit e0f6738c31
5 changed files with 116 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
src/merge_path_calc.cpp
View File

@@ -185,8 +185,10 @@ namespace custom_planner {
if(type == START)
{
// int idx_check = edge_front_end_idx_;
int idx_check = static_cast<int>(posesOnPathWay.size());
// FIX: Validate edge_front_end_idx_ trước khi sử dụng
if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast<int>(posesOnPathWay.size())) {
if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
return -1;
}
@@ -194,7 +196,7 @@ namespace custom_planner {
double min_dist = std::numeric_limits<double>::max();
double distance;
for (int i = 0; i < edge_front_end_idx_; ++i)
for (int i = 0; i < idx_check; ++i)
{
distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(),
posesOnPathWay[i].getY() - pose.getY());
@@ -221,10 +223,10 @@ namespace custom_planner {
angle_threshold = MAX_ANGLE_THRESHOLD;
// === BƯỚC 4: TÌM ĐIỂM THỎA MÃN ĐIỀU KIỆN GÓC ===
for(int i = store_start_nearest_idx_; i <= edge_front_end_idx_; i++)
for(int i = store_start_nearest_idx_; i <= idx_check; i++)
{
// Bounds checking đầy đủ trước khi truy cập
if (i + 1 < static_cast<int>(posesOnPathWay.size()) && i + 1 <= edge_front_end_idx_)
if (i + 1 < static_cast<int>(posesOnPathWay.size()) && i + 1 <= idx_check)
{
double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i + 1]);
@@ -236,7 +238,7 @@ namespace custom_planner {
}
if(i >= edge_front_end_idx_ - 1)
if(i >= idx_check - 1)
{
// Đã đến cuối mà không tìm thấy điểm phù hợp
return -1; // Không tìm thấy điểm phù hợp
@@ -331,16 +333,19 @@ namespace custom_planner {
start_pose.setYaw(data_convert::getYaw(pose.pose.orientation));
int idx = findNearestPointOnPath(start_pose, posesOnPathWay, START);
// int idx_check = edge_front_end_idx_;
int idx_check = static_cast<int>(posesOnPathWay.size());
// Trường hợp tạo đường thẳng
if(idx == -1)
{
// FIX: Validate edge_front_end_idx_ trước khi sử dụng
if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast<int>(posesOnPathWay.size())) {
// FIX: Validate idx_check trước khi sử dụng
if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
return false;
}
start_target_idx_ = edge_front_end_idx_ - 1;
start_target_idx_ = idx_check - 1;
double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();
@@ -393,7 +398,7 @@ namespace custom_planner {
// end_idx_ = start_target_idx_;
bool found_suitable_point = false; // FIX: Thêm flag để tránh goto
for(int i = start_target_idx_; i <= edge_front_end_idx_; i++)
for(int i = start_target_idx_; i <= idx_check; i++)
{
// FIX: Validate bounds trước khi truy cập
if (i >= static_cast<int>(posesOnPathWay.size())) {
@@ -409,7 +414,7 @@ namespace custom_planner {
found_suitable_point = true;
break;
}
if(i == edge_front_end_idx_)
if(i == idx_check)
{
// FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
found_suitable_point = false;
@@ -422,12 +427,14 @@ namespace custom_planner {
// Clear và tạo straight line thay vì goto
control_points.clear();
// int idx_check = edge_front_end_idx_;
int idx_check = static_cast<int>(posesOnPathWay.size());
// FIX: Validate lại edge_front_end_idx_
if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast<int>(posesOnPathWay.size())) {
if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
return false;
}
start_target_idx_ = edge_front_end_idx_ - 1;
start_target_idx_ = idx_check - 1;
double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();