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

update 5/2

Browse Source
This commit is contained in:
HiepLM
2026-02-05 14:13:40 +07:00
parent cab5655769
commit a43b714f01
13 changed files with 409 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
View File

@@ -197,7 +197,8 @@ namespace robot_nav_core_adapter
// Plugin handling
boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
robot_nav_core2::LocalPlanner::Ptr planner_;
bool initialized_;
// Pointer Copies
TFListenerPtr tf_;
@@ -211,7 +212,6 @@ namespace robot_nav_core_adapter
robot_nav_core_adapter::NavCoreAdapterConfig last_config_;
robot_nav_core_adapter::NavCoreAdapterConfig default_config_;
bool setup_;
};
} // namespace robot_nav_core_adapter

125
src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -75,87 +75,104 @@ namespace robot_nav_core_adapter
*/
void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
{
tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
costmap_robot_ = costmap_robot;
costmap_adapter_ = std::make_shared<CostmapAdapter>();
costmap_adapter_->initialize(costmap_robot);
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
std::string planner_name;
if (adapter_nh_.hasParam("planner_name"))
if (!initialized_)
{
adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
costmap_robot_ = costmap_robot;
costmap_adapter_ = std::make_shared<CostmapAdapter>();
costmap_adapter_->initialize(costmap_robot);
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
std::string planner_name;
if (adapter_nh_.hasParam("planner_name"))
{
adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
}
else
{
planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_.setParam("planner_name", planner_name);
}
try
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(planner_name);
planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str());
exit(EXIT_FAILURE);
}
last_config_.planner_name = planner_name;
robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str());
planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str());
}
catch (const boost::system::system_error &ex)
{
robot::log_error_at(__FILE__, __LINE__, "System error while loading planner \"%s\": %s", planner_name.c_str(), ex.what());
exit(EXIT_FAILURE);
}
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what());
exit(EXIT_FAILURE);
}
initialized_ = true;
robot::log_info("LocalPlannerAdapter initialized successfully");
}
else
{
planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_.setParam("planner_name", planner_name);
}
try
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(planner_name);
planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str());
exit(EXIT_FAILURE);
}
robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str());
planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str());
}
catch (const boost::system::system_error &ex)
{
robot::log_error_at(__FILE__, __LINE__, "System error while loading planner \"%s\": %s", planner_name.c_str(), ex.what());
exit(EXIT_FAILURE);
}
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what());
exit(EXIT_FAILURE);
robot::log_info("LocalPlannerAdapter already initialized");
}
}
bool LocalPlannerAdapter::swapPlanner(std::string planner_name)
{
boost::recursive_mutex::scoped_lock l(configuration_mutex_);
if (!setup_)
if (!initialized_)
{
return false;
}
if (planner_name != last_config_.planner_name)
{
robot_nav_core2::LocalPlanner::Ptr new_planner;
robot_nav_core2::LocalPlanner::Ptr old_planner = planner_;
auto old_loader = planner_loader_;
try
{
// Tạo planner mới
std::string path_file_so;
planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
boost::unique_lock<boost::recursive_mutex> lock(configuration_mutex_);
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(planner_name);
auto new_loader = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
new_planner = planner_loader_();
auto new_planner = new_loader();
if (!new_planner)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str());
throw std::runtime_error("Failed to load planner " + planner_name);
}
new_planner->initialize(adapter_nh_, planner_name, tf_, costmap_robot_);
if (planner_)
planner_.reset();
new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
planner_ = new_planner;
planner_loader_ = new_loader;
last_config_.planner_name = planner_name;
robot::log_info("Loaded new planner: %s", planner_name.c_str());
// Release old planner while old loader is still alive.
old_planner.reset();
old_loader = boost::function<robot_nav_core2::LocalPlanner::Ptr()>();
lock.unlock();
}
catch (const std::exception &ex)
{
robot::log_error("Failed to load planner %s: %s", planner_name.c_str(), ex.what());
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: %s", planner_name.c_str(), ex.what());
planner_ = old_planner;
planner_loader_ = old_loader;
return false;
}
}

6
src/Navigations/Packages/move_base/CMakeLists.txt
View File

@@ -4,7 +4,7 @@ project(move_base VERSION 1.0.0 LANGUAGES CXX)
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building move_base with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building move_base with Standalone CMake")
@@ -90,6 +90,10 @@ add_library(${PROJECT_NAME} SHARED
if(BUILDING_WITH_CATKIN)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_compile_definitions(${PROJECT_NAME}
PUBLIC BUILD_WITH_ROS
)
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

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

@@ -4,6 +4,9 @@
*
* Author: Kevin Lee
*********************************************************************/
#ifdef BUILD_WITH_ROS
#include <ros/ros.h>
#endif
#include <move_base/move_base.h>
#include <move_base/convert_data.h>
#include <cmath>
@@ -231,6 +234,8 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
throw std::runtime_error("Failed to load global planner " + global_planner);
}
last_config_.base_global_planner = global_planner;
default_config_.base_global_planner = global_planner;
if (planner_->initialize(global_planner, planner_costmap_robot_))
robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__);
else
@@ -333,7 +338,54 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
void move_base::MoveBase::swapPlanner(std::string base_global_planner)
{
if (base_global_planner != last_config_.base_global_planner)
{
robot_nav_core::BaseGlobalPlanner::Ptr old_planner = planner_;
auto old_loader = planner_loader_;
// initialize the global planner
try
{
// wait for the current planner to finish planning
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(base_global_planner);
auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
auto new_planner = new_loader();
if (!new_planner)
{
robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr", __FILE__, __LINE__);
throw std::runtime_error("Failed to load global planner " + base_global_planner);
}
if (planner_plan_)
planner_plan_->clear();
if (latest_plan_)
latest_plan_->clear();
if (controller_plan_)
controller_plan_->clear();
resetState();
new_planner->initialize(base_global_planner, planner_costmap_robot_);
planner_ = new_planner;
planner_loader_ = new_loader;
last_config_.base_global_planner = base_global_planner;
// Release old planner while old loader is still alive.
old_planner.reset();
old_loader = boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()>();
lock.unlock();
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d]\n Failed to create the %s planner, are you sure it is properly registered and that the \
containing library is built? Exception: %s",__FILE__, __LINE__, base_global_planner.c_str(), ex.what());
planner_ = old_planner;
planner_loader_ = old_loader;
}
}
}
void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt)
@@ -788,8 +840,9 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase] Exception in swapPlanner: %s", e.what());
throw std::exception(e);
robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
@@ -897,8 +950,9 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
}
catch (const std::exception &e)
{
std::cerr << e.what() << "\n";
throw std::exception(e);
robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
@@ -923,6 +977,7 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
{
robot::log_info("[MoveBase::moveStraightTo] Entry");
if (setup_)
{
std::string global_planner = "TwoPointsPlanner";
@@ -938,12 +993,10 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
}
return false;
}
if (fabs(xy_goal_tolerance) > 0.001)
this->setXyGoalTolerance(fabs(xy_goal_tolerance));
else
this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
@@ -954,7 +1007,17 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
tc_->swapPlanner(go_straight_planner_name_);
try
{
tc_->swapPlanner(go_straight_planner_name_);
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::moveStraightTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
{
@@ -965,6 +1028,48 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
if (cancel_ctr_)
cancel_ctr_ = false;
// Check if action server exists
if (!as_)
{
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server");
}
catch (const std::exception &e)
{
lock.unlock();
robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
return false;
}
lock.unlock();
return true;
}
@@ -987,7 +1092,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
return false;
}
this->setXyGoalTolerance(0.25);
this->setXyGoalTolerance(std::numeric_limits<double>::infinity());
if (fabs(yaw_goal_tolerance) > 0.001)
this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
@@ -1003,7 +1108,17 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
tc_->swapPlanner(rotate_planner_name_);
try
{
tc_->swapPlanner(rotate_planner_name_);
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::rotateTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
{
@@ -1013,17 +1128,49 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
}
if (cancel_ctr_)
cancel_ctr_ = false;
robot_geometry_msgs::PoseStamped pose;
if (!this->getRobotPose(pose))
// Check if action server exists
if (!as_)
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
nav_feedback_->goal_checked = false;
}
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
robot::log_info("[MoveBase::rotateTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server");
}
catch (const std::exception &e)
{
lock.unlock();
robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
return false;
}
lock.unlock();
return true;
}