#include #include #include #include #include #include using namespace std; namespace custom_planner { CustomPlanner::CustomPlanner() : initialized_(false), costmap_robot_(NULL) { } CustomPlanner::CustomPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) : initialized_(false), costmap_robot_(NULL) { initialize(name, costmap_robot); } bool CustomPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) { if (!initialized_) { robot::NodeHandle private_nh("~/" + name); 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)); name_ = name; costmap_robot_ = costmap_robot; footprint_ = costmap_robot_->getRobotFootprint(); initialized_ = true; printf("[custom_planner] Initialized successfully"); } } bool CustomPlanner::makePlan(const robot_protocol_msgs::Order& msg, const robot_geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& goal, std::vector& plan) { plan.clear(); // Check for reinitialization needs bool do_init = false; if(!makePlanWithOrder(msg, start, goal)) return false; if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() || current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY()) { printf("Costmap dimensions have changed from (%d x %d) to (%d x %d), reinitializing custom_planner.", current_env_width_, current_env_height_, costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY()); do_init = true; } else if (footprint_ != costmap_robot_->getRobotFootprint()) { printf("Robot footprint has changed, reinitializing custom_planner."); do_init = true; } if (do_init) { initialized_ = false; initialize(name_, costmap_robot_); } // ===== STEP 2: VALIDATE PATHWAY ===== if (posesOnPathWay.empty()) { printf("[custom_planner] posesOnPathWay is empty"); return false; } // ===== STEP 3: VALIDATE START/GOAL POSITIONS ===== unsigned int mx_start, my_start, mx_end, my_end; if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start) || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_end, my_end)) { printf("[custom_planner] Cannot convert world coordinates to map coordinates"); } // ===== 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); if (distance < min_dist) { min_dist = distance; nearest_idx = i; } } merge_path_calc_.store_start_nearest_idx_ = nearest_idx; // ===== STEP 5: CALCULATE DISTANCES ===== const double DISTANCE_THRESHOLD = 0.1; 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 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 d_path_to_goal = std::sqrt(dx_goal * dx_goal + dy_goal * dy_goal); // ===== STEP 6: GENERATE PATH BASED ON DISTANCE CONDITIONS ===== if (d_start_to_path <= DISTANCE_THRESHOLD && d_path_to_goal <= DISTANCE_THRESHOLD) { // 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++) { 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.z = 0; pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); plan.push_back(pose); } } else if (d_start_to_path > DISTANCE_THRESHOLD && d_path_to_goal <= DISTANCE_THRESHOLD) { // CASE 2: Start is far, goal is close - NURBS from start to pathway printf("[custom_planner] Using start NURBS path reverse_(%x)", reverse_); std::vector start_control_points; bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints( start_control_points, start, posesOnPathWay, START); if (valid_start_nurbs) { std::vector nurbs_path = merge_path_calc_.calculateNURBSPath(start_control_points, reverse_); // Add NURBS path for (const auto& pose : nurbs_path) { robot_geometry_msgs::PoseStamped new_pose = pose; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; plan.push_back(new_pose); } // Add remaining pathway 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.z = 0; 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++) { 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.z = 0; pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); plan.push_back(pose); } } } else if (d_start_to_path <= DISTANCE_THRESHOLD && d_path_to_goal > DISTANCE_THRESHOLD) { // CASE 3: Start is close, goal is far - NURBS from pathway to goal printf("[custom_planner] Using goal NURBS path"); std::vector goal_control_points; bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints( goal_control_points, goal, posesOnPathWay, GOAL); if (valid_goal_nurbs) { // Add pathway up to goal connection point for (size_t i = nearest_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.z = 0; pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); plan.push_back(pose); } // Add NURBS path to goal std::vector nurbs_path = merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_); for (const auto& pose : nurbs_path) { robot_geometry_msgs::PoseStamped new_pose = pose; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; plan.push_back(new_pose); } } else { // Fallback: use entire pathway 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.z = 0; pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); plan.push_back(pose); } } } else { // CASE 4: Both start and goal are far - Dual NURBS or special handling printf("[custom_planner] Using dual NURBS path"); std::vector start_control_points, goal_control_points; bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints( start_control_points, start, posesOnPathWay, START); bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints( goal_control_points, goal, posesOnPathWay, GOAL); bool valid_index_order = merge_path_calc_.start_target_idx_ < merge_path_calc_.goal_target_idx_; if (!valid_index_order && valid_start_nurbs && valid_goal_nurbs) { // Special case: Goal-only NURBS when indices are invalid costmap_robot_->getRobotPose(goal_control_points[0]); goal_control_points[goal_control_points.size() - 1] = goal; std::vector nurbs_path = merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_); // Skip first point to avoid duplication for (size_t i = 1; i < nurbs_path.size(); i++) { robot_geometry_msgs::PoseStamped new_pose = nurbs_path[i]; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; plan.push_back(new_pose); } } else if (valid_start_nurbs && valid_goal_nurbs && valid_index_order) { // Dual NURBS: Start NURBS + Pathway + Goal NURBS // Add start NURBS path std::vector start_nurbs_path = merge_path_calc_.calculateNURBSPath(start_control_points, false); for (const auto& pose : start_nurbs_path) { robot_geometry_msgs::PoseStamped new_pose = pose; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; plan.push_back(new_pose); } // Add middle pathway segment for (size_t i = (merge_path_calc_.start_target_idx_ - 1); 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.z = 0; pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); plan.push_back(pose); } // Add goal NURBS path std::vector goal_nurbs_path = merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_); for (const auto& pose : goal_nurbs_path) { robot_geometry_msgs::PoseStamped new_pose = pose; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; plan.push_back(new_pose); } } 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++) { 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.z = 0; pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); plan.push_back(pose); } } } // ===== STEP 7: ADD FINAL GOAL POSE ===== robot_geometry_msgs::PoseStamped pose_goal; pose_goal.header.stamp = plan_time; pose_goal.header.frame_id = costmap_robot_->getGlobalFrameID(); pose_goal.pose = goal.pose; plan.push_back(pose_goal); // ===== STEP 8: FINALIZE AND PUBLISH PLAN ===== // Set sequence numbers for (size_t i = 0; i < plan.size(); ++i) { plan[i].header.seq = i; } // Publish plan for visualization nav_msgs::Path gui_path; gui_path.header.frame_id = costmap_robot_->getGlobalFrameID(); gui_path.header.stamp = plan_time; gui_path.poses = plan; // plan_pub_.publish(gui_path); printf("[custom_planner] Generated plan with %zu waypoints", plan.size()); return true; } void CustomPlanner::transformFootprintToEdges(const robot_geometry_msgs::Pose &robot_pose, const std::vector &footprint, std::vector &out_footprint) { out_footprint.resize(2 * footprint.size()); double yaw = data_convert::getYaw(robot_pose.orientation); for (unsigned int i = 0; i < footprint.size(); i++) { out_footprint[2 * i].x = robot_pose.position.x + cos(yaw) * footprint[i].x - sin(yaw) * footprint[i].y; out_footprint[2 * i].y = robot_pose.position.y + sin(yaw) * footprint[i].x + cos(yaw) * footprint[i].y; if (i == 0) { out_footprint.back().x = out_footprint[i].x; out_footprint.back().y = out_footprint[i].y; } else { out_footprint[2 * i - 1].x = out_footprint[2 * i].x; out_footprint[2 * i - 1].y = out_footprint[2 * i].y; } } } bool CustomPlanner::makePlanWithOrder(robot_protocol_msgs::Order msg, const robot_geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& goal) { vector savePosesOnEdge; 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(); edges_info_.clear(); RobotDirectionChangeAngle_info_.clear(); std::string all_edges, all_nodes; int total_edge = (int)msg.edges.size(); int total_node = (int)msg.nodes.size(); if(total_edge <= 0 && total_node <= 0) { double delta_x = start.pose.position.x - goal.pose.position.x; double delta_y = start.pose.position.y - goal.pose.position.y; double distance_start_to_goal = std::hypot(delta_x, delta_y); 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))); return true; } } for(int i = 0; i < total_node; i++){ all_nodes += "[" + std::to_string(i) + "] " + msg.nodes[i].nodeDescription.c_str(); if (i != total_node - 1) { all_nodes += " | "; // phân cách giữa các edge } orderNodes[msg.nodes[i].nodeId].nodeId = msg.nodes[i].nodeId; orderNodes[msg.nodes[i].nodeId].sequenceId = msg.nodes[i].sequenceId; orderNodes[msg.nodes[i].nodeId].position_x = (double)msg.nodes[i].nodePosition.x; orderNodes[msg.nodes[i].nodeId].position_y = (double)msg.nodes[i].nodePosition.y; orderNodes[msg.nodes[i].nodeId].theta = (double)msg.nodes[i].nodePosition.theta; } for(int i = 0; i < total_edge; i++) { all_edges += "[" + std::to_string(i) + "] " + msg.edges[i].edgeDescription; if (i != total_edge - 1) { all_edges += " | "; // phân cách giữa các edge } auto start_nodeId_it = orderNodes.find(msg.edges[i].startNodeId); auto end_nodeId_it = orderNodes.find(msg.edges[i].endNodeId); if(start_nodeId_it!=orderNodes.end()&&end_nodeId_it!=orderNodes.end()) { int degree = 0; int order = 0; vector posesOnEdge; vector> control_points; std::vector knot_vector; std::vector weight_vector; control_points.reserve(msg.edges[i].trajectory.controlPoints.size()); knot_vector.reserve(msg.edges[i].trajectory.knotVector.size()); weight_vector.reserve(msg.edges[i].trajectory.controlPoints.size()); for(int j = 0;j<(int)msg.edges[i].trajectory.controlPoints.size();j++) { control_points.push_back(Eigen::Vector3d(msg.edges[i].trajectory.controlPoints[j].x, msg.edges[i].trajectory.controlPoints[j].y, 0)); weight_vector.push_back(msg.edges[i].trajectory.controlPoints[j].weight); } for(int k = 0 ;k < (int)msg.edges[i].trajectory.knotVector.size();k++) { knot_vector.push_back(msg.edges[i].trajectory.knotVector[k]); } degree = (int)msg.edges[i].trajectory.degree; if(curveIsValid(degree, knot_vector, control_points)) { 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); //Tính độ dài của cạnh thứ i 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; } double resolution = costmap_robot_->getCostmap()->getResolution(); // double resolution = 0.05; // Default resolution, can be adjusted double t_intervel_new = merge_path_calc_.calculateAdaptiveStep(edge_length, resolution); //Tính các điểm theo step mới 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); if(!std::isnan(curve_point.x)&&!std::isnan(curve_point.y)) posesOnEdge.push_back(Pose(curve_point.x, curve_point.y, 0)); } if ((int)posesOnEdge.size() < 3) { printf("Edge has too few poses for angle calculation"); continue; } double angle_1 = 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(), posesOnEdge[(int)(posesOnEdge.size()/2)].getY(), posesOnEdge.back().getX(),posesOnEdge.back().getY()); double delta_angle = fabs(fabs(angle_1) - fabs(angle_2)); bool found_intersection = false; if((int)edges_info_.size() > 0) if(edges_info_.back().isCurve || delta_angle > EPSILON) for (int idx_segment_A = 0; idx_segment_A + 1 < (int)savePosesOnEdge.size(); ++idx_segment_A) { robot_geometry_msgs::Point gp1 ; gp1.x = savePosesOnEdge[idx_segment_A].getX(); gp1.y = savePosesOnEdge[idx_segment_A].getY(); robot_geometry_msgs::Point gp2; gp2.x = savePosesOnEdge[idx_segment_A + 1].getX(); gp2.y = savePosesOnEdge[idx_segment_A + 1].getY(); for (int idx_segment_B = (int)posesOnEdge.size() - 1; idx_segment_B > 0; --idx_segment_B) { robot_geometry_msgs::Point lp1; lp1.x = posesOnEdge[idx_segment_B].getX(); lp1.y = posesOnEdge[idx_segment_B].getY(); robot_geometry_msgs::Point lp2; lp2.x = posesOnEdge[idx_segment_B - 1].getX(); lp2.y = posesOnEdge[idx_segment_B - 1].getY(); if (doIntersect(gp1, gp2, lp1, lp2)) { posesOnEdge.erase(posesOnEdge.begin(), posesOnEdge.begin() + idx_segment_B); savePosesOnEdge.erase(savePosesOnEdge.begin() + idx_segment_A + 1, savePosesOnEdge.end()); auto intersection_point = computeIntersectionPoint(gp1, gp2, lp1, lp2); if (intersection_point) { robot_geometry_msgs::Point p = intersection_point.value(); // savePosesOnEdge.push_back(Pose(p.x, p.y, 0)); posesOnEdge.insert(posesOnEdge.begin(), Pose(p.x, p.y, 0)); } int save_idx = -1; for(int idx_edges_info = 0; idx_edges_info < (int)edges_info_.size(); idx_edges_info++) { if(idx_segment_A >= edges_info_[idx_edges_info].start_edge_idx && idx_segment_A <= edges_info_[idx_edges_info].end_edge_idx) { save_idx = idx_edges_info; break; } } if (save_idx != -1) { edges_info_.erase(edges_info_.begin() + save_idx + 1, edges_info_.end()); if (!edges_info_.empty()) edges_info_.back().end_edge_idx = idx_segment_A; } found_intersection = true; break; } } if(found_intersection == true) break; } savePosesOnEdge.insert(savePosesOnEdge.end(), posesOnEdge.begin(), posesOnEdge.end()); if (!posesOnEdge.empty()) { InfEdge edge_info; // tạo struct mới if (i == 0) { edge_info.start_edge_idx = 0; edge_info.end_edge_idx = edge_info.start_edge_idx + (int)posesOnEdge.size() - 1; merge_path_calc_.edge_front_start_idx_ = edge_info.start_edge_idx; merge_path_calc_.edge_front_end_idx_ = edge_info.end_edge_idx; } else { edge_info.start_edge_idx = edges_info_.back().end_edge_idx + 1; edge_info.end_edge_idx = edge_info.start_edge_idx + (int)posesOnEdge.size() - 1; merge_path_calc_.edge_back_start_idx_ = edge_info.start_edge_idx; merge_path_calc_.edge_back_end_idx_ = edge_info.end_edge_idx; } // Gán isCurve edge_info.isCurve = (delta_angle >= EPSILON); // Thêm vào vector edges_info_.push_back(edge_info); } } else{ printf("Trajectory of Edge: %s, startNodeId: %s, endNodeId: %s is invalid", msg.edges[i].edgeId.c_str(), msg.edges[i].startNodeId.c_str(), msg.edges[i].endNodeId.c_str()); return false; } } else{ printf("Edge: %s not found startNodeId: %s or endNodeId: %s", msg.edges[i].edgeId.c_str(), msg.edges[i].startNodeId.c_str(), msg.edges[i].endNodeId.c_str()); return false; } } printf("[custom_planner][makePlanWithOrder] All nodes: %s", all_nodes.c_str()); printf("[custom_planner][makePlanWithOrder] All edges: %s", all_edges.c_str()); // printf("[custom_planner][makePlanWithOrder] savePosesOnEdge: %d", (int)savePosesOnEdge.size()); total_edge = (int)edges_info_.size(); bool status_countRobotDirectionChangeAngle = countRobotDirectionChangeAngle(savePosesOnEdge, total_edge); bool status_calcAllYaw = false; // printf("[custom_planner][makePlanWithOrder] DEBUG 1000 total_edge: %d", total_edge); if(status_countRobotDirectionChangeAngle) status_calcAllYaw = calcAllYaw(start, goal, savePosesOnEdge, total_edge); // printf("[custom_planner][makePlanWithOrder] DEBUG 2000"); return status_calcAllYaw; } bool CustomPlanner::doIntersect(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1, const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2) { auto orientation = [](const robot_geometry_msgs::Point& a, const robot_geometry_msgs::Point& b, const robot_geometry_msgs::Point& c) { double val = (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y); if (std::abs(val) < 1e-6) return 0; // colinear return (val > 0) ? 1 : 2; // clockwise or counterclockwise }; int o1 = orientation(p1, q1, p2); int o2 = orientation(p1, q1, q2); int o3 = orientation(p2, q2, p1); int o4 = orientation(p2, q2, q1); return (o1 != o2 && o3 != o4); } std::optional CustomPlanner::computeIntersectionPoint(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1, const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2) { double x1 = p1.x, y1 = p1.y; double x2 = q1.x, y2 = q1.y; double x3 = p2.x, y3 = p2.y; double x4 = q2.x, y4 = q2.y; double denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); if (std::abs(denom) < 1e-8) return std::nullopt; // Đường thẳng song song hoặc trùng nhau double pre = (x1 * y2 - y1 * x2); double post = (x3 * y4 - y3 * x4); double x = (pre * (x3 - x4) - (x1 - x2) * post) / denom; double y = (pre * (y3 - y4) - (y1 - y2) * post) / denom; // Kiểm tra xem điểm giao nằm trên cả hai đoạn không auto between = [](double a, double b, double c) { return std::min(a, b) - 1e-6 <= c && c <= std::max(a, b) + 1e-6; }; if (between(x1, x2, x) && between(y1, y2, y) && between(x3, x4, x) && between(y3, y4, y)) { robot_geometry_msgs::Point pt; pt.x = x; pt.y = y; pt.z = 0.0; return pt; } return std::nullopt; // Giao điểm nằm ngoài đoạn } bool CustomPlanner::countRobotDirectionChangeAngle(vector& savePosesOnEdge, int& total_edge) { if (savePosesOnEdge.empty() || total_edge == 0) return false; skipEdge_flag_ = false; // Kiểm tra 2 cạnh đầu không phải curve if (edges_info_.size() > 1 && !edges_info_[0].isCurve && !edges_info_[1].isCurve) { int idx0 = edges_info_[0].start_edge_idx; int idx1 = edges_info_[1].start_edge_idx; int idx2 = edges_info_[1].end_edge_idx; if (idx0 >= 0 && idx1 >= 0 && idx2 < (int)savePosesOnEdge.size()) { double angle = merge_path_calc_.signedAngle(savePosesOnEdge[idx0], savePosesOnEdge[idx1], savePosesOnEdge[idx2]); skipEdge_flag_ = (fabs(angle) < 0.15 * M_PI); } } for (int i = 0; i < total_edge; i++) { if (!edges_info_[i].isCurve) continue; // Tính các chỉ số cần thiết int prev_idx = i - 1; int next_idx = i + 1; // Tính connecting_angle_start nếu có cạnh trước double connecting_angle_start = M_PI; if (prev_idx >= 0) { int prev_end = edges_info_[prev_idx].end_edge_idx; int curr_start = edges_info_[i].start_edge_idx; connecting_angle_start = merge_path_calc_.signedAngle(savePosesOnEdge[prev_end - 2], savePosesOnEdge[curr_start], savePosesOnEdge[curr_start + 2]); } // Tính connecting_angle_end nếu có cạnh sau double connecting_angle_end = M_PI; if (next_idx < total_edge) { int curr_end = edges_info_[i].end_edge_idx; int next_start = edges_info_[next_idx].start_edge_idx; connecting_angle_end = merge_path_calc_.signedAngle(savePosesOnEdge[curr_end - 2], savePosesOnEdge[next_start], savePosesOnEdge[next_start + 2]); } // Hàm phụ để tính cost auto calc_cost = [&](double angle) -> int { if (fabs(angle) <= (0.1 * M_PI)) return 20; if (fabs(angle) < (0.45 * M_PI)) return 10; return 0; }; int cost_start = calc_cost(connecting_angle_start); int cost_end = calc_cost(connecting_angle_end); // Gom phần kiểm tra cost_start và cost_end thành 1 block auto process_cost = [&](int cost, int idx_first, int idx_second, int idx_first_end, int idx_second_end) { if (cost > 0) { // Tính toán các góc kiểm tra, lưu biến tạm nếu dùng nhiều lần Pose pose_centre = pointOnAngleBisector(savePosesOnEdge[idx_first_end - 2], savePosesOnEdge[idx_second], savePosesOnEdge[idx_second + 2], 0.1); double angle_check_1 = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_first_end - 1]); double angle_check_2 = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_first_end - 3]); double angle_check_3 = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_second + 1]); double angle_check_4 = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_second + 3]); double delta_angle_1 = fabs(angle_check_2) - fabs(angle_check_1); double delta_angle_2 = fabs(angle_check_4) - fabs(angle_check_3); if (delta_angle_1 > EPSILON || delta_angle_2 > EPSILON) cost += 20; else { // Kiểm tra dấu double angle_check_2b = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_first]); double angle_check_4b = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_second_end]); if (isOppositeSign(angle_check_1, angle_check_2b) || isOppositeSign(angle_check_3, angle_check_4b)) cost += 10; } } return cost; }; // Xử lý cost_start if (prev_idx >= 0) { cost_start = process_cost(cost_start, edges_info_[prev_idx].start_edge_idx, edges_info_[i].start_edge_idx, edges_info_[prev_idx].end_edge_idx, edges_info_[i].end_edge_idx); } // Xử lý cost_end if (next_idx < total_edge) { cost_end = process_cost(cost_end, edges_info_[i].start_edge_idx, edges_info_[next_idx].start_edge_idx, edges_info_[i].end_edge_idx, edges_info_[next_idx].end_edge_idx); } // Nếu cost đủ lớn thì lưu lại cặp cạnh cong if (cost_start >= 30 && prev_idx >= 0) { RobotDirectionChangeAngle_info_.emplace_back( RobotDirectionChangeAngle{edges_info_[prev_idx].start_edge_idx, edges_info_[prev_idx].end_edge_idx, edges_info_[i].start_edge_idx, edges_info_[i].end_edge_idx}); // count_two_curve_idx++; } if (cost_end >= 30 && next_idx < total_edge) { RobotDirectionChangeAngle_info_.emplace_back( RobotDirectionChangeAngle{edges_info_[i].start_edge_idx, edges_info_[i].end_edge_idx, edges_info_[next_idx].start_edge_idx, edges_info_[next_idx].end_edge_idx}); // count_two_curve_idx++; } // Nếu đã lưu 1 cặp thì bỏ qua cạnh tiếp theo if (cost_start >= 30 || cost_end >= 30) i++; // else return false; } // printf("[custom_planner][countRobotDirectionChangeAngle] DEBUG: 400 count_two_curve_idx(%d)", count_two_curve_idx); return true; } bool CustomPlanner::calcAllYaw(const robot_geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& goal, vector& savePosesOnEdge, int& total_edge) { int total_two_curve = (int)RobotDirectionChangeAngle_info_.size(); // printf("[custom_planner][calcAllYaw] total_two_curve: %d", total_two_curve); vector savePoseTMP; if(!savePoseTMP.empty()) savePoseTMP.clear(); if(savePosesOnEdge.empty()) { printf("[custom_planner][calcAllYaw] savePosesOnEdge is empty!\n"); return false; } 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); if(!rotating_robot_plag_) { if(total_two_curve > 0) { if(total_two_curve == 1) { vector Pose_start_tmp; vector Pose_goal_tmp; vector::iterator start_it_start = savePosesOnEdge.begin(); vector::iterator start_it_end = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_.front().end_edge_idx_first + 1; vector::iterator goal_it_start = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_.back().start_edge_idx_second; vector::iterator goal_it_end = savePosesOnEdge.end(); Pose_start_tmp.insert(Pose_start_tmp.end(), start_it_start, start_it_end); Pose_goal_tmp.insert(Pose_goal_tmp.end(), goal_it_start, goal_it_end); setYawAllPosesOnEdge(Pose_start_tmp,false); setYawAllPosesOnEdge(Pose_goal_tmp,true); savePoseTMP.insert(savePoseTMP.begin(), Pose_start_tmp.begin(),Pose_start_tmp.end()); savePoseTMP.insert(savePoseTMP.end(), Pose_goal_tmp.begin(),Pose_goal_tmp.end()); reverse_ = false; } else { // Khởi tạo phần đầu path vector Pose_start_tmp; if (!savePosesOnEdge.empty() && !RobotDirectionChangeAngle_info_.empty()) { vector::iterator start_it_start = savePosesOnEdge.begin(); vector::iterator start_it_end = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_[0].end_edge_idx_first + 1; if (start_it_end <= savePosesOnEdge.end()) { Pose_start_tmp.insert(Pose_start_tmp.end(), start_it_start, start_it_end); setYawAllPosesOnEdge(Pose_start_tmp, false); savePoseTMP.insert(savePoseTMP.end(), Pose_start_tmp.begin(), Pose_start_tmp.end()); } } // Xử lý các đoạn giữa path int flip = -1; for(int i = 0; i < total_two_curve - 1; i++) { vector Pose_mid_tmp; vector::iterator it_start = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_[i].start_edge_idx_second; vector::iterator it_end = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_[i+1].end_edge_idx_first + 1; if (it_start < savePosesOnEdge.end() && it_end <= savePosesOnEdge.end()) { Pose_mid_tmp.insert(Pose_mid_tmp.end(), it_start, it_end); flip *= -1; setYawAllPosesOnEdge(Pose_mid_tmp, flip > 0); if (!Pose_mid_tmp.empty()) { savePoseTMP.insert(savePoseTMP.end(), Pose_mid_tmp.begin(), Pose_mid_tmp.end()); } } } vector Pose_goal_tmp; if (!savePosesOnEdge.empty() && !RobotDirectionChangeAngle_info_.empty()) { vector::iterator goal_it_start = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_.back().start_edge_idx_second; vector::iterator goal_it_end = savePosesOnEdge.end(); if (goal_it_start < savePosesOnEdge.end()) { Pose_goal_tmp.insert(Pose_goal_tmp.end(), goal_it_start, goal_it_end); flip *= -1; setYawAllPosesOnEdge(Pose_goal_tmp, flip > 0); if(flip > 0) reverse_ = true; else reverse_ = false; if (!Pose_goal_tmp.empty()) { savePoseTMP.insert(savePoseTMP.end(), Pose_goal_tmp.begin(), Pose_goal_tmp.end()); } } } } // posesOnPathWay.insert(posesOnPathWay.end(), Pose_goal_tmp.begin(),Pose_goal_tmp.end()); } else { if(merge_path_calc_.status_yaw_edge_end_) { reverse_ = true; // printf("custom_planner][calcAllYaw] DEBUG 300"); setYawAllPosesOnEdge(savePosesOnEdge,true); } else { reverse_ = false; // printf("custom_planner][calcAllYaw] DEBUG 310"); setYawAllPosesOnEdge(savePosesOnEdge,false); } savePoseTMP.insert(savePoseTMP.end(), savePosesOnEdge.begin(),savePosesOnEdge.end()); } vector> MatrixPose; MatrixPose.resize(total_edge); // Duyệt ngược để insert không làm lệch chỉ số các cạnh phía trước for(int i = total_edge - 1; i >= 1; i--) { int idx_1 = edges_info_[i-1].end_edge_idx - 2; int idx_2 = edges_info_[i].start_edge_idx; int idx_3 = edges_info_[i].start_edge_idx + 2; // Kiểm tra indices hợp lệ if (idx_1 < 0 || idx_1 >= static_cast(savePoseTMP.size()) || idx_2 < 0 || idx_2 >= static_cast(savePoseTMP.size()) || idx_3 < 0 || idx_3 >= static_cast(savePoseTMP.size())) { printf("Invalid indices: %d, %d, %d", idx_1, idx_2, idx_3); continue; } double delta_Angle = merge_path_calc_.signedAngle(savePoseTMP[idx_1], savePoseTMP[idx_2], savePoseTMP[idx_3]); // Cần tính lại các góc quay if(fabs(delta_Angle) > (0.45 * M_PI) && fabs(delta_Angle) < (0.85 * M_PI)) { MatrixPose[i].clear(); int max_step = (int)((M_PI - fabs(delta_Angle)) / (0.05 * M_PI)); for(int j = 0; j <= max_step; j++) { Pose intermediate_pose; intermediate_pose.setX(savePoseTMP[edges_info_[i].start_edge_idx].getX()); intermediate_pose.setY(savePoseTMP[edges_info_[i].start_edge_idx].getY()); if(delta_Angle < 0) { intermediate_pose.setYaw(savePoseTMP[edges_info_[i-1].end_edge_idx].getYaw() + j * (0.05 * M_PI)); } else { intermediate_pose.setYaw(savePoseTMP[edges_info_[i-1].end_edge_idx].getYaw() - j * (0.05 * M_PI)); } // intermediate_pose.getX(), intermediate_pose.getY(), intermediate_pose.getYaw()); MatrixPose[i].push_back(intermediate_pose); } MatrixPose[i].shrink_to_fit(); int insert_pos = edges_info_[i].start_edge_idx; savePoseTMP.insert(savePoseTMP.begin() + insert_pos, MatrixPose[i].begin(), MatrixPose[i].end()); } } posesOnPathWay.insert(posesOnPathWay.end(), savePoseTMP.begin(), savePoseTMP.end()); } else // rotating_robot_plag_ is true { if(merge_path_calc_.status_yaw_edge_end_) { reverse_ = false; setYawAllPosesOnEdge(savePosesOnEdge, true); } else{ reverse_ = true; setYawAllPosesOnEdge(savePosesOnEdge, false); } vector> MatrixPose; MatrixPose.resize(total_edge); // Duyệt ngược để insert không làm lệch chỉ số các cạnh phía trước for(int i = total_edge - 1; i >= 1; i--) { int idx_1 = edges_info_[i-1].end_edge_idx - 2; int idx_2 = edges_info_[i].start_edge_idx; int idx_3 = edges_info_[i].start_edge_idx + 2; double delta_Angle = merge_path_calc_.signedAngle(savePosesOnEdge[idx_1], savePosesOnEdge[idx_2], savePosesOnEdge[idx_3]); delta_Angle = delta_Angle - M_PI; if(fabs(delta_Angle) > (0.05 * M_PI)) { MatrixPose[i].clear(); int max_step = (int)((M_PI - fabs(delta_Angle)) / (0.05 * M_PI)); for(int j = 0; j <= max_step; j++) { Pose intermediate_pose; intermediate_pose.setX(savePosesOnEdge[edges_info_[i].start_edge_idx].getX()); intermediate_pose.setY(savePosesOnEdge[edges_info_[i].start_edge_idx].getY()); if(delta_Angle > 0) { intermediate_pose.setYaw(savePosesOnEdge[edges_info_[i-1].end_edge_idx].getYaw() + j * (0.05 * M_PI)); } else { intermediate_pose.setYaw(savePosesOnEdge[edges_info_[i-1].end_edge_idx].getYaw() - j * (0.05 * M_PI)); } MatrixPose[i].push_back(intermediate_pose); } int insert_pos = edges_info_[i].start_edge_idx; savePosesOnEdge.insert(savePosesOnEdge.begin() + insert_pos, MatrixPose[i].begin(), MatrixPose[i].end()); } MatrixPose[i].shrink_to_fit(); } posesOnPathWay.insert(posesOnPathWay.end(), savePosesOnEdge.begin(), savePosesOnEdge.end()); } if (skipEdge_flag_) { 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()); 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 dist = std::hypot(dx, dy); if (dist < min_dist) { min_dist = dist; nearest_idx = i; } } if (nearest_idx > 0 && nearest_idx < (int)posesOnPathWay.size()) { 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_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()); } } return true; } Pose CustomPlanner::pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length) { // Vector BA double ux = A.getX() - B.getX(); double uy = A.getY() - B.getY(); double ul = std::hypot(ux, uy); ux /= ul; uy /= ul; // Vector BC double vx = C.getX() - B.getX(); double vy = C.getY() - B.getY(); double vl = std::hypot(vx, vy); vx /= vl; vy /= vl; // Vector bisector double wx = ux + vx; double wy = uy + vy; double wl = std::hypot(wx, wy); // Chuẩn hóa và nhân độ dài Pose P; P.setX(B.getX() + length * wx / wl); P.setY(B.getY() + length * wy / wl); return P; } bool CustomPlanner::isThetaValid(double theta) { bool result = false; if(theta < -M_PI || theta > M_PI) result = false; else result = true; return result; } bool CustomPlanner::curveIsValid(int degree, const std::vector &knot_vector, vector>& control_points) { if(degree < 1 || degree > 9) { printf("degree is invalid value\n"); return false; } if(!((knot_vector.size() - degree - 1) == control_points.size())) { printf("relation between degree, number of knots, and number of control points is invalid\n"); return false; } // if(std::is_sorted(knot_vector.begin(), knot_vector.end())) // { // printf("knot vector is not monotonic"); // return false; // } return true; } void CustomPlanner::setYawAllPosesOnEdge(vector& posesOnEdge, bool reverse) { if(!reverse) { if(!posesOnEdge.empty()){ if(posesOnEdge.size()>2){ for(int i = 0; i<((int)posesOnEdge.size()-1); i++) { double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(), posesOnEdge[i+1].getX(), posesOnEdge[i+1].getY()); posesOnEdge[i].setYaw(theta); } posesOnEdge.back().setYaw(posesOnEdge[posesOnEdge.size()-2].getYaw()); } else if(posesOnEdge.size()==2) { if(posesOnEdge[0].getX()!=posesOnEdge[1].getX()) { double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(), posesOnEdge[1].getX(), posesOnEdge[1].getY()); posesOnEdge[0].setYaw(theta); posesOnEdge[1].setYaw(theta); } } } } else { if(!posesOnEdge.empty()){ if(posesOnEdge.size()>2){ for(int i = (int)posesOnEdge.size() -1; i>0; i--) { double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(), posesOnEdge[i-1].getX(), posesOnEdge[i-1].getY()); posesOnEdge[i].setYaw(theta); } posesOnEdge.front().setYaw(posesOnEdge[1].getYaw()); } else if(posesOnEdge.size()==2) { if(posesOnEdge[1].getX()!=posesOnEdge[0].getX()) { double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(), posesOnEdge[0].getX(), posesOnEdge[0].getY()); posesOnEdge[1].setYaw(theta); posesOnEdge[0].setYaw(theta); } } } } } 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 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))); return (delta_angle > (0.5*M_PI)); } // Export factory function static boost::shared_ptr custom_planner_plugin() { return boost::make_shared(); } // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) BOOST_DLL_ALIAS(custom_planner_plugin, custom_planner) }