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_)
{
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;