680 lines
30 KiB
C++
Executable File
680 lines
30 KiB
C++
Executable File
#include"custom_planner/merge_path_calc.h"
|
|
|
|
namespace custom_planner {
|
|
|
|
MergePathCalc::MergePathCalc(){
|
|
curve_design = new Curve_common();
|
|
spline_inf = new Spline_Inf();
|
|
}
|
|
MergePathCalc::~MergePathCalc(){
|
|
// Giải phóng bộ nhớ
|
|
delete curve_design;
|
|
delete spline_inf;
|
|
}
|
|
|
|
std::vector<robot_geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points, bool reverse)
|
|
{
|
|
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path;
|
|
std::vector<robot_geometry_msgs::PoseStamped> saved_poses; // Lưu các pose đã tính toán
|
|
|
|
if((int)nurbs_path.size() > 0)
|
|
{
|
|
nurbs_path.clear(); // Xóa đường cong cũ nếu có
|
|
saved_poses.clear(); // Xóa các pose đã lưu
|
|
}
|
|
|
|
// Chuyển đổi control points sang định dạng Eigen::Vector3d
|
|
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> eigen_control_points;
|
|
for(const auto& cp : control_points)
|
|
{
|
|
Eigen::Vector3d point(cp.pose.position.x, cp.pose.position.y, 0);
|
|
eigen_control_points.push_back(point);
|
|
}
|
|
// Tạo knot vector tự động
|
|
std::vector<double> knot_vector;
|
|
int degree = 2;
|
|
int n = control_points.size() - 1; // n + 1 control points
|
|
int m = n + degree + 1; // m + 1 knots
|
|
// Tạo uniform knot vector
|
|
for(int i = 0; i <= m; i++)
|
|
{
|
|
if(i <= degree) knot_vector.push_back(0.0);
|
|
else knot_vector.push_back(1.0);
|
|
// else knot_vector.push_back((double)(i - degree)/(m - 2*degree));
|
|
}
|
|
|
|
// Điều chỉnh weights để tăng ảnh hưởng của các control points
|
|
std::vector<double> weights(control_points.size(), 1.0);
|
|
|
|
spline_inf->knot_vector.clear();
|
|
spline_inf->control_point.clear();
|
|
spline_inf->weight.clear();
|
|
|
|
// Cài đặt thông số cho spline
|
|
curve_design->ReadSplineInf(spline_inf, degree + 1, eigen_control_points, knot_vector);
|
|
curve_design->ReadSplineInf(spline_inf, weights, false);
|
|
|
|
// Tính độ dài đường cong để xác định số điểm cần lấy mẫu
|
|
double NURBS_length = calculateNURBSLength(curve_design, spline_inf, 0.01);
|
|
double desired_spacing = 0.05; // Khoảng cách mong muốn giữa các điểm
|
|
double step = calculateAdaptiveStep(NURBS_length, desired_spacing);
|
|
|
|
for(double t = 0.0; t <= 1.0; t += step)
|
|
{
|
|
robot_geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
|
|
if(!std::isnan(point.x) && !std::isnan(point.y))
|
|
{
|
|
robot_geometry_msgs::PoseStamped pose;
|
|
pose.header.stamp = robot::Time::now();
|
|
pose.pose.position = point;
|
|
pose.pose.orientation.x = 0.0;
|
|
pose.pose.orientation.y = 0.0;
|
|
pose.pose.orientation.z = 0.0;
|
|
pose.pose.orientation.w = 1.0;
|
|
|
|
// nurbs_path.push_back(pose);
|
|
saved_poses.push_back(pose);
|
|
if(saved_poses.size() > 1)
|
|
{
|
|
updatePoseOrientation(saved_poses, nurbs_path, reverse);
|
|
}
|
|
}
|
|
}
|
|
if(!saved_poses.empty())
|
|
{
|
|
double end_yaw;
|
|
if(!reverse)
|
|
end_yaw = calculateAngle(saved_poses.back().pose.position.x,
|
|
saved_poses.back().pose.position.y,
|
|
control_points.back().pose.position.x,
|
|
control_points.back().pose.position.y);
|
|
else
|
|
end_yaw = calculateAngle(control_points.back().pose.position.x,
|
|
control_points.back().pose.position.y,
|
|
saved_poses.back().pose.position.x,
|
|
saved_poses.back().pose.position.y);
|
|
end_yaw = normalizeAngle(end_yaw);
|
|
|
|
tf3::Quaternion q;
|
|
q.setRPY(0, 0, end_yaw);
|
|
saved_poses.back().pose.orientation.x = q.x();
|
|
saved_poses.back().pose.orientation.y = q.y();
|
|
saved_poses.back().pose.orientation.z = q.z();
|
|
saved_poses.back().pose.orientation.w = q.w();
|
|
|
|
nurbs_path.push_back(saved_poses.back());
|
|
}
|
|
return nurbs_path;
|
|
}
|
|
|
|
double MergePathCalc::calculateNURBSLength(Curve_common* curve_design,
|
|
Spline_Inf* spline_inf,
|
|
double initial_step)
|
|
{
|
|
double length = 0.0;
|
|
robot_geometry_msgs::Point prev_point, curr_point;
|
|
std::vector<double> segment_lengths;
|
|
|
|
static double step;
|
|
// Đường cong bậc 2 trở lên thì bước nhảy bằng initial_step
|
|
step = initial_step;
|
|
|
|
// Tính độ dài với step
|
|
for(double u = 0; u <= 1; u += step)
|
|
{
|
|
// Lấy điểm trên đường cong tại tham số u
|
|
curr_point = curve_design->CalculateCurvePoint(spline_inf, u, true);
|
|
|
|
if(u > 0 && !std::isnan(curr_point.x) && !std::isnan(curr_point.y))
|
|
{
|
|
double segment_length = std::sqrt(std::pow(curr_point.x - prev_point.x, 2) +
|
|
std::pow(curr_point.y - prev_point.y, 2));
|
|
length += segment_length;
|
|
segment_lengths.push_back(segment_length);
|
|
}
|
|
prev_point = curr_point;
|
|
}
|
|
return length;
|
|
}
|
|
|
|
void MergePathCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
|
|
std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
|
|
bool reverse)
|
|
{
|
|
double yaw;
|
|
// ROS_WARN("[merge_path_calc] DEBUG: reverse(%x)", reverse);
|
|
if(!reverse)
|
|
yaw = calculateAngle(saved_poses[saved_poses.size() - 2].pose.position.x,
|
|
saved_poses[saved_poses.size() - 2].pose.position.y,
|
|
saved_poses.back().pose.position.x,
|
|
saved_poses.back().pose.position.y);
|
|
else
|
|
yaw = calculateAngle(saved_poses.back().pose.position.x,
|
|
saved_poses.back().pose.position.y,
|
|
saved_poses[saved_poses.size() - 2].pose.position.x,
|
|
saved_poses[saved_poses.size() - 2].pose.position.y);
|
|
|
|
yaw = normalizeAngle(yaw);
|
|
tf3::Quaternion q;
|
|
q.setRPY(0, 0, yaw);
|
|
saved_poses[saved_poses.size() - 2].pose.orientation.x = q.x();
|
|
saved_poses[saved_poses.size() - 2].pose.orientation.y = q.y();
|
|
saved_poses[saved_poses.size() - 2].pose.orientation.z = q.z();
|
|
saved_poses[saved_poses.size() - 2].pose.orientation.w = q.w();
|
|
|
|
nurbs_path.push_back(saved_poses[saved_poses.size() - 2]);
|
|
}
|
|
|
|
int MergePathCalc::findNearestPointOnPath(Pose& pose,
|
|
std::vector<Pose>& posesOnPathWay,
|
|
point_type type)
|
|
{
|
|
// Input validation - FIX: Thêm validation cơ bản
|
|
if (posesOnPathWay.empty()) {
|
|
return -1;
|
|
}
|
|
|
|
// Các hằng số chung cho cả START và GOAL
|
|
static const double MAX_DISTANCE = 1.0; // Ngưỡng khoảng cách tối đa (m)
|
|
static const double MIN_DISTANCE = 0.1; // Khoảng cách tối thiểu (m)
|
|
static const double MIN_SCALE = 0.2; // Giá trị tối thiểu của hệ số
|
|
static const double MAX_SCALE = 1.0; // Giá trị tối đa của hệ số
|
|
static const double MAX_ANGLE_THRESHOLD = 0.77 * M_PI; // Ngưỡng góc tối đa
|
|
|
|
int nearest_idx = 0;
|
|
|
|
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 (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
|
|
return -1;
|
|
}
|
|
|
|
// === BƯỚC 1: TÌM ĐIỂM GẦN NHẤT CƠ BẢN ===
|
|
double min_dist = std::numeric_limits<double>::max();
|
|
double distance;
|
|
|
|
for (int i = 0; i < idx_check; ++i)
|
|
{
|
|
distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(),
|
|
posesOnPathWay[i].getY() - pose.getY());
|
|
|
|
if (distance < min_dist)
|
|
{
|
|
min_dist = distance;
|
|
nearest_idx = i;
|
|
}
|
|
}
|
|
|
|
// Lưu chỉ mục của điểm gần nhất
|
|
store_start_nearest_idx_ = nearest_idx;
|
|
|
|
// === BƯỚC 2: TÍNH TOÁN HỆ SỐ SCALE ĐỘNG ===
|
|
// Nội suy tuyến tính ngược: khoảng cách giảm → scale tăng
|
|
double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) *
|
|
(MAX_DISTANCE - std::min(min_dist, MAX_DISTANCE)) /
|
|
(MAX_DISTANCE - MIN_DISTANCE);
|
|
|
|
// === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC ===
|
|
double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI;
|
|
if(angle_threshold >= MAX_ANGLE_THRESHOLD)
|
|
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 <= idx_check; i++)
|
|
{
|
|
// Bounds checking đầy đủ trước khi truy cập
|
|
if (i + 1 < static_cast<int>(posesOnPathWay.size()) && i + 1 <= idx_check)
|
|
{
|
|
double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i + 1]);
|
|
|
|
if(std::abs(angle) >= angle_threshold)
|
|
{
|
|
nearest_idx = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
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
|
|
}
|
|
}
|
|
}
|
|
else if(type == GOAL)
|
|
{
|
|
// FIX: Validate edge_back_start_idx_ trước khi sử dụng
|
|
if (edge_back_start_idx_ < 0 || edge_back_start_idx_ >= static_cast<int>(posesOnPathWay.size())) {
|
|
return -1;
|
|
}
|
|
|
|
// === BƯỚC 1: TÌM ĐIỂM GẦN NHẤT CƠ BẢN (DUYỆT NGƯỢC) ===
|
|
double min_dist = std::numeric_limits<double>::max();
|
|
|
|
for (int i = static_cast<int>(posesOnPathWay.size()) - 1; i >= edge_back_start_idx_; --i)
|
|
{
|
|
double distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(),
|
|
posesOnPathWay[i].getY() - pose.getY());
|
|
if (distance < min_dist)
|
|
{
|
|
min_dist = distance;
|
|
nearest_idx = i;
|
|
}
|
|
}
|
|
|
|
// Lưu chỉ mục của điểm gần nhất
|
|
store_goal_nearest_idx_ = nearest_idx;
|
|
|
|
// === BƯỚC 2: TÍNH TOÁN HỆ SỐ SCALE ĐỘNG (GIỐNG START) ===
|
|
// Nội suy tuyến tính ngược: khoảng cách giảm → scale tăng
|
|
double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) *
|
|
(MAX_DISTANCE - std::min(min_dist, MAX_DISTANCE)) /
|
|
(MAX_DISTANCE - MIN_DISTANCE);
|
|
|
|
// === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC (GIỐNG START) ===
|
|
double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI;
|
|
if(angle_threshold >= MAX_ANGLE_THRESHOLD)
|
|
angle_threshold = MAX_ANGLE_THRESHOLD;
|
|
|
|
|
|
// === BƯỚC 4: TÌM ĐIỂM THỎA MÃN ĐIỀU KIỆN GÓC (DUYỆT NGƯỢC) ===
|
|
// Bắt đầu từ điểm đã tìm được, duyệt ngược để tìm điểm thỏa mãn góc
|
|
for(int i = nearest_idx; i >= edge_back_start_idx_; i--)
|
|
{
|
|
// FIX: Bounds checking đầy đủ trước khi truy cập
|
|
if (i - 1 >= edge_back_start_idx_ && i - 1 >= 0)
|
|
{
|
|
double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i - 1]);
|
|
|
|
if(std::abs(angle) >= angle_threshold)
|
|
{
|
|
nearest_idx = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// FIX: Logic condition sửa lại để rõ ràng
|
|
if(i <= edge_back_start_idx_ + 1)
|
|
{
|
|
// Đã đến đầu mà không tìm thấy điểm phù hợp
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
}
|
|
// FIX: Thêm else case để handle invalid type
|
|
else {
|
|
return -1;
|
|
}
|
|
|
|
return nearest_idx;
|
|
}
|
|
|
|
bool MergePathCalc::findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
|
|
const robot_geometry_msgs::PoseStamped& pose,
|
|
std::vector<Pose>& posesOnPathWay,
|
|
point_type type)
|
|
{
|
|
// FIX: Input validation
|
|
if (posesOnPathWay.empty()) return false;
|
|
|
|
// FIX: Chỉ clear một lần duy nhất thay vì clear nhiều lần
|
|
control_points.clear();
|
|
|
|
if(type == START)
|
|
{
|
|
Pose start_pose;
|
|
start_pose.setX(pose.pose.position.x);
|
|
start_pose.setY(pose.pose.position.y);
|
|
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 idx_check trước khi sử dụng
|
|
if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
|
|
return false;
|
|
}
|
|
|
|
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();
|
|
|
|
// Thêm điểm đầu
|
|
control_points.push_back(pose);
|
|
|
|
// Với bậc 2, thêm 1 điểm trung gian nằm trên đường thẳng
|
|
robot_geometry_msgs::PoseStamped mid_pose;
|
|
mid_pose.pose.position.x = start_pose.getX() + dx/2.0;
|
|
mid_pose.pose.position.y = start_pose.getY() + dy/2.0;
|
|
mid_pose.pose.orientation = pose.pose.orientation;
|
|
// FIX: Copy header từ pose gốc
|
|
mid_pose.header = pose.header;
|
|
control_points.push_back(mid_pose);
|
|
|
|
// Thêm điểm cuối
|
|
robot_geometry_msgs::PoseStamped end_pose;
|
|
end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
|
|
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
|
|
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
|
|
// FIX: Copy header từ pose gốc
|
|
end_pose.header = pose.header;
|
|
control_points.push_back(end_pose);
|
|
|
|
return true;
|
|
}
|
|
// Tìm đường cong
|
|
else
|
|
{
|
|
start_target_idx_ = idx;
|
|
|
|
// Tạo các control point cho NURBS
|
|
control_points.push_back(pose);
|
|
|
|
robot_geometry_msgs::PoseStamped mid_pose;
|
|
mid_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
|
|
mid_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
|
|
mid_pose.pose.orientation = pose.pose.orientation;
|
|
// FIX: Copy header từ pose gốc
|
|
mid_pose.header = pose.header;
|
|
control_points.push_back(mid_pose);
|
|
|
|
|
|
double dx_sm = start_pose.getX() - posesOnPathWay[start_target_idx_].getX();
|
|
double dy_sm = start_pose.getY() - posesOnPathWay[start_target_idx_].getY();
|
|
double dist_sm = std::hypot(dx_sm,dy_sm);
|
|
|
|
// FIX: Bỏ biến end_idx_ không sử dụng
|
|
// end_idx_ = start_target_idx_;
|
|
|
|
bool found_suitable_point = false; // FIX: Thêm flag để tránh goto
|
|
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())) {
|
|
break;
|
|
}
|
|
|
|
double dx_me = posesOnPathWay[i].getX() - posesOnPathWay[start_target_idx_].getX();
|
|
double dy_me = posesOnPathWay[i].getY() - posesOnPathWay[start_target_idx_].getY();
|
|
double dist_me = std::hypot(dx_me,dy_me);
|
|
if(dist_me >= dist_sm)
|
|
{
|
|
start_target_idx_ = i;
|
|
found_suitable_point = true;
|
|
break;
|
|
}
|
|
if(i == idx_check)
|
|
{
|
|
// FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
|
|
found_suitable_point = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// FIX: Xử lý trường hợp không tìm thấy điểm phù hợp bằng cách tạo straight line
|
|
if (!found_suitable_point) {
|
|
// 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 (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
|
|
return false;
|
|
}
|
|
|
|
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();
|
|
|
|
control_points.push_back(pose);
|
|
|
|
robot_geometry_msgs::PoseStamped mid_pose_straight;
|
|
mid_pose_straight.pose.position.x = start_pose.getX() + dx/2.0;
|
|
mid_pose_straight.pose.position.y = start_pose.getY() + dy/2.0;
|
|
mid_pose_straight.pose.orientation = pose.pose.orientation;
|
|
mid_pose_straight.header = pose.header;
|
|
control_points.push_back(mid_pose_straight);
|
|
|
|
robot_geometry_msgs::PoseStamped end_pose_straight;
|
|
end_pose_straight.pose.position.x = posesOnPathWay[start_target_idx_].getX();
|
|
end_pose_straight.pose.position.y = posesOnPathWay[start_target_idx_].getY();
|
|
end_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
|
|
end_pose_straight.header = pose.header;
|
|
control_points.push_back(end_pose_straight);
|
|
|
|
return true;
|
|
}
|
|
|
|
robot_geometry_msgs::PoseStamped end_pose;
|
|
end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
|
|
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
|
|
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
|
|
// FIX: Copy header từ pose gốc
|
|
end_pose.header = pose.header;
|
|
control_points.push_back(end_pose);
|
|
|
|
return true;
|
|
}
|
|
}
|
|
else if(type == GOAL)
|
|
{
|
|
Pose goal_pose;
|
|
goal_pose.setX(pose.pose.position.x);
|
|
goal_pose.setY(pose.pose.position.y);
|
|
goal_pose.setYaw(data_convert::getYaw(pose.pose.orientation));
|
|
|
|
int idx = findNearestPointOnPath(goal_pose, posesOnPathWay, GOAL);
|
|
|
|
if(idx == -1)
|
|
{
|
|
// Trường hợp tạo đường thẳng
|
|
// FIX: Validate size trước khi sử dụng
|
|
if (posesOnPathWay.empty()) {
|
|
return false;
|
|
}
|
|
|
|
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
|
|
|
|
// Thêm điểm đầu từ đường đã có
|
|
robot_geometry_msgs::PoseStamped start_pose;
|
|
start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
|
|
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
|
|
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
|
|
// FIX: Copy header từ pose gốc
|
|
start_pose.header = pose.header;
|
|
control_points.push_back(start_pose);
|
|
|
|
// Vector hướng từ goal tới start
|
|
double dx = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
|
|
double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
|
|
|
|
// Thêm 1 điểm trung gian cho bậc 2
|
|
robot_geometry_msgs::PoseStamped mid_pose;
|
|
mid_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx/2.0;
|
|
mid_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy/2.0;
|
|
mid_pose.pose.orientation = start_pose.pose.orientation;
|
|
// FIX: Copy header từ pose gốc
|
|
mid_pose.header = pose.header;
|
|
control_points.push_back(mid_pose);
|
|
|
|
// Thêm điểm goal
|
|
control_points.push_back(pose);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
// Trường hợp tạo đường cong
|
|
goal_target_idx_ = idx;
|
|
|
|
double dx = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
|
|
double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
|
|
double dist_me = std::hypot(dx, dy);
|
|
|
|
bool found_suitable_point = false; // FIX: Thêm flag để tránh goto
|
|
for(int i = goal_target_idx_; i >= edge_back_start_idx_; i--)
|
|
{
|
|
// FIX: Validate bounds trước khi truy cập
|
|
if (i < 0 || i >= static_cast<int>(posesOnPathWay.size())) {
|
|
break;
|
|
}
|
|
|
|
double dx_sm = posesOnPathWay[i].getX() - posesOnPathWay[goal_target_idx_].getX();
|
|
double dy_sm = posesOnPathWay[i].getY() - posesOnPathWay[goal_target_idx_].getY();
|
|
double dist_sm = std::hypot(dx_sm,dy_sm);
|
|
if(dist_sm >= dist_me)
|
|
{
|
|
goal_target_idx_ = i;
|
|
found_suitable_point = true;
|
|
break;
|
|
}
|
|
if(i == edge_back_start_idx_)
|
|
{
|
|
// FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
|
|
found_suitable_point = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// FIX: Xử lý trường hợp không tìm thấy điểm phù hợp bằng cách tạo straight line
|
|
if (!found_suitable_point) {
|
|
// Clear và tạo straight line thay vì goto
|
|
control_points.clear();
|
|
|
|
// FIX: Validate lại size
|
|
if (posesOnPathWay.empty()) {
|
|
return false;
|
|
}
|
|
|
|
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
|
|
|
|
robot_geometry_msgs::PoseStamped start_pose_straight;
|
|
start_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
|
|
start_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
|
|
start_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
|
|
start_pose_straight.header = pose.header;
|
|
control_points.push_back(start_pose_straight);
|
|
|
|
double dx_straight = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
|
|
double dy_straight = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
|
|
|
|
robot_geometry_msgs::PoseStamped mid_pose_straight;
|
|
mid_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx_straight/2.0;
|
|
mid_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy_straight/2.0;
|
|
mid_pose_straight.pose.orientation = start_pose_straight.pose.orientation;
|
|
mid_pose_straight.header = pose.header;
|
|
control_points.push_back(mid_pose_straight);
|
|
|
|
control_points.push_back(pose);
|
|
return true;
|
|
}
|
|
|
|
// Thêm điểm đầu từ đường đã có
|
|
robot_geometry_msgs::PoseStamped start_pose;
|
|
start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
|
|
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
|
|
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
|
|
// FIX: Copy header từ pose gốc
|
|
start_pose.header = pose.header;
|
|
control_points.push_back(start_pose);
|
|
|
|
robot_geometry_msgs::PoseStamped mid_pose;
|
|
mid_pose.pose.position.x = posesOnPathWay[idx].getX();
|
|
mid_pose.pose.position.y = posesOnPathWay[idx].getY();
|
|
mid_pose.pose.orientation = pose.pose.orientation;
|
|
// FIX: Copy header từ pose gốc
|
|
mid_pose.header = pose.header;
|
|
control_points.push_back(mid_pose);
|
|
|
|
// Thêm điểm goal
|
|
control_points.push_back(pose);
|
|
return true;
|
|
}
|
|
}
|
|
// FIX: Thêm else case để handle invalid type
|
|
else {
|
|
return false;
|
|
}
|
|
|
|
// FIX: Code không bao giờ reach được đây, nhưng để đảm bảo
|
|
// return false; // Không tìm thấy control points
|
|
}
|
|
|
|
double MergePathCalc::signedAngle(Pose& pose, std::vector<Pose>& posesOnPathWay,
|
|
point_type type, int& idx)
|
|
{
|
|
if(type == START)
|
|
{
|
|
// Vector AB
|
|
double bax = pose.getX() - posesOnPathWay[idx].getX();
|
|
double bay = pose.getY() - posesOnPathWay[idx].getY();
|
|
|
|
// Vector BC
|
|
double bcx = posesOnPathWay[idx+1].getX() - posesOnPathWay[idx].getX();
|
|
double bcy = posesOnPathWay[idx+1].getY() - posesOnPathWay[idx].getY();
|
|
|
|
// Tích vô hướng
|
|
double dot = bax * bcx + bay * bcy;
|
|
// Tích hướng 2D (z-component)
|
|
double cross = bax * bcy - bay * bcx;
|
|
|
|
// Góc có hướng [-π, π]
|
|
double angle_rad = std::atan2(cross, dot);
|
|
return angle_rad;
|
|
}
|
|
else if(type == GOAL)
|
|
{
|
|
// Vector AB
|
|
double bax = posesOnPathWay[idx-1].getX() - posesOnPathWay[idx].getX();
|
|
double bay = posesOnPathWay[idx-1].getY() - posesOnPathWay[idx].getY();
|
|
|
|
// Vector BC
|
|
double bcx = pose.getX() - posesOnPathWay[idx].getX();
|
|
double bcy = pose.getY() - posesOnPathWay[idx].getY();
|
|
|
|
// Tích vô hướng
|
|
double dot = bax * bcx + bay * bcy;
|
|
// Tích hướng 2D (z-component)
|
|
double cross = bax * bcy - bay * bcx;
|
|
|
|
// Góc có hướng [-π, π]
|
|
double angle_rad = std::atan2(cross, dot);
|
|
return angle_rad;
|
|
}
|
|
else return -1; // Trả về -1 nếu không phải START hoặc GOAL
|
|
|
|
}
|
|
|
|
double MergePathCalc::signedAngle(Pose& start_pose, Pose& mid_pose, Pose& end_pose)
|
|
{
|
|
// Vector AB
|
|
double bax = mid_pose.getX() - start_pose.getX();
|
|
double bay = mid_pose.getY() - start_pose.getY();
|
|
|
|
// Vector BC
|
|
double bcx = mid_pose.getX() - end_pose.getX();
|
|
double bcy = mid_pose.getY() - end_pose.getY();
|
|
|
|
// Tích vô hướng
|
|
double dot = bax * bcx + bay * bcy;
|
|
// Tích hướng 2D (z-component)
|
|
double cross = bax * bcy - bay * bcx;
|
|
|
|
// Góc có hướng [-π, π]
|
|
double angle_rad = std::atan2(cross, dot);
|
|
return angle_rad;
|
|
}
|
|
|
|
} // namespace custom_planner
|