diff --git a/include/custom_planner/custom_planner.h b/include/custom_planner/custom_planner.h index 6f619eb..408a1e1 100755 --- a/include/custom_planner/custom_planner.h +++ b/include/custom_planner/custom_planner.h @@ -30,17 +30,7 @@ using namespace std; #include "custom_planner/merge_path_calc.h" #include - -#include -#include - #include -// #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& 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 posesOnPathWay; - Pose start_on_path; - std::map orderNodes; + Spline_Inf* input_spline_inf_; + Curve_common* CurveDesign_; + vector 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 service_servers_; - - // ros::Publisher sbpl_plan_footprint_pub_; boost::mutex mutex_; }; } diff --git a/include/custom_planner/custom_planner_config.h b/include/custom_planner/custom_planner_config.h new file mode 100644 index 0000000..10feb93 --- /dev/null +++ b/include/custom_planner/custom_planner_config.h @@ -0,0 +1,12 @@ +#ifndef CUSTOM_PLANNER_CONFIG_H_ +#define CUSTOM_PLANNER_CONFIG_H_ + +#include + +namespace custom_planner +{ + // struct + +} + +#endif // CUSTOM_PLANNER_CONFIG_H_ \ No newline at end of file diff --git a/src/custom_planner.cpp b/src/custom_planner.cpp index ca5cc0a..a706b00 100755 --- a/src/custom_planner.cpp +++ b/src/custom_planner.cpp @@ -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::max(); - for (int i = 0; i < static_cast(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(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 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 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 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 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 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 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::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)); } diff --git a/src/custom_planner_config.cpp b/src/custom_planner_config.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/merge_path_calc.cpp b/src/merge_path_calc.cpp index f4ae642..afa2aa8 100755 --- a/src/merge_path_calc.cpp +++ b/src/merge_path_calc.cpp @@ -185,8 +185,10 @@ namespace custom_planner { if(type == START) { + // int idx_check = edge_front_end_idx_; + int idx_check = static_cast(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(posesOnPathWay.size())) { + if (idx_check <= 0 || idx_check > static_cast(posesOnPathWay.size())) { return -1; } @@ -194,7 +196,7 @@ namespace custom_planner { double min_dist = std::numeric_limits::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(posesOnPathWay.size()) && i + 1 <= edge_front_end_idx_) + if (i + 1 < static_cast(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(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(posesOnPathWay.size())) { + // FIX: Validate idx_check trước khi sử dụng + if (idx_check <= 0 || idx_check > static_cast(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(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(posesOnPathWay.size()); // FIX: Validate lại edge_front_end_idx_ - if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast(posesOnPathWay.size())) { + if (idx_check <= 0 || idx_check > static_cast(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();