From 7baa7000b84d2a2881aba6f64567303fc8ec0ec3 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Sun, 22 Mar 2026 09:16:05 +0700 Subject: [PATCH] update path --- .../src/pnkx_docking_local_planner.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index 51e318f..885aa1e 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -398,7 +398,6 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S else if (!ret_nav_) { traj = nav_algorithm_->calculator(pose, velocity); - if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_) { if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_) @@ -406,9 +405,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S traj = dkpl_.front()->docking_nav_->calculator(pose, velocity); } } + local_plan_.header.stamp = robot::Time::now(); + robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now()); + local_plan_ = robot_nav_2d_utils::pathToPath(path); } else + { traj = rotate_algorithm_->calculator(pose, velocity); + local_plan_.header.stamp = robot::Time::now(); + robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now()); + local_plan_ = robot_nav_2d_utils::pathToPath(path); + } cmd_vel.velocity = traj.velocity; return cmd_vel;