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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
include/custom_planner/custom_planner.h
View File

@ -30,17 +30,7 @@ using namespace std;
#include "custom_planner/merge_path_calc.h"
#include <robot_geometry_msgs/PoseArray.h>
#include <thread>
#include <boost/thread.hpp>
#include <robot_protocol_msgs/Order.h>
// #include "vda5050_msgs/Order.h"
// #include "vda5050_msgs/Trajectory.h"
// #include "vda5050_msgs/Edge.h"
// #include "vda5050_msgs/Node.h"
// #include "vda5050_msgs/ControlPoint.h"
// #include "vda5050_msgs/NodePosition.h"
const double EPSILON = 1e-6;
@ -115,15 +105,6 @@ private:
bool countRobotDirectionChangeAngle(vector<Pose>& savePosesOnEdge, int& total_edge);
inline double calculateAngle(double xA, double yA, double xB, double yB)
{
double deltaX = xB - xA;
double deltaY = yB - yA;
double angleRad = atan2(deltaY, deltaX);
// double angleDeg = angleRad * 180.0 / M_PI;
return angleRad;
}
inline bool isOppositeSign(double angleA, double angleB)
{
return (angleA > 0 && angleB < 0) || (angleA < 0 && angleB > 0);
@ -173,41 +154,12 @@ private:
bool skipEdge_flag_ = false;
bool reverse_ = false;
Spline_Inf* input_spline_inf;
Curve_common* CurveDesign;
Pathway* pathway;
Pose* startPose;
vector<Pose> posesOnPathWay;
Pose start_on_path;
std::map<string, OrderNode> orderNodes;
Spline_Inf* input_spline_inf_;
Curve_common* CurveDesign_;
vector<Pose> posesOnPathWay_;
// vda5050_msgs::Order order_msg_;
uint16_t start_on_path_index;
bool initialized_;
bool rotating_robot_plag_ = false; /**< whether the robot is rotating or not, used to determine the direction of the path */
std::string planner_type_; /**< sbpl method to use for planning. choices are ARAPlanner and ADPlanner */
double allocated_time_; /**< amount of time allowed for search */
double initial_epsilon_; /**< initial epsilon for beginning the anytime search */
std::string environment_type_; /** what type of environment in which to plan. choices are 2D and XYThetaLattice. */
std::string cost_map_topic_; /** what topic is being used for the costmap topic */
bool forward_search_; /** whether to use forward or backward search */
std::string primitive_filename_; /** where to find the motion primitives for the current robot */
int force_scratch_limit_; /** the number of cells that have to be changed in the costmap to force the planner to plan from scratch even if its an incremental planner */
unsigned char lethal_obstacle_;
unsigned char inscribed_inflated_obstacle_;
unsigned char circumscribed_cost_;
unsigned char sbpl_cost_multiplier_;
bool publish_footprint_path_;
int visualizer_skip_poses_;
bool allow_unknown_;
std::string name_;
robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
@ -215,13 +167,6 @@ private:
unsigned int current_env_width_;
unsigned int current_env_height_;
// ros::Subscriber order_msg_sub_;
// ros::Publisher plan_pub_;
// ros::Publisher stats_publisher_;
// vector<ros::ServiceServer> service_servers_;
// ros::Publisher sbpl_plan_footprint_pub_;
boost::mutex mutex_;
};
}

12
include/custom_planner/custom_planner_config.h Normal file
View File

@ -0,0 +1,12 @@
#ifndef CUSTOM_PLANNER_CONFIG_H_
#define CUSTOM_PLANNER_CONFIG_H_
#include <robot/robot.h>
namespace custom_planner
{
// struct
}
#endif // CUSTOM_PLANNER_CONFIG_H_

179
src/custom_planner.cpp
View File

@ -28,31 +28,8 @@ namespace custom_planner
robot::NodeHandle p_nh("~");
printf("Name is %s", name.c_str());
pathway = new Pathway();
startPose = new Pose();
input_spline_inf = new Spline_Inf();
CurveDesign = new Curve_common();
private_nh.getParam("planner_type", planner_type_, string("ARAPlanner"));
private_nh.getParam("allocated_time", allocated_time_, 10.0);
private_nh.getParam("initial_epsilon", initial_epsilon_, 3.0);
private_nh.getParam("environment_type", environment_type_, string("XYThetaLattice"));
private_nh.getParam("forward_search", forward_search_, bool(false));
p_nh.getParam("primitive_filename", primitive_filename_, string(""));
private_nh.getParam("force_scratch_limit", force_scratch_limit_, 500);
double nominalvel_mpersecs, timetoturn45degsinplace_secs;
private_nh.getParam("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
private_nh.getParam("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
int lethal_obstacle;
private_nh.getParam("lethal_obstacle", lethal_obstacle, 20);
lethal_obstacle_ = (unsigned char)lethal_obstacle;
inscribed_inflated_obstacle_ = lethal_obstacle_ - 1;
private_nh.getParam("publish_footprint_path", publish_footprint_path_, bool(true));
private_nh.getParam("visualizer_skip_poses", visualizer_skip_poses_, 5);
private_nh.getParam("allow_unknown", allow_unknown_, bool(true));
input_spline_inf_ = new Spline_Inf();
CurveDesign_ = new Curve_common();
name_ = name;
costmap_robot_ = costmap_robot;
@ -93,8 +70,8 @@ namespace custom_planner
}
// ===== STEP 2: VALIDATE PATHWAY =====
if (posesOnPathWay.empty()) {
printf("[custom_planner] posesOnPathWay is empty");
if (posesOnPathWay_.empty()) {
printf("[custom_planner] posesOnPathWay_ is empty");
return false;
}
@ -108,9 +85,9 @@ namespace custom_planner
// ===== STEP 4: FIND NEAREST POINT ON PATHWAY =====
int nearest_idx = 0;
double min_dist = std::numeric_limits<double>::max();
for (int i = 0; i < static_cast<int>(posesOnPathWay.size()); ++i) {
double distance = std::hypot(posesOnPathWay[i].getX() - start.pose.position.x,
posesOnPathWay[i].getY() - start.pose.position.y);
for (int i = 0; i < static_cast<int>(posesOnPathWay_.size()); ++i) {
double distance = std::hypot(posesOnPathWay_[i].getX() - start.pose.position.x,
posesOnPathWay_[i].getY() - start.pose.position.y);
if (distance < min_dist) {
min_dist = distance;
nearest_idx = i;
@ -124,13 +101,13 @@ namespace custom_planner
robot::Time plan_time = robot::Time::now();
// Calculate distance from start to nearest pathway point
double dx_start = posesOnPathWay[nearest_idx].getX() - start.pose.position.x;
double dy_start = posesOnPathWay[nearest_idx].getY() - start.pose.position.y;
double dx_start = posesOnPathWay_[nearest_idx].getX() - start.pose.position.x;
double dy_start = posesOnPathWay_[nearest_idx].getY() - start.pose.position.y;
double d_start_to_path = std::sqrt(dx_start * dx_start + dy_start * dy_start);
// Calculate distance from pathway end to goal
double dx_goal = goal.pose.position.x - posesOnPathWay.back().getX();
double dy_goal = goal.pose.position.y - posesOnPathWay.back().getY();
double dx_goal = goal.pose.position.x - posesOnPathWay_.back().getX();
double dy_goal = goal.pose.position.y - posesOnPathWay_.back().getY();
double d_path_to_goal = std::sqrt(dx_goal * dx_goal + dy_goal * dy_goal);
// ===== STEP 6: GENERATE PATH BASED ON DISTANCE CONDITIONS =====
@ -139,14 +116,14 @@ namespace custom_planner
// CASE 1: Both start and goal are close to pathway - Direct path
printf("[custom_planner] Using direct pathway");
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) {
for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
@ -156,7 +133,7 @@ namespace custom_planner
std::vector<robot_geometry_msgs::PoseStamped> start_control_points;
bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints(
start_control_points, start, posesOnPathWay, START);
start_control_points, start, posesOnPathWay_, START);
if (valid_start_nurbs) {
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path =
@ -172,26 +149,26 @@ namespace custom_planner
}
// Add remaining pathway
for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay.size(); i++) {
for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
} else {
// Fallback to direct path
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) {
for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
}
@ -202,7 +179,7 @@ namespace custom_planner
std::vector<robot_geometry_msgs::PoseStamped> goal_control_points;
bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints(
goal_control_points, goal, posesOnPathWay, GOAL);
goal_control_points, goal, posesOnPathWay_, GOAL);
if (valid_goal_nurbs) {
// Add pathway up to goal connection point
@ -210,10 +187,10 @@ namespace custom_planner
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
@ -230,14 +207,14 @@ namespace custom_planner
}
} else {
// Fallback: use entire pathway
for (size_t i = 0; i < posesOnPathWay.size(); i++) {
for (size_t i = 0; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
}
@ -248,9 +225,9 @@ namespace custom_planner
std::vector<robot_geometry_msgs::PoseStamped> start_control_points, goal_control_points;
bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints(
start_control_points, start, posesOnPathWay, START);
start_control_points, start, posesOnPathWay_, START);
bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints(
goal_control_points, goal, posesOnPathWay, GOAL);
goal_control_points, goal, posesOnPathWay_, GOAL);
bool valid_index_order = merge_path_calc_.start_target_idx_ < merge_path_calc_.goal_target_idx_;
@ -276,7 +253,7 @@ namespace custom_planner
// Add start NURBS path
std::vector<robot_geometry_msgs::PoseStamped> start_nurbs_path =
merge_path_calc_.calculateNURBSPath(start_control_points, false);
merge_path_calc_.calculateNURBSPath(start_control_points, reverse_);
for (const auto& pose : start_nurbs_path) {
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
@ -286,14 +263,14 @@ namespace custom_planner
}
// Add middle pathway segment
for (size_t i = (merge_path_calc_.start_target_idx_ - 1); i < merge_path_calc_.goal_target_idx_; i++) {
for (size_t i = (merge_path_calc_.start_target_idx_ ); i < merge_path_calc_.goal_target_idx_; i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
@ -311,14 +288,14 @@ namespace custom_planner
} else {
// Fallback: Direct pathway from nearest point
printf("[custom_planner] NURBS control points not found, using fallback path");
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) {
for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
}
@ -379,8 +356,8 @@ namespace custom_planner
int count_two_curve_idx = 0;
//Xóa mảng nếu còn chứa phần tử không xác định
orderNodes.clear();
posesOnPathWay.clear();
std::map<string, OrderNode> orderNodes;
posesOnPathWay_.clear();
edges_info_.clear();
RobotDirectionChangeAngle_info_.clear();
@ -395,8 +372,8 @@ namespace custom_planner
if(distance_start_to_goal > 0.2) {
return false;
} else {
posesOnPathWay.push_back(Pose(start.pose.position.x, start.pose.position.y, data_convert::getYaw(start.pose.orientation)));
posesOnPathWay.push_back(Pose(goal.pose.position.x, goal.pose.position.y, data_convert::getYaw(goal.pose.orientation)));
posesOnPathWay_.push_back(Pose(start.pose.position.x, start.pose.position.y, data_convert::getYaw(start.pose.orientation)));
posesOnPathWay_.push_back(Pose(goal.pose.position.x, goal.pose.position.y, data_convert::getYaw(goal.pose.orientation)));
return true;
}
}
@ -454,14 +431,14 @@ namespace custom_planner
{
double t_intervel = 0.01;
order = degree + 1;
input_spline_inf->control_point.clear();
input_spline_inf->knot_vector.clear();
input_spline_inf->weight.clear();
CurveDesign->ReadSplineInf(input_spline_inf, order, control_points, knot_vector);
CurveDesign->ReadSplineInf(input_spline_inf, weight_vector, false);
input_spline_inf_->control_point.clear();
input_spline_inf_->knot_vector.clear();
input_spline_inf_->weight.clear();
CurveDesign_->ReadSplineInf(input_spline_inf_, order, control_points, knot_vector);
CurveDesign_->ReadSplineInf(input_spline_inf_, weight_vector, false);
//Tính độ dài của cạnh thứ i
double edge_length = merge_path_calc_.calculateNURBSLength(CurveDesign, input_spline_inf, t_intervel);
double edge_length = merge_path_calc_.calculateNURBSLength(CurveDesign_, input_spline_inf_, t_intervel);
//Tính step của cạnh thứ i
if (!costmap_robot_ || !costmap_robot_->getCostmap()) {
return false;
@ -474,7 +451,7 @@ namespace custom_planner
for(double u_test = 0; u_test <= 1; u_test += t_intervel_new)
{
robot_geometry_msgs::Point curve_point;
curve_point = CurveDesign->CalculateCurvePoint(input_spline_inf, u_test, true);
curve_point = CurveDesign_->CalculateCurvePoint(input_spline_inf_, u_test, true);
if(!std::isnan(curve_point.x)&&!std::isnan(curve_point.y))
posesOnEdge.push_back(Pose(curve_point.x, curve_point.y, 0));
}
@ -484,11 +461,11 @@ namespace custom_planner
continue;
}
double angle_1 = calculateAngle(posesOnEdge.front().getX(),posesOnEdge.front().getY(),
double angle_1 = merge_path_calc_.calculateAngle(posesOnEdge.front().getX(),posesOnEdge.front().getY(),
posesOnEdge[(int)(posesOnEdge.size()/2)].getX(),
posesOnEdge[(int)(posesOnEdge.size()/2)].getY());
double angle_2 = calculateAngle(posesOnEdge[(int)(posesOnEdge.size()/2)].getX(),
double angle_2 = merge_path_calc_.calculateAngle(posesOnEdge[(int)(posesOnEdge.size()/2)].getX(),
posesOnEdge[(int)(posesOnEdge.size()/2)].getY(),
posesOnEdge.back().getX(),posesOnEdge.back().getY());
@ -809,7 +786,7 @@ namespace custom_planner
return false;
}
if(!(int)posesOnPathWay.empty()) posesOnPathWay.clear();
if(!(int)posesOnPathWay_.empty()) posesOnPathWay_.clear();
merge_path_calc_.status_yaw_edge_end_ = checkYawEdgeEnd(savePosesOnEdge[(int)savePosesOnEdge.size()-2],
savePosesOnEdge[(int)savePosesOnEdge.size()-1], goal);
@ -902,7 +879,7 @@ namespace custom_planner
}
}
}
// posesOnPathWay.insert(posesOnPathWay.end(), Pose_goal_tmp.begin(),Pose_goal_tmp.end());
// posesOnPathWay_.insert(posesOnPathWay_.end(), Pose_goal_tmp.begin(),Pose_goal_tmp.end());
}
else
{
@ -969,7 +946,7 @@ namespace custom_planner
savePoseTMP.insert(savePoseTMP.begin() + insert_pos, MatrixPose[i].begin(), MatrixPose[i].end());
}
}
posesOnPathWay.insert(posesOnPathWay.end(),
posesOnPathWay_.insert(posesOnPathWay_.end(),
savePoseTMP.begin(),
savePoseTMP.end());
}
@ -1018,7 +995,7 @@ namespace custom_planner
}
MatrixPose[i].shrink_to_fit();
}
posesOnPathWay.insert(posesOnPathWay.end(),
posesOnPathWay_.insert(posesOnPathWay_.end(),
savePosesOnEdge.begin(),
savePosesOnEdge.end());
}
@ -1028,12 +1005,12 @@ namespace custom_planner
int nearest_idx = -1;
double min_dist = std::numeric_limits<double>::max();
int start_idx = edges_info_[0].end_edge_idx;
int end_idx = std::min(edges_info_[1].end_edge_idx, (int)posesOnPathWay.size());
int end_idx = std::min(edges_info_[1].end_edge_idx, (int)posesOnPathWay_.size());
for (int i = start_idx; i < end_idx; i++)
{
double dx = start.pose.position.x - posesOnPathWay[i].getX();
double dy = start.pose.position.y - posesOnPathWay[i].getY();
double dx = start.pose.position.x - posesOnPathWay_[i].getX();
double dy = start.pose.position.y - posesOnPathWay_[i].getY();
double dist = std::hypot(dx, dy);
if (dist < min_dist)
@ -1043,17 +1020,17 @@ namespace custom_planner
}
}
if (nearest_idx > 0 && nearest_idx < (int)posesOnPathWay.size())
if (nearest_idx > 0 && nearest_idx < (int)posesOnPathWay_.size())
{
posesOnPathWay.erase(posesOnPathWay.begin(), posesOnPathWay.begin() + nearest_idx);
posesOnPathWay_.erase(posesOnPathWay_.begin(), posesOnPathWay_.begin() + nearest_idx);
merge_path_calc_.edge_front_start_idx_ = 0;
merge_path_calc_.edge_front_end_idx_ = end_idx - nearest_idx;
int total = edges_info_.back().start_edge_idx - edges_info_.back().end_edge_idx;
merge_path_calc_.edge_back_end_idx_ = (int)posesOnPathWay.size() - 1;
merge_path_calc_.edge_back_end_idx_ = (int)posesOnPathWay_.size() - 1;
merge_path_calc_.edge_back_start_idx_ = merge_path_calc_.edge_back_end_idx_ - total;
// posesOnPathWay.shrink_to_fit();
// printf("[custom_planner][makePlanWithOrder] DEBUG: 2 posesOnPathWay size(%d)", (int)posesOnPathWay.size());
// posesOnPathWay_.shrink_to_fit();
// printf("[custom_planner][makePlanWithOrder] DEBUG: 2 posesOnPathWay_ size(%d)", (int)posesOnPathWay_.size());
}
}
return true;
@ -1124,7 +1101,7 @@ namespace custom_planner
if(posesOnEdge.size()>2){
for(int i = 0; i<((int)posesOnEdge.size()-1); i++)
{
double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
double theta = merge_path_calc_.calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
posesOnEdge[i+1].getX(), posesOnEdge[i+1].getY());
posesOnEdge[i].setYaw(theta);
}
@ -1134,7 +1111,7 @@ namespace custom_planner
{
if(posesOnEdge[0].getX()!=posesOnEdge[1].getX())
{
double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(),
double theta = merge_path_calc_.calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(),
posesOnEdge[1].getX(), posesOnEdge[1].getY());
posesOnEdge[0].setYaw(theta);
posesOnEdge[1].setYaw(theta);
@ -1148,7 +1125,7 @@ namespace custom_planner
if(posesOnEdge.size()>2){
for(int i = (int)posesOnEdge.size() -1; i>0; i--)
{
double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
double theta = merge_path_calc_.calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
posesOnEdge[i-1].getX(), posesOnEdge[i-1].getY());
posesOnEdge[i].setYaw(theta);
}
@ -1158,7 +1135,7 @@ namespace custom_planner
{
if(posesOnEdge[1].getX()!=posesOnEdge[0].getX())
{
double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(),
double theta = merge_path_calc_.calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(),
posesOnEdge[0].getX(), posesOnEdge[0].getY());
posesOnEdge[1].setYaw(theta);
posesOnEdge[0].setYaw(theta);
@ -1170,11 +1147,19 @@ namespace custom_planner
bool CustomPlanner::checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, const robot_geometry_msgs::PoseStamped& goal)
{
double angle = calculateAngle(start_edge_pose.getX(), start_edge_pose.getY(), end_edge_pose.getX(), end_edge_pose.getY());
double angle = merge_path_calc_.calculateAngle(start_edge_pose.getX(), start_edge_pose.getY(), end_edge_pose.getX(), end_edge_pose.getY());
double yaw_goal = data_convert::getYaw(goal.pose.orientation);
// printf("[custom_planner] DEBUG: angle: %f; yaw_goal: %f",angle, yaw_goal);
double delta_angle = fabs((merge_path_calc_.normalizeAngle(yaw_goal)) - (merge_path_calc_.normalizeAngle(angle)));
auto normalizeAngle0To2Pi = [](double angle) -> double
{
angle = fmod(angle, 2.0 * M_PI);
if (angle < 0)
angle += 2.0 * M_PI;
return angle;
};
double delta_angle = fabs((normalizeAngle0To2Pi(yaw_goal)) - (normalizeAngle0To2Pi(angle)));
return (delta_angle > (0.5*M_PI));
}

0
src/custom_planner_config.cpp Normal file
View File

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();