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

update

Browse Source
This commit is contained in:
hoangson02
2026-02-26 11:12:07 +00:00
parent ab3e65de1b
commit 83f0e85e4a
1 changed files with 34 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -802,7 +802,11 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
// Swap planner
try
{
tc_->swapPlanner(position_planner_name_);
if(!tc_->swapPlanner(position_planner_name_))
{
robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
return false;
}
}
catch (const std::exception &e)
{
@@ -917,7 +921,11 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
try
{
tc_->swapPlanner(position_planner_name_);
if(!tc_->swapPlanner(position_planner_name_))
{
robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
return false;
}
}
catch (const std::exception &e)
{
@@ -1027,7 +1035,11 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
tc_->swapPlanner(docking_planner_name_);
if(!tc_->swapPlanner(docking_planner_name_))
{
robot::log_error("[MoveBase::dockTo] Failed to swapPlanner");
return false;
}
}
catch (const std::exception &e)
{
@@ -1091,7 +1103,20 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
try
{
tc_->swapPlanner(go_straight_planner_name_);
robot::log_info("[MoveBase::moveStraightTo] Entry swapPlanner");
if(!tc_->swapPlanner(go_straight_planner_name_))
{
if(tf_ == nullptr)
{
robot::log_error("[MoveBase::moveTo] tf_ pointer is null!");
throw std::runtime_error("Null 'tf_' pointer encountered");
}
else
{
robot::log_info("[MoveBase::moveTo] tf_ pointer is not null!");
}
return false;
}
}
catch (const std::exception &e)
{
@@ -1198,7 +1223,11 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
try
{
tc_->swapPlanner(rotate_planner_name_);
if(!tc_->swapPlanner(rotate_planner_name_))
{
robot::log_error("[MoveBase::rotateTo] Failed to swapPlanner");
return false;
}
}
catch (const std::exception &e)
{