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

add docking to

Browse Source
This commit is contained in:
QUYVN
2026-03-19 04:02:08 +00:00
parent 98ce71eb69
commit 180a646e35
6 changed files with 144 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -812,7 +812,17 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
{
if (setup_)
{
swapPlanner(default_config_.base_global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(default_config_.base_global_planner);
}
}
else
{
@@ -963,7 +973,17 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
{
if (setup_)
{
swapPlanner(default_config_.base_global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(default_config_.base_global_planner);
}
}
else
{
@@ -1120,7 +1140,17 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
if (setup_)
{
swapPlanner(default_config_.base_global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(default_config_.base_global_planner);
}
}
else
{
@@ -1233,7 +1263,17 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
if (setup_)
{
swapPlanner(default_config_.base_global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(default_config_.base_global_planner);
}
}
else
{
@@ -1342,7 +1382,17 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
if (setup_)
{
std::string global_planner = "TwoPointsPlanner";
swapPlanner(global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, go_straight_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(global_planner);
}
}
else
{
@@ -1465,7 +1515,17 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
if (setup_)
{
std::string global_planner = "TwoPointsPlanner";
swapPlanner(global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, rotate_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(global_planner);
}
}
else
{