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

first commit

Browse Source
This commit is contained in:
duongtd
2025-12-19 16:45:50 +07:00
commit 377ebe3d6f
15 changed files with 5090 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

673
src/merge_path_calc.cpp Executable file
View File

@@ -0,0 +1,673 @@
#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<geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<geometry_msgs::PoseStamped>& control_points, bool reverse)
{
std::vector<geometry_msgs::PoseStamped> nurbs_path;
std::vector<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)
{
geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
if(!std::isnan(point.x) && !std::isnan(point.y))
{
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;
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<geometry_msgs::PoseStamped>& saved_poses,
std::vector<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)
{
// 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())) {
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 < edge_front_end_idx_; ++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 <= edge_front_end_idx_; i++)
{
// Bounds checking đầy đủ trước khi truy cập
if (i + 1 < static_cast<int>(posesOnPathWay.size()) && i + 1 <= edge_front_end_idx_)
{
double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i + 1]);
if(std::abs(angle) >= angle_threshold)
{
nearest_idx = i;
break;
}
}
if(i >= edge_front_end_idx_ - 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<geometry_msgs::PoseStamped>& control_points,
const 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);
// 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())) {
return false;
}
start_target_idx_ = edge_front_end_idx_ - 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
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
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);
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 <= edge_front_end_idx_; 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 == edge_front_end_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 edge_front_end_idx_
if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast<int>(posesOnPathWay.size())) {
return false;
}
start_target_idx_ = edge_front_end_idx_ - 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);
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);
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;
}
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ó
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
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;
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();
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ó
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);
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