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
{
auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
auto prev_carrot_pose_it = transform_plan_.poses.begin();
double distance_it = 0;
for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
{
double dx = it->pose.x - carrot_pose_it->pose.x;
double dy = it->pose.y - carrot_pose_it->pose.y;
distance_it += std::hypot(dx, dy);
if (distance_it > costmap_robot_->getCostmap()->getResolution())
{
prev_carrot_pose_it = it;
break;
}
}
// auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
// auto prev_carrot_pose_it = transform_plan_.poses.begin();
// double distance_it = 0;
// for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
// {
// double dx = it->pose.x - carrot_pose_it->pose.x;
// double dy = it->pose.y - carrot_pose_it->pose.y;
// distance_it += std::hypot(dx, dy);
// if (distance_it > costmap_robot_->getCostmap()->getResolution())
// {
// prev_carrot_pose_it = it;
// break;
// }
// }
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(robot_geometry_msgs::Pose2D());
// 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(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 goal_pose(back);
// 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)
x_direction = dir_path > 0 ? FORWARD : BACKWARD;
else
x_direction = 0.0;
}
catch (std::exception &e)
{