HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix direction

Browse Source
This commit is contained in:
HiepLM
2026-04-06 04:36:23 +07:00
parent 01e278befb
commit d88e547676
1 changed files with 27 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@@ -404,37 +404,44 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
} }
} }
else else if(compute_plan_.poses.size() == 1)
{ {
try try
{ {
auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_); // auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
auto prev_carrot_pose_it = transform_plan_.poses.begin(); // auto prev_carrot_pose_it = transform_plan_.poses.begin();
double distance_it = 0; // double distance_it = 0;
for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) // for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
{ // {
double dx = it->pose.x - carrot_pose_it->pose.x; // double dx = it->pose.x - carrot_pose_it->pose.x;
double dy = it->pose.y - carrot_pose_it->pose.y; // double dy = it->pose.y - carrot_pose_it->pose.y;
distance_it += std::hypot(dx, dy); // distance_it += std::hypot(dx, dy);
if (distance_it > costmap_robot_->getCostmap()->getResolution()) // if (distance_it > costmap_robot_->getCostmap()->getResolution())
{ // {
prev_carrot_pose_it = it; // prev_carrot_pose_it = it;
break; // break;
} // }
} // }
robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution() // robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) // ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
: robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D()); // : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); // robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
// teb_local_planner::PoseSE2 start_pose(front); // teb_local_planner::PoseSE2 start_pose(front);
// teb_local_planner::PoseSE2 goal_pose(back); // teb_local_planner::PoseSE2 goal_pose(back);
// const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec()); // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
const double dir_path = 0.0;
auto goal_pose = compute_plan_.poses.front().pose;
auto start_pose = pose.pose;
double angle_path = atan2(goal_pose.y - start_pose.y, goal_pose.x - start_pose.x);
double dir_path = cos(fabs(angle_path - goal_pose.theta));
if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
x_direction = dir_path > 0 ? FORWARD : BACKWARD; x_direction = dir_path > 0 ? FORWARD : BACKWARD;
else
x_direction = 0.0;
} }
catch (std::exception &e) catch (std::exception &e)
{ {