1188 lines
52 KiB
C++
Executable File
1188 lines
52 KiB
C++
Executable File
#include <custom_planner/custom_planner.h>
|
|
#include <nav_msgs/Path.h>
|
|
#include <robot_geometry_msgs/Point.h>
|
|
#include <costmap_2d/inflation_layer.h>
|
|
#include <tf3/LinearMath/Quaternion.h>
|
|
#include <boost/dll/alias.hpp>
|
|
|
|
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<robot_geometry_msgs::PoseStamped>& 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<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);
|
|
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<robot_geometry_msgs::PoseStamped> start_control_points;
|
|
bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints(
|
|
start_control_points, start, posesOnPathWay, START);
|
|
|
|
if (valid_start_nurbs) {
|
|
std::vector<robot_geometry_msgs::PoseStamped> 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<robot_geometry_msgs::PoseStamped> 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<robot_geometry_msgs::PoseStamped> 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<robot_geometry_msgs::PoseStamped> 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<robot_geometry_msgs::PoseStamped> 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<robot_geometry_msgs::PoseStamped> 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<robot_geometry_msgs::PoseStamped> 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<robot_geometry_msgs::Point> &footprint,
|
|
std::vector<robot_geometry_msgs::Point> &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<Pose> 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<Pose> posesOnEdge;
|
|
vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> control_points;
|
|
|
|
std::vector<double> knot_vector;
|
|
std::vector<double> 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<robot_geometry_msgs::Point> 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<Pose>& 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<Pose>& savePosesOnEdge,
|
|
int& total_edge)
|
|
{
|
|
int total_two_curve = (int)RobotDirectionChangeAngle_info_.size();
|
|
// printf("[custom_planner][calcAllYaw] total_two_curve: %d", total_two_curve);
|
|
vector<Pose> 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> Pose_start_tmp;
|
|
vector<Pose> Pose_goal_tmp;
|
|
vector<Pose>::iterator start_it_start = savePosesOnEdge.begin();
|
|
vector<Pose>::iterator start_it_end = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_.front().end_edge_idx_first + 1;
|
|
vector<Pose>::iterator goal_it_start = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_.back().start_edge_idx_second;
|
|
vector<Pose>::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> Pose_start_tmp;
|
|
if (!savePosesOnEdge.empty() && !RobotDirectionChangeAngle_info_.empty())
|
|
{
|
|
vector<Pose>::iterator start_it_start = savePosesOnEdge.begin();
|
|
vector<Pose>::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> Pose_mid_tmp;
|
|
vector<Pose>::iterator it_start = savePosesOnEdge.begin() +
|
|
RobotDirectionChangeAngle_info_[i].start_edge_idx_second;
|
|
vector<Pose>::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> Pose_goal_tmp;
|
|
if (!savePosesOnEdge.empty() && !RobotDirectionChangeAngle_info_.empty())
|
|
{
|
|
vector<Pose>::iterator goal_it_start = savePosesOnEdge.begin() +
|
|
RobotDirectionChangeAngle_info_.back().start_edge_idx_second;
|
|
vector<Pose>::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<vector<Pose>> 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<int>(savePoseTMP.size()) ||
|
|
idx_2 < 0 || idx_2 >= static_cast<int>(savePoseTMP.size()) ||
|
|
idx_3 < 0 || idx_3 >= static_cast<int>(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<vector<Pose>> 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<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());
|
|
|
|
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<double> &knot_vector,
|
|
vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& 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<Pose>& 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<nav_core::BaseGlobalPlanner> custom_planner_plugin() {
|
|
return boost::make_shared<custom_planner::CustomPlanner>();
|
|
}
|
|
|
|
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
|
|
BOOST_DLL_ALIAS(custom_planner_plugin, custom_planner)
|
|
|
|
}
|