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

Merge remote-tracking branch 'origin/3.0' into awc-devel

Browse Source
This commit is contained in:
hoangson02
2026-03-22 02:58:56 +00:00
parent 6ff54e4154 7baa7000b8
commit cf0c6e7caf
1 changed files with 8 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
View File

@@ -398,7 +398,6 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
else if (!ret_nav_) else if (!ret_nav_)
{ {
traj = nav_algorithm_->calculator(pose, velocity); traj = nav_algorithm_->calculator(pose, velocity);
if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_) if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
{ {
if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_) 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); 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 else
{
traj = rotate_algorithm_->calculator(pose, velocity); 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; cmd_vel.velocity = traj.velocity;
return cmd_vel; return cmd_vel;