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

update init

Browse Source
This commit is contained in:
HiepLM
2025-12-24 14:29:58 +07:00
parent f0225ae5e3
commit 10379d5b16
107 changed files with 5532 additions and 767 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h
View File

@@ -136,7 +136,7 @@ namespace nav_core
* @brief Constructs the local planner
* @param name The name to give this instance of the local planner
* @param tf A pointer to a transform listener
* @param costmap_ros The cost map to use for assigning costs to local plans
* @param costmap_robot The cost map to use for assigning costs to local plans
*/
virtual void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) = 0;

3
src/Navigations/Cores/nav_core2/CMakeLists.txt
View File

@@ -28,8 +28,7 @@ target_link_libraries(nav_core2 INTERFACE
tf3
nav_grid
nav_2d_msgs
robot::node_handle
robot::console
robot_cpp
)
# Set include directories

14
src/Navigations/Cores/nav_core2/README.md
View File

@@ -3,15 +3,15 @@ A replacement interface for [nav_core](https://github.com/ros-planning/navigatio
There were a few key reasons for creating new interfaces rather than extending the old ones.
* Use `nav_2d_msgs` to eliminate unused data fields
* Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROS`.
* Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROBOT`.
* Provide more data in the interfaces for easier testing.
* Use Exceptions rather than booleans to provide more information about the types of errors encountered.
## `Costmap`
`costmap_2d::Costmap2DROS` has been a vital part of the navigation stack for years, but it was not without its flaws.
* Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROS`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested.
`costmap_2d::Costmap2DROBOT` has been a vital part of the navigation stack for years, but it was not without its flaws.
* Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROBOT`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested.
* Initialization also started an update thread, which is also not always needed in testing.
* Using `Costmap2DROS` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
* Using `Costmap2DROBOT` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `nav_grid::NavGrid<unsigned char>` for abstracting away the data storage and coordinate manipulation, and provides a few other key methods for costmap functioning such as
* a mutex
@@ -20,7 +20,7 @@ The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `n
The `Costmap` can be loaded using `pluginlib`, allowing for arbitrary implementations of underlying update algorithms, include the layered costmap approach.
One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROS`.
One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROBOT`.
Note: One could also imagine the `Costmap` interface being templatized itself like `NavGrid` instead of forcing use of `unsigned char`. While this may be possible, it was decided that it was a bit ambitious and the use of templates would force all of the implementation into headers, and would ultimately obfuscate the operation of algorithms.
@@ -29,7 +29,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
| `nav_core` | `nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
## Local Planner
@@ -37,7 +37,7 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
| `nav_core` | `nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const nav_2d_msgs::Path2D&)`||
|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|

2
src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h
View File

@@ -137,7 +137,7 @@ public:
* @class CostmapDataLagException
* @brief Indicates costmap is out of date because data in not up to date.
*
* Functions similarly to `!Costmap2DROS::isCurrent()`
* Functions similarly to `!Costmap2Drobot::isCurrent()`
*/
class CostmapDataLagException: public CostmapSafetyException
{

28
src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
View File

@@ -33,6 +33,7 @@ target_link_libraries(local_planner_adapter
Boost::system
dl
costmap_adapter
robot_cpp
${PACKAGES_DIR}
)
target_include_directories(local_planner_adapter PRIVATE
@@ -53,6 +54,33 @@ target_include_directories(global_planner_adapter PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
# Ensure runtime loader prefers in-tree adapter libs over system/ROS libs.
# Without this, dlopen() may resolve libcostmap_adapter.so from /opt/ros/noetic/lib,
# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
set(_NAV_CORE_ADAPTER_RPATH
"${CMAKE_BINARY_DIR}/src/Navigations/Cores/nav_core_adapter"
"${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
"${CMAKE_BINARY_DIR}/src/Libraries/tf3"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
)
set_target_properties(costmap_adapter PROPERTIES
BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
LINK_FLAGS "-Wl,--disable-new-dtags"
)
set_target_properties(local_planner_adapter PROPERTIES
BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
LINK_FLAGS "-Wl,--disable-new-dtags"
)
set_target_properties(global_planner_adapter PROPERTIES
BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
LINK_FLAGS "-Wl,--disable-new-dtags"
)
# Cài đặt header files
install(DIRECTORY include/nav_core_adapter
DESTINATION include

6
src/Navigations/Cores/nav_core_adapter/README.md
View File

@@ -3,7 +3,7 @@ This package contains adapters for using `nav_core` plugins as `nav_core2` plugi
* Converting between 2d and 3d datatypes.
* Converting between returning false and throwing exceptions on failure.
We also provide an adapter for using a `costmap_2d::Costmap2DROS` as a plugin for the `nav_core2::Costmap` interface.
We also provide an adapter for using a `costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
## Adapter Classes
* Global Planner Adapters
@@ -13,8 +13,8 @@ as a `nav_core2` plugin, like in `locomotor`.
* Local Planner Adapter
* `LocalPlannerAdapter` is used for employing a `nav_core2` local planner interface (such as `dwb_local_planner`) as a `nav_core` plugin, like in `move_base`. In addition to the standard adaptation steps listed above, the local planner adapter also uses the costmap to grab the global pose and subscribes to the odometry in order to get the current velocity.
* There is no `LocalPlannerAdapter2`. The `nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `nav_core` counterpart. This information would be ignored by a `nav_core` planner, so no adapter is provided.
* `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROS` object. It is not a perfect adaptation, because
* `Costmap2DROS` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though.
* `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROBOT` object. It is not a perfect adaptation, because
* `Costmap2DROBOT` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though.
* `setInfo` is not implemented.
## Parameter Setup

8
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h
View File

@@ -48,14 +48,14 @@ class CostmapAdapter : public nav_core2::Costmap
{
public:
/**
* @brief Deconstructor for possibly freeing the costmap_ros_ object
* @brief Deconstructor for possibly freeing the costmap_robot_ object
*/
virtual ~CostmapAdapter();
/**
* @brief Initialize from an existing Costmap2DROS object
* @param costmap_ros A Costmap2DROS object
* @param needs_destruction Whether to free the costmap_ros object when this class is destroyed
* @brief Initialize from an existing Costmap2DROBOT object
* @param costmap_robot A Costmap2DROBOT object
* @param needs_destruction Whether to free the costmap_robot object when this class is destroyed
*/
void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);

3
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h
View File

@@ -196,11 +196,10 @@ namespace nav_core_adapter
costmap_2d::Costmap2DROBOT *costmap_robot_;
boost::recursive_mutex configuration_mutex_;
// std::shared_ptr<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>> dsrv_;
robot::NodeHandle private_nh_;
robot::NodeHandle adapter_nh_;
// void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level);
nav_core_adapter::NavCoreAdapterConfig last_config_;
nav_core_adapter::NavCoreAdapterConfig default_config_;
bool setup_;

55
src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -41,6 +41,7 @@
#include <nav_2d_utils/parameters.h>
#include <nav_core2/exceptions.h>
#include <boost/dll/alias.hpp>
#include <boost/dll/import.hpp>
#include <memory>
#include <string>
#include <vector>
@@ -54,20 +55,19 @@ namespace nav_core_adapter
LocalPlannerAdapter::~LocalPlannerAdapter()
{
std::cout << "=== LocalPlannerAdapter destructor called ===" << std::endl;
robot::log_info("=== LocalPlannerAdapter destructor called ===");
if (planner_)
{
std::cout << "Planner use_count before reset: " << planner_.use_count() << std::endl;
planner_.reset();
std::cout << "Planner reset complete" << std::endl;
robot::log_info("Planner reset complete");
}
else
{
std::cout << "Planner already null" << std::endl;
robot::log_warning("Planner already null");
}
std::cout << "=== LocalPlannerAdapter destructor finished ===" << std::endl;
robot::log_info("=== LocalPlannerAdapter destructor finished ===");
}
/**
@@ -80,10 +80,9 @@ namespace nav_core_adapter
costmap_adapter_ = std::make_shared<CostmapAdapter>();
costmap_adapter_->initialize(costmap_robot);
robot::NodeHandle nh;
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
robot::log_info("Adapter namespace: %s", private_nh_.getNamespace().c_str());
std::string planner_name;
if (adapter_nh_.hasParam("planner_name"))
{
@@ -91,29 +90,35 @@ namespace nav_core_adapter
}
else
{
planner_name = nav_2d_utils::loadParameterWithDeprecation(
adapter_nh_, "planner_name", "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_.setParam("planner_name", planner_name);
}
robot::log_info("Loading plugin %s", planner_name.c_str());
// planner_ = planner_loader_.createInstance(planner_name);
std::string path_file_so;
planner_loader_ = boost::dll::import_alias<nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
try
{
robot::log_error("Failed to load planner %s", planner_name.c_str());
planner_loader_ = boost::dll::import_alias<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(adapter_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);
}
planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
// odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh);
// dsrv_ = std::make_shared<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>>(configuration_mutex_, adapter_nh_);
// dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>::CallbackType cb =
// boost::bind(&LocalPlannerAdapter::reconfigureCB, this, _1, _2);
// dsrv_->setCallback(cb);
}
bool LocalPlannerAdapter::swapPlanner(std::string planner_name)
@@ -140,7 +145,7 @@ namespace nav_core_adapter
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
}
new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
new_planner->initialize(adapter_nh_, planner_name, tf_, costmap_robot_);
if (planner_)
planner_.reset();
planner_ = new_planner;

12
src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp
View File

@@ -37,11 +37,11 @@
TEST(LocalPlannerAdapter, unload_local_planner)
{
tf2_ros::Buffer tf;
tf2_robot::Buffer tf;
tf.setUsingDedicatedThread(true);
// This empty transform is added to satisfy the constructor of
// Costmap2DROS, which waits for the transform from map to base_link
// Costmap2DROBOT, which waits for the transform from map to base_link
// to become available.
geometry_msgs::TransformStamped base_rel_map;
base_rel_map.child_frame_id = "base_link";
@@ -51,19 +51,19 @@ TEST(LocalPlannerAdapter, unload_local_planner)
nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter();
costmap_2d::Costmap2DROS costmap_ros("local_costmap", tf);
lpa->initialize("lpa", &tf, &costmap_ros);
costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf);
lpa->initialize("lpa", &tf, &costmap_robot);
delete lpa;
// Simple test to make sure costmap hasn't been deleted
EXPECT_EQ("map", costmap_ros.getGlobalFrameID());
EXPECT_EQ("map", costmap_robot.getGlobalFrameID());
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "unload_test");
robot::init(argc, argv, "unload_test");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

2
src/Navigations/Libraries/nav_grid/README.md
View File

@@ -1,6 +1,6 @@
# nav_grid
Many navigation algorithms rely on the concept of a two dimensional grid being overlaid on the world, with a value being assigned to each grid cell. In the original navigation stack, `Costmap2DROS` associated an `unsigned char` with grid cells, global planners cache distance to the goal as `float`s and local planners would cache various metrics in a grid to quickly calculate the strength of different trajectories.
Many navigation algorithms rely on the concept of a two dimensional grid being overlaid on the world, with a value being assigned to each grid cell. In the original navigation stack, `Costmap2DROBOT` associated an `unsigned char` with grid cells, global planners cache distance to the goal as `float`s and local planners would cache various metrics in a grid to quickly calculate the strength of different trajectories.
![nav_grid diagram](doc/nav_grid.png)

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

@@ -118,8 +118,7 @@ if(BUILDING_WITH_CATKIN)
nav_core
costmap_2d
xmlrpcpp
robot::node_handle
robot::console
robot_cpp
tf3_sensor_msgs
tf3_geometry_msgs
dl
@@ -147,11 +146,12 @@ else()
data_convert
dl
pthread
nav_2d_utils
)
target_link_libraries(move_base
PUBLIC ${PACKAGES_DIR}
PUBLIC robot::node_handle robot::console
PUBLIC robot_cpp
PRIVATE Boost::boost
)
@@ -183,7 +183,7 @@ endif()
# ========================================================
add_executable(move_base_main src/move_base_main.cpp)
target_link_libraries(move_base_main
PRIVATE move_base
PRIVATE move_base robot_cpp
PRIVATE Boost::boost
)
@@ -193,6 +193,18 @@ if(BUILDING_WITH_CATKIN)
)
endif()
# Set RPATH for executable to find libraries in build directory first
# Use RPATH instead of RUNPATH for higher priority
set_target_properties(move_base_main PROPERTIES
BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp:${CMAKE_BINARY_DIR}/src/Libraries/nav_2d_utils"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp:${CMAKE_BINARY_DIR}/src/Libraries/nav_2d_utils"
BUILD_WITH_INSTALL_RPATH FALSE
INSTALL_RPATH_USE_LINK_PATH TRUE
SKIP_BUILD_RPATH FALSE
)
# Force use of RPATH instead of RUNPATH
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--disable-new-dtags")
# ========================================================
# Installation
# ========================================================

12
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@@ -295,6 +295,7 @@ namespace move_base
std::string robot_base_frame_, global_frame_;
std::vector<std::string> recovery_behavior_names_;
unsigned int recovery_index_;
bool recovery_behavior_enabled_;
geometry_msgs::PoseStamped global_pose_;
double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
@@ -302,10 +303,10 @@ namespace move_base
int32_t max_planning_retries_;
uint32_t planning_retries_;
double conservative_reset_dist_, clearing_radius_;
// ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
// ros::Subscriber goal_sub_;
// ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
// robot::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
// robot::Subscriber goal_sub_;
// robot::ServiceServer make_plan_srv_, clear_costmaps_srv_;
bool shutdown_costmaps_, clearing_rotation_allowed_;
bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
double oscillation_timeout_, oscillation_distance_;
@@ -327,9 +328,6 @@ namespace move_base
geometry_msgs::PoseStamped planner_goal_;
boost::thread *planner_thread_;
// boost::recursive_mutex configuration_mutex_;
// dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_;
// void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
move_base::MoveBaseConfig last_config_;
move_base::MoveBaseConfig default_config_;

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

@@ -32,6 +32,7 @@ move_base::MoveBase::MoveBase(TFListenerPtr tf)
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false)
{
initialize(tf);
}
move_base::MoveBase::~MoveBase()
@@ -39,49 +40,66 @@ move_base::MoveBase::~MoveBase()
recovery_behaviors_.clear();
if (planner_costmap_robot_ != NULL)
{
delete planner_costmap_robot_;
planner_costmap_robot_ = NULL;
}
if (controller_costmap_robot_ != NULL)
{
delete controller_costmap_robot_;
controller_costmap_robot_ = NULL;
}
planner_thread_->interrupt();
planner_thread_->join();
if (planner_thread_ != NULL)
{
planner_thread_->interrupt();
planner_thread_->join();
delete planner_thread_;
planner_thread_ = NULL;
}
if (planner_plan_ != NULL)
{
delete planner_plan_;
planner_plan_ = NULL;
}
if (latest_plan_ != NULL)
{
delete latest_plan_;
latest_plan_ = NULL;
}
if (controller_plan_ != NULL)
{
delete controller_plan_;
controller_plan_ = NULL;
}
delete planner_thread_;
delete planner_plan_;
delete latest_plan_;
delete controller_plan_;
planner_.reset();
tc_.reset();
initialized_ = false;
tf_.reset();
planner_plan_ = NULL;
latest_plan_ = NULL;
controller_plan_ = NULL;
planner_thread_ = NULL;
planner_costmap_robot_ = NULL;
controller_costmap_robot_ = NULL;
}
void move_base::MoveBase::initialize(TFListenerPtr tf)
{
printf("[%s:%d] ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
if (!initialized_)
{
tf_ = tf;
// NodeHandle("~") will automatically load YAML files from config directory
robot::NodeHandle nh("~");
private_nh_ = nh;
private_nh_.printAllParams();
private_nh_ = robot::NodeHandle("~");
recovery_trigger_ = PLANNING_R;
// get some parameters that will be global to the move base node
std::string global_planner, local_planner;
private_nh_.getParam("base_global_planner", global_planner, std::string(""));
printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str());
robot::log_info("[%s %d] global_planner: %s", __FILE__, __LINE__, global_planner.c_str());
private_nh_.getParam("base_local_planner", local_planner, std::string(""));
printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str());
robot::log_info("[%s %d] local_planner: %s", __FILE__, __LINE__, local_planner.c_str());
// Handle nested YAML nodes for costmap config
std::string robot_base_frame;
@@ -135,7 +153,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
// set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
// we'll assume the radius of the robot to be consistent with what's specified for the costmaps
// From config param
double inscribed_radius;
@@ -151,10 +168,11 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
// create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
planner_costmap_robot_->pause();
// initialize the global planner
try
{
@@ -166,17 +184,17 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
planner_ = planner_loader_();
if (!planner_)
{
robot::log_error("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
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);
}
if(planner_->initialize(global_planner, planner_costmap_robot_))
robot::log_info("[%s:%d] Global planner initialized successfully", __FILE__, __LINE__);
robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__);
else
robot::log_error("[%s:%d] Global planner initialized failed", __FILE__, __LINE__);
robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__);
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d] EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what());
robot::log_error("[%s:%d]\n EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what());
throw std::runtime_error("Failed to create the " + global_planner + " planner");
}
// create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
@@ -188,18 +206,18 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so";
controller_loader_ =
boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations);
path_file_so, local_planner, boost::dll::load_mode::append_decorations);
tc_ = controller_loader_();
if (!tc_)
{
robot::log_error("[%s:%d] ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__);
robot::log_error("[%s:%d]\n ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__);
throw std::runtime_error("Failed to load local planner " + local_planner);
}
// tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d] EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what());
robot::log_error("[%s:%d]\n EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what());
throw std::runtime_error("Failed to create the " + local_planner + " planner");
}
@@ -207,7 +225,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
planner_costmap_robot_->start();
controller_costmap_robot_->start();
try
{
old_linear_fwd_ = tc_->getTwistLinear(true);
@@ -221,12 +238,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
std::cerr << e.what() << '\n';
}
// // advertise a service for getting a plan
// make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this);
// // advertise a service for clearing the costmaps
// clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this);
// if we shutdown our costmaps when we're deactivated... we'll do that now
if (shutdown_costmaps_)
{
@@ -237,7 +248,7 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
// load any user specified recovery behaviors, and if that fails load the defaults
if (!loadRecoveryBehaviors(private_nh_))
{
robot::log_warning("[%s:%d] Loading default recovery behaviors", __FILE__, __LINE__);
robot::log_warning("[%s:%d]\n Loading default recovery behaviors", __FILE__, __LINE__);
loadDefaultRecoveryBehaviors();
}
@@ -254,15 +265,15 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
}
else
{
robot::log_error("[%s:%d] ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
}
initialized_ = true;
robot::log_info("[%s:%d] ========== End: initialize() - SUCCESS ==========", __FILE__, __LINE__);
robot::log_info("========== End: initialize() - SUCCESS ==========");
}
else
{
robot::log_warning("[%s:%d] Already initialized, skipping", __FILE__, __LINE__);
robot::log_warning("[%s:%d]\n Already initialized, skipping", __FILE__, __LINE__);
}
}
@@ -336,7 +347,7 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
if (cancel_ctr_)
cancel_ctr_ = false;
// move_base_msgs::MoveBaseActionGoal action_goal;
// action_goal.header.stamp = ros::Time::now();
// action_goal.header.stamp = robot::Time::now();
// action_goal.goal.target_pose = goal;
// action_goal_pub_.publish(action_goal);
@@ -427,7 +438,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
cancel_ctr_ = false;
// move_base_msgs::MoveBaseActionGoal action_goal;
// action_goal.header.stamp = ros::Time::now();
// action_goal.header.stamp = robot::Time::now();
// action_goal.goal.target_pose = goal;
// action_goal_pub_.publish(action_goal);
lock.unlock();
@@ -478,7 +489,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
if (cancel_ctr_)
cancel_ctr_ = false;
// move_base_msgs::MoveBaseActionGoal action_goal;
// action_goal.header.stamp = ros::Time::now();
// action_goal.header.stamp = robot::Time::now();
// action_goal.goal.target_pose = goal;
// action_goal_pub_.publish(action_goal);
@@ -543,7 +554,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
}
// move_base_msgs::MoveBaseActionGoal action_goal;
// action_goal.header.stamp = ros::Time::now();
// action_goal.header.stamp = robot::Time::now();
// action_goal.goal.target_pose = goal;
// action_goal.goal.target_pose.pose.position.x = pose.pose.position.x + 0.05 * cos(tf2::getYaw(pose.pose.orientation));
// action_goal.goal.target_pose.pose.position.y = pose.pose.position.y + 0.05 * sin(tf2::getYaw(pose.pose.orientation));
@@ -687,26 +698,26 @@ bool move_base::MoveBase::setTwistAngular(const geometry_msgs::Vector3 &angular)
void move_base::MoveBase::setYawGoalTolerance(double yaw_goal_tolerance)
{
double yaw = 0.0;
if (private_nh_.getParam("yaw_goal_tolerance", yaw, 0.0))
{
private_nh_.setParam("yaw_goal_tolerance", yaw_goal_tolerance);
std::cout << private_nh_.getNamespace() << " yaw_goal_tolerance " << yaw_goal_tolerance << std::endl;
}
else
std::cerr << "yaw_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl;
// double yaw = 0.0;
// if (private_nh_.getParam("yaw_goal_tolerance", yaw, 0.0))
// {
// private_nh_.setParam("yaw_goal_tolerance", yaw_goal_tolerance);
// std::cout << private_nh_.getNamespace() << " yaw_goal_tolerance " << yaw_goal_tolerance << std::endl;
// }
// else
// std::cerr << "yaw_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl;
}
void move_base::MoveBase::setXyGoalTolerance(double xy_goal_tolerance)
{
double xy = 0.0;
if (private_nh_.getParam("xy_goal_tolerance", xy, 0.0))
{
private_nh_.setParam("xy_goal_tolerance", xy_goal_tolerance);
std::cout << private_nh_.getNamespace() << " xy_goal_tolerance " << xy_goal_tolerance << std::endl;
}
else
std::cerr << "xy_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl;
// double xy = 0.0;
// if (private_nh_.getParam("xy_goal_tolerance", xy, 0.0))
// {
// private_nh_.setParam("xy_goal_tolerance", xy_goal_tolerance);
// std::cout << private_nh_.getNamespace() << " xy_goal_tolerance " << xy_goal_tolerance << std::endl;
// }
// else
// std::cerr << "xy_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl;
}
// bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
@@ -754,90 +765,89 @@ bool move_base::MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std::
bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
{
YAML::Node behavior_list = node.getParamValue("recovery_behaviors");
if (behavior_list && behavior_list.IsSequence())
{
// Validate all behaviors first
for (size_t i = 0; i < behavior_list.size(); ++i)
{
YAML::Node behavior = behavior_list[i].as<YAML::Node>();
if (!behavior.IsMap())
{
std::cerr << "Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead." << std::endl;
return false;
}
// YAML::Node behavior_list = node.getParamValue("recovery_behaviors");
// if (behavior_list && behavior_list.IsSequence())
// {
// // Validate all behaviors first
// for (size_t i = 0; i < behavior_list.size(); ++i)
// {
// YAML::Node behavior = behavior_list[i].as<YAML::Node>();
// if (!behavior.IsMap())
// {
// robot::log_warning("Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead.");
// return false;
// }
if (!behavior["name"] || !behavior["type"])
{
std::cerr << "Recovery behaviors must have a name and a type. Using the default recovery behaviors instead." << std::endl;
return false;
}
// if (!behavior["name"] || !behavior["type"])
// {
// robot::log_warning("Recovery behaviors must have a name and a type. Using the default recovery behaviors instead.");
// return false;
// }
// check for recovery behaviors with the same name
std::string name_i = behavior["name"].as<std::string>();
printf("[%s:%d] name_i: %s\n", __FILE__, __LINE__, name_i.c_str());
for (size_t j = i + 1; j < behavior_list.size(); ++j)
{
YAML::Node behavior_j = behavior_list[j].as<YAML::Node>();
if (behavior_j.IsMap() && behavior_j["name"])
{
std::string name_j = behavior_j["name"].as<std::string>();
if (name_i == name_j)
{
std::cerr << "A recovery behavior with the name " << name_i << " already exists, this is not allowed. Using the default recovery behaviors instead." << std::endl;
return false;
}
}
}
}
// // check for recovery behaviors with the same name
// std::string name_i = behavior["name"].as<std::string>();
// for (size_t j = i + 1; j < behavior_list.size(); ++j)
// {
// YAML::Node behavior_j = behavior_list[j].as<YAML::Node>();
// if (behavior_j.IsMap() && behavior_j["name"])
// {
// std::string name_j = behavior_j["name"].as<std::string>();
// if (name_i == name_j)
// {
// robot::log_warning("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.", name_i.c_str());
// return false;
// }
// }
// }
// }
// if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
for (size_t i = 0; i < behavior_list.size(); ++i)
{
try
{
YAML::Node behavior = behavior_list[i].as<YAML::Node>();
// Load recovery behavior using boost::dll::import_alias
// behavior["type"] should be the factory function name (e.g., "create_plugin")
std::string behavior_type = behavior["type"].as<std::string>();
std::string behavior_name = behavior["name"].as<std::string>();
std::string path_file_so;
// Load the factory function from the shared library
auto recovery_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
// // if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
// for (size_t i = 0; i < behavior_list.size(); ++i)
// {
// try
// {
// YAML::Node behavior = behavior_list[i].as<YAML::Node>();
// // Load recovery behavior using boost::dll::import_alias
// // behavior["type"] should be the factory function name (e.g., "create_plugin")
// std::string behavior_type = behavior["type"].as<std::string>();
// std::string behavior_name = behavior["name"].as<std::string>();
// std::string path_file_so;
// // Load the factory function from the shared library
// auto recovery_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
// path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
// Create instance using the factory function
std::shared_ptr<nav_core::RecoveryBehavior> behavior_ptr(recovery_loader());
// // Create instance using the factory function
// std::shared_ptr<nav_core::RecoveryBehavior> behavior_ptr(recovery_loader());
// shouldn't be possible, but it won't hurt to check
if (behavior_ptr == nullptr)
{
std::cerr << "The factory function returned a null pointer without throwing an exception. This should not happen" << std::endl;
return false;
}
// initialize the recovery behavior with its name
behavior_ptr->initialize(behavior_name, tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
recovery_behavior_names_.push_back(behavior_name);
recovery_behaviors_.push_back(behavior_ptr);
}
catch (const std::exception &ex)
{
std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as<std::string>()
<< ". Error: " << ex.what() << std::endl;
return false;
}
}
}
else if (behavior_list)
{
std::cerr << "The recovery behavior specification must be a list. We'll use the default recovery behaviors instead." << std::endl;
return false;
}
else
{
// if no recovery_behaviors are specified, we'll just load the defaults
return false;
}
// // shouldn't be possible, but it won't hurt to check
// if (behavior_ptr == nullptr)
// {
// std::cerr << "The factory function returned a null pointer without throwing an exception. This should not happen" << std::endl;
// return false;
// }
// // initialize the recovery behavior with its name
// behavior_ptr->initialize(behavior_name, tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
// recovery_behavior_names_.push_back(behavior_name);
// recovery_behaviors_.push_back(behavior_ptr);
// }
// catch (const std::exception &ex)
// {
// std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as<std::string>()
// << ". Error: " << ex.what() << std::endl;
// return false;
// }
// }
// }
// else if (behavior_list)
// {
// std::cerr << "The recovery behavior specification must be a list. We'll use the default recovery behaviors instead." << std::endl;
// return false;
// }
// else
// {
// // if no recovery_behaviors are specified, we'll just load the defaults
// return false;
// }
// if we've made it here... we've constructed a recovery behavior list successfully
return true;
@@ -856,7 +866,7 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors()
// first, we'll load a recovery behavior to clear the costmap (conservative reset)
try
{
std::string path_file_so;
std::string path_file_so ;
auto clear_costmap_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
std::shared_ptr<nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader());

69
src/Navigations/Packages/move_base/src/move_base_main.cpp
View File

@@ -29,12 +29,71 @@
#include <move_base/move_base.h>
#include <iostream>
#include <robot/node_handle.h>
#include <nav_2d_utils/parameters.h>
#include <boost/dll/import.hpp>
#include <robot/console.h>
int main(int argc, char** argv)
{
std::shared_ptr<tf3::BufferCore> tf = std::make_shared<tf3::BufferCore>();
move_base::MoveBase move_base;
move_base.initialize(tf);
std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl;
return(0);
try {
std::shared_ptr<tf3::BufferCore> tf = std::make_shared<tf3::BufferCore>();
robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
"/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so",
"MoveBase", boost::dll::load_mode::append_decorations);
robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
if (nav_ptr == nullptr) {
robot::log_error("[%s:%d]\n Error: Failed to create navigation", __FILE__, __LINE__);
return 1;
}
robot::log_info("[%s:%d]\n Navigation created, initializing...", __FILE__, __LINE__);
nav_ptr->initialize(tf);
robot::log_info("[%s:%d]\n Navigation initialized successfully", __FILE__, __LINE__);
std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl;
// Explicitly reset before create_plugin goes out of scope to ensure proper cleanup order
robot::log_info("[%s:%d]\n Cleaning up...", __FILE__, __LINE__);
nav_ptr.reset();
robot::log_info("[%s:%d]\n Cleanup complete", __FILE__, __LINE__);
return 0;
} catch (const std::exception &e) {
robot::log_error("[%s:%d]\n Exception: %s", __FILE__, __LINE__, e.what());
return 1;
} catch (...) {
robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__);
return 1;
}
// robot::NodeHandle private_nh_ = robot::NodeHandle("~");
// robot::NodeHandle adapter_nh_ = robot::NodeHandle(private_nh_, "LocalPlannerAdapter");
// robot::log_info_at(__FILE__, __LINE__, "\nUsing adapter namespace: %s", adapter_nh_.getNamespace().c_str());
// 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 = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
// adapter_nh_.setParam("planner_name", planner_name);
// }
// robot::log_info_at(__FILE__, __LINE__, "Planner name: %s", planner_name.c_str());
// robot::NodeHandle parent_;
// parent_ = adapter_nh_;
// robot::NodeHandle planner_nh_ = robot::NodeHandle(parent_, "PNKXLocalPlanner");
// std::string algorithm_nav_name;
// planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
// robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
return 0;
}