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

update

Browse Source
This commit is contained in:
HiepLM 2026-01-09 10:39:30 +07:00
parent a3e5168d88
commit f5e7e1f1e0
5 changed files with 385 additions and 311 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/Algorithms/Packages/global_planners/custom_planner

@ -1 +1 @@
Subproject commit 540d79321b53ef36a4979f15c6e36b759c134e07 Subproject commit a2bebb5be969471a135601da78866cc9d1db84b1

74
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
View File

@ -9,6 +9,7 @@
#include <robot_geometry_msgs/Pose2D.h> #include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Vector3.h> #include <robot_geometry_msgs/Vector3.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h> #include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_msgs/Odometry.h>
#include <robot_nav_msgs/OccupancyGrid.h> #include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h> #include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud.h> #include <robot_sensor_msgs/PointCloud.h>
@ -199,115 +200,115 @@ namespace move_base_core
* @brief Get the robot's footprint (outline shape) in the global frame. * @brief Get the robot's footprint (outline shape) in the global frame.
* @return A vector of points representing the robot's footprint polygon. * @return A vector of points representing the robot's footprint polygon.
*/ */
virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() const = 0; virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() = 0;
/** /**
* @brief Add a static map to the navigation system. * @brief Add a static map to the navigation system.
* @param frame_id The frame ID of the map. * @param map_name The name of the map.
* @param map The map to add. * @param map The map to add.
*/ */
virtual void addStaticMap(const std::string &frame_id, robot_nav_msgs::OccupancyGrid *map) = 0; virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) = 0;
/** /**
* @brief Add a laser scan to the navigation system. * @brief Add a laser scan to the navigation system.
* @param frame_id The frame ID of the laser scan. * @param laser_scan_name The name of the laser scan.
* @param laser_scan The laser scan to add. * @param laser_scan The laser scan to add.
*/ */
virtual void addLaserScan(const std::string &frame_id, robot_sensor_msgs::LaserScan *laser_scan) = 0; virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) = 0;
/** /**
* @brief Add a point cloud to the navigation system. * @brief Add a point cloud to the navigation system.
* @param frame_id The frame ID of the point cloud. * @param point_cloud_name The name of the point cloud.
* @param point_cloud The point cloud to add. * @param point_cloud The point cloud to add.
*/ */
virtual void addPointCloud(const std::string &frame_id, robot_sensor_msgs::PointCloud *point_cloud) = 0; virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) = 0;
/** /**
* @brief Add a point cloud2 to the navigation system. * @brief Add a point cloud2 to the navigation system.
* @param frame_id The frame ID of the point cloud2. * @param point_cloud2_name The name of the point cloud2.
* @param point_cloud2 The point cloud2 to add. * @param point_cloud2 The point cloud2 to add.
*/ */
virtual void addPointCloud2(const std::string &frame_id, robot_sensor_msgs::PointCloud2 *point_cloud2) = 0; virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0;
/** /**
* @brief Get a static map from the navigation system. * @brief Get a static map from the navigation system.
* @param frame_id The frame ID of the map. * @param map_name The name of the map.
* @return The map. * @return The map.
*/ */
virtual robot_nav_msgs::OccupancyGrid* getStaticMap(const std::string &frame_id) = 0; virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) = 0;
/** /**
* @brief Get a laser scan from the navigation system. * @brief Get a laser scan from the navigation system.
* @param frame_id The frame ID of the laser scan. * @param laser_scan_name The name of the laser scan.
* @return The laser scan. * @return The laser scan.
*/ */
virtual robot_sensor_msgs::LaserScan* getLaserScan(const std::string &frame_id) = 0; virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) = 0;
/** /**
* @brief Get a point cloud from the navigation system. * @brief Get a point cloud from the navigation system.
* @param frame_id The frame ID of the point cloud. * @param point_cloud_name The name of the point cloud.
* @return The point cloud. * @return The point cloud.
*/ */
virtual robot_sensor_msgs::PointCloud* getPointCloud(const std::string &frame_id) = 0; virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) = 0;
/** /**
* @brief Get a point cloud2 from the navigation system. * @brief Get a point cloud2 from the navigation system.
* @param frame_id The frame ID of the point cloud2. * @param point_cloud2_name The name of the point cloud2.
* @return The point cloud2. * @return The point cloud2.
*/ */
virtual robot_sensor_msgs::PointCloud2* getPointCloud2(const std::string &frame_id) = 0; virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) = 0;
/** /**
* @brief Get all static maps from the navigation system. * @brief Get all static maps from the navigation system.
* @return The static maps. * @return The static maps.
*/ */
virtual std::map<std::string, robot_nav_msgs::OccupancyGrid*> getAllStaticMaps() = 0; virtual std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() = 0;
/** /**
* @brief Get all laser scans from the navigation system. * @brief Get all laser scans from the navigation system.
* @return The laser scans. * @return The laser scans.
*/ */
virtual std::map<std::string, robot_sensor_msgs::LaserScan*> getAllLaserScans() = 0; virtual std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() = 0;
/** /**
* @brief Get all point clouds from the navigation system. * @brief Get all point clouds from the navigation system.
* @return The point clouds. * @return The point clouds.
*/ */
virtual std::map<std::string, robot_sensor_msgs::PointCloud*> getAllPointClouds() = 0; virtual std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() = 0;
/** /**
* @brief Get all point cloud2s from the navigation system. * @brief Get all point cloud2s from the navigation system.
* @return The point cloud2s. * @return The point cloud2s.
*/ */
virtual std::map<std::string, robot_sensor_msgs::PointCloud2*> getAllPointCloud2s() = 0; virtual std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() = 0;
/** /**
* @brief Remove a static map from the navigation system. * @brief Remove a static map from the navigation system.
* @param frame_id The frame ID of the map. * @param map_name The name of the map.
* @return True if the map was removed successfully. * @return True if the map was removed successfully.
*/ */
virtual bool removeStaticMap(const std::string &frame_id) = 0; virtual bool removeStaticMap(const std::string &map_name) = 0;
/** /**
* @brief Remove a laser scan from the navigation system. * @brief Remove a laser scan from the navigation system.
* @param frame_id The frame ID of the laser scan. * @param laser_scan_name The name of the laser scan.
* @return True if the laser scan was removed successfully. * @return True if the laser scan was removed successfully.
*/ */
virtual bool removeLaserScan(const std::string &frame_id) = 0; virtual bool removeLaserScan(const std::string &laser_scan_name) = 0;
/** /**
* @brief Remove a point cloud from the navigation system. * @brief Remove a point cloud from the navigation system.
* @param frame_id The frame ID of the point cloud. * @param point_cloud_name The name of the point cloud.
* @return True if the point cloud was removed successfully. * @return True if the point cloud was removed successfully.
*/ */
virtual bool removePointCloud(const std::string &frame_id) = 0; virtual bool removePointCloud(const std::string &point_cloud_name) = 0;
/** /**
* @brief Remove a point cloud2 from the navigation system. * @brief Remove a point cloud2 from the navigation system.
* @param frame_id The frame ID of the point cloud2. * @param point_cloud2_name The name of the point cloud2.
* @return True if the point cloud2 was removed successfully. * @return True if the point cloud2 was removed successfully.
*/ */
virtual bool removePointCloud2(const std::string &frame_id) = 0; virtual bool removePointCloud2(const std::string &point_cloud2_name) = 0;
/** /**
* @brief Remove all static maps from the navigation system. * @brief Remove all static maps from the navigation system.
@ -339,6 +340,13 @@ namespace move_base_core
*/ */
virtual bool removeAllData() = 0; virtual bool removeAllData() = 0;
/**
* @brief Add an odometry to the navigation system.
* @param odometry_name The name of the odometry.
* @param odometry The odometry to add.
*/
virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) = 0;
/** /**
* @brief Send a goal for the robot to navigate to. * @brief Send a goal for the robot to navigate to.
* @param goal Target pose in the global frame. * @param goal Target pose in the global frame.
@ -469,10 +477,10 @@ namespace move_base_core
std::shared_ptr<NavFeedback> nav_feedback_; std::shared_ptr<NavFeedback> nav_feedback_;
std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_; std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
std::map<std::string, robot_nav_msgs::OccupancyGrid*> static_maps_; std::map<std::string, robot_nav_msgs::OccupancyGrid> static_maps_;
std::map<std::string, robot_sensor_msgs::LaserScan*> laser_scans_; std::map<std::string, robot_sensor_msgs::LaserScan> laser_scans_;
std::map<std::string, robot_sensor_msgs::PointCloud*> point_clouds_; std::map<std::string, robot_sensor_msgs::PointCloud> point_clouds_;
std::map<std::string, robot_sensor_msgs::PointCloud2*> point_cloud2s_; std::map<std::string, robot_sensor_msgs::PointCloud2> point_cloud2s_;
BaseNavigation() = default; BaseNavigation() = default;
}; };

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

@ -88,115 +88,115 @@ namespace move_base
* @brief Get the robot's footprint (outline shape) in the global frame. * @brief Get the robot's footprint (outline shape) in the global frame.
* @return A vector of points representing the robot's footprint polygon. * @return A vector of points representing the robot's footprint polygon.
*/ */
virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() const override; virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() override;
/** /**
* @brief Add a static map to the navigation system. * @brief Add a static map to the navigation system.
* @param frame_id The frame ID of the map. * @param map_name The name of the map.
* @param map The map to add. * @param map The map to add.
*/ */
virtual void addStaticMap(const std::string &frame_id, robot_nav_msgs::OccupancyGrid *map) override; virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) override;
/**
* @brief Get a static map from the navigation system.
* @param frame_id The frame ID of the map.
* @return The map.
*/
virtual robot_nav_msgs::OccupancyGrid* getStaticMap(const std::string &frame_id) override;
/** /**
* @brief Add a laser scan to the navigation system. * @brief Add a laser scan to the navigation system.
* @param frame_id The frame ID of the laser scan. * @param laser_scan_name The name of the laser scan.
* @param laser_scan The laser scan to add. * @param laser_scan The laser scan to add.
*/ */
virtual void addLaserScan(const std::string &frame_id, robot_sensor_msgs::LaserScan *laser_scan) override; virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) override;
/** /**
* @brief Add a point cloud to the navigation system. * @brief Add a point cloud to the navigation system.
* @param frame_id The frame ID of the point cloud. * @param point_cloud_name The name of the point cloud.
* @param point_cloud The point cloud to add. * @param point_cloud The point cloud to add.
*/ */
virtual void addPointCloud(const std::string &frame_id, robot_sensor_msgs::PointCloud *point_cloud) override; virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) override;
/** /**
* @brief Add a point cloud2 to the navigation system. * @brief Add a point cloud2 to the navigation system.
* @param frame_id The frame ID of the point cloud2. * @param point_cloud2_name The name of the point cloud2.
* @param point_cloud2 The point cloud2 to add. * @param point_cloud2 The point cloud2 to add.
*/ */
virtual void addPointCloud2(const std::string &frame_id, robot_sensor_msgs::PointCloud2 *point_cloud2) override; virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) override;
/**
* @brief Get a static map from the navigation system.
* @param map_name The name of the map.
* @return The map.
*/
virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) override;
/** /**
* @brief Get a laser scan from the navigation system. * @brief Get a laser scan from the navigation system.
* @param frame_id The frame ID of the laser scan. * @param laser_scan_name The name of the laser scan.
* @return The laser scan. * @return The laser scan.
*/ */
virtual robot_sensor_msgs::LaserScan* getLaserScan(const std::string &frame_id) override; virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) override;
/** /**
* @brief Get a point cloud from the navigation system. * @brief Get a point cloud from the navigation system.
* @param frame_id The frame ID of the point cloud. * @param point_cloud_name The name of the point cloud.
* @return The point cloud. * @return The point cloud.
*/ */
virtual robot_sensor_msgs::PointCloud* getPointCloud(const std::string &frame_id) override; virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) override;
/** /**
* @brief Get a point cloud2 from the navigation system. * @brief Get a point cloud2 from the navigation system.
* @param frame_id The frame ID of the point cloud2. * @param point_cloud2_name The name of the point cloud2.
* @return The point cloud2. * @return The point cloud2.
*/ */
virtual robot_sensor_msgs::PointCloud2* getPointCloud2(const std::string &frame_id) override; virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) override;
/** /**
* @brief Get all static maps from the navigation system. * @brief Get all static maps from the navigation system.
* @return The static maps. * @return The static maps.
*/ */
virtual std::map<std::string, robot_nav_msgs::OccupancyGrid*> getAllStaticMaps() override; virtual std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() override;
/** /**
* @brief Get all laser scans from the navigation system. * @brief Get all laser scans from the navigation system.
* @return The laser scans. * @return The laser scans.
*/ */
virtual std::map<std::string, robot_sensor_msgs::LaserScan*> getAllLaserScans() override; virtual std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() override;
/** /**
* @brief Get all point clouds from the navigation system. * @brief Get all point clouds from the navigation system.
* @return The point clouds. * @return The point clouds.
*/ */
virtual std::map<std::string, robot_sensor_msgs::PointCloud*> getAllPointClouds() override; virtual std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() override;
/** /**
* @brief Get all point cloud2s from the navigation system. * @brief Get all point cloud2s from the navigation system.
* @return The point cloud2s. * @return The point cloud2s.
*/ */
virtual std::map<std::string, robot_sensor_msgs::PointCloud2*> getAllPointCloud2s() override; virtual std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() override;
/** /**
* @brief Remove a static map from the navigation system. * @brief Remove a static map from the navigation system.
* @param frame_id The frame ID of the map. * @param map_name The name of the map.
* @return True if the map was removed successfully. * @return True if the map was removed successfully.
*/ */
virtual bool removeStaticMap(const std::string &frame_id) override; virtual bool removeStaticMap(const std::string &map_name) override;
/** /**
* @brief Remove a laser scan from the navigation system. * @brief Remove a laser scan from the navigation system.
* @param frame_id The frame ID of the laser scan. * @param laser_scan_name The name of the laser scan.
* @return True if the laser scan was removed successfully. * @return True if the laser scan was removed successfully.
*/ */
virtual bool removeLaserScan(const std::string &frame_id) override; virtual bool removeLaserScan(const std::string &laser_scan_name) override;
/** /**
* @brief Remove a point cloud from the navigation system. * @brief Remove a point cloud from the navigation system.
* @param frame_id The frame ID of the point cloud. * @param point_cloud_name The name of the point cloud.
* @return True if the point cloud was removed successfully. * @return True if the point cloud was removed successfully.
*/ */
virtual bool removePointCloud(const std::string &frame_id) override; virtual bool removePointCloud(const std::string &point_cloud_name) override;
/** /**
* @brief Remove a point cloud2 from the navigation system. * @brief Remove a point cloud2 from the navigation system.
* @param frame_id The frame ID of the point cloud2. * @param point_cloud2_name The name of the point cloud2.
* @return True if the point cloud2 was removed successfully. * @return True if the point cloud2 was removed successfully.
*/ */
virtual bool removePointCloud2(const std::string &frame_id) override; virtual bool removePointCloud2(const std::string &point_cloud2_name) override;
/** /**
* @brief Remove all static maps from the navigation system. * @brief Remove all static maps from the navigation system.
@ -228,6 +228,12 @@ namespace move_base
*/ */
virtual bool removeAllData() override; virtual bool removeAllData() override;
/**
* @brief Add an odometry to the navigation system.
* @param odometry_name The name of the odometry.
* @param odometry The odometry to add.
*/
virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) override;
/** /**
* @brief Send a goal for the robot to navigate to. * @brief Send a goal for the robot to navigate to.
@ -371,17 +377,19 @@ namespace move_base
/** /**
* @brief Update the global costmap. * @brief Update the global costmap.
* @param frame_id The frame ID of the costmap. * @param value The value to update the costmap.
* @return True if the global costmap was updated successfully, false otherwise. * @param name The name of the costmap.
*/ */
bool updateGlobalCostmap(const std::string &frame_id); template<typename T>
void updateGlobalCostmap(const T& value, const std::string &name);
/** /**
* @brief Update the local costmap. * @brief Update the local costmap.
* @param frame_id The frame ID of the costmap. * @param value The value to update the costmap.
* @return True if the local costmap was updated successfully, false otherwise. * @param name The name of the costmap.
*/ */
bool updateLocalCostmap(const std::string &frame_id); template<typename T>
void updateLocalCostmap(const T& value, const std::string &name);
/** /**
* @brief Performs a control cycle * @brief Performs a control cycle
@ -444,7 +452,7 @@ namespace move_base
*/ */
void resetState(); void resetState();
// void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal); // void goalCB(const robot_geometry_msgs::PoseStamped &goal);
void planThread(); void planThread();

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

@ -24,7 +24,7 @@
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
move_base::MoveBase::MoveBase() move_base::MoveBase::MoveBase()
: initialized_(false), : initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
@ -33,7 +33,7 @@ move_base::MoveBase::MoveBase()
} }
move_base::MoveBase::MoveBase(robot::TFListenerPtr tf) move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
: initialized_(false), : initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
@ -46,6 +46,18 @@ move_base::MoveBase::~MoveBase()
{ {
recovery_behaviors_.clear(); recovery_behaviors_.clear();
// Cleanup static maps (shared_ptr tự động cleanup)
static_maps_.clear();
// Cleanup laser scans (shared_ptr tự động cleanup)
laser_scans_.clear();
// Cleanup point clouds (shared_ptr tự động cleanup)
point_clouds_.clear();
// Cleanup point cloud2s (shared_ptr tự động cleanup)
point_cloud2s_.clear();
if (planner_costmap_robot_ != NULL) if (planner_costmap_robot_ != NULL)
{ {
delete planner_costmap_robot_; delete planner_costmap_robot_;
@ -181,35 +193,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{ {
planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_); planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
planner_costmap_robot_->pause(); planner_costmap_robot_->pause();
robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap(); robot_costmap_2d::LayeredCostmap *layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
// for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
// layer != layered_costmap_->getPlugins()->end();
// ++layer)
// {
// boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
// if (!static_layer)
// continue;
// robot_nav_msgs::OccupancyGrid *occupancy_grid = new robot_nav_msgs::OccupancyGrid();
// occupancy_grid->header.frame_id = "map";
// occupancy_grid->header.stamp = robot::Time::now();
// occupancy_grid->info.resolution = 0.05;
// occupancy_grid->info.width = 100;
// occupancy_grid->info.height = 100;
// occupancy_grid->info.origin.position.x = 0.0;
// occupancy_grid->info.origin.position.y = 0.0;
// occupancy_grid->info.origin.position.z = 0.0;
// occupancy_grid->info.origin.orientation.x = 0.0;
// occupancy_grid->info.origin.orientation.y = 0.0;
// occupancy_grid->info.origin.orientation.z = 0.0;
// occupancy_grid->info.origin.orientation.w = 1.0;
// occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height);
// for (int i = 0; i < occupancy_grid->data.size(); i++)
// {
// occupancy_grid->data[i] = robot_costmap_2d::NO_INFORMATION;
// }
// if (occupancy_grid)
// static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
// }
} }
catch (const std::exception &ex) catch (const std::exception &ex)
{ {
@ -230,7 +214,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
robot::log_error("[%s:%d]\n 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); throw std::runtime_error("Failed to load global planner " + global_planner);
} }
if(planner_->initialize(global_planner, planner_costmap_robot_)) if (planner_->initialize(global_planner, planner_costmap_robot_))
robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__); robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__);
else else
robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__); robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__);
@ -245,7 +229,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{ {
controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_); controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
controller_costmap_robot_->pause(); controller_costmap_robot_->pause();
robot_costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap(); robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
// for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin(); // for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
// layer != layered_costmap_->getPlugins()->end(); // layer != layered_costmap_->getPlugins()->end();
// ++layer) // ++layer)
@ -368,88 +352,138 @@ void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msg
controller_costmap_robot_->setUnpaddedRobotFootprint(fprt); controller_costmap_robot_->setUnpaddedRobotFootprint(fprt);
} }
std::vector<robot_geometry_msgs::Point> move_base::MoveBase::getRobotFootprint() const std::vector<robot_geometry_msgs::Point> move_base::MoveBase::getRobotFootprint()
{ {
if (planner_costmap_robot_) if (planner_costmap_robot_)
return planner_costmap_robot_->getUnpaddedRobotFootprint(); return planner_costmap_robot_->getUnpaddedRobotFootprint();
return std::vector<robot_geometry_msgs::Point>(); return std::vector<robot_geometry_msgs::Point>();
} }
void move_base::MoveBase::addStaticMap(const std::string &frame_id, robot_nav_msgs::OccupancyGrid *map) void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map)
{ {
static_maps_[frame_id] = map; // Kiểm tra xem map_name đã tồn tại trong static_maps_ chưa
auto it = static_maps_.find(map_name);
if (it == static_maps_.end())
{
static_maps_[map_name] = map;
}
else
{
it->second = map;
}
updateGlobalCostmap<robot_nav_msgs::OccupancyGrid>(map, map_name);
updateLocalCostmap<robot_nav_msgs::OccupancyGrid>(map, map_name);
} }
robot_nav_msgs::OccupancyGrid* move_base::MoveBase::getStaticMap(const std::string &frame_id) robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::string &map_name)
{ {
auto it = static_maps_.find(frame_id); auto it = static_maps_.find(map_name);
if (it != static_maps_.end()) if (it != static_maps_.end())
{
return it->second; return it->second;
return nullptr; }
return robot_nav_msgs::OccupancyGrid();
} }
void move_base::MoveBase::addLaserScan(const std::string &frame_id, robot_sensor_msgs::LaserScan *laser_scan) void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
{ {
laser_scans_[frame_id] = laser_scan;
auto it = laser_scans_.find(laser_scan_name);
if (it == laser_scans_.end())
{
laser_scans_[laser_scan_name] = laser_scan;
}
else
{
it->second = laser_scan;
}
updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, laser_scan_name);
updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, laser_scan_name);
} }
robot_sensor_msgs::LaserScan* move_base::MoveBase::getLaserScan(const std::string &frame_id) robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)
{ {
auto it = laser_scans_.find(frame_id); auto it = laser_scans_.find(laser_scan_name);
if (it != laser_scans_.end()) if (it != laser_scans_.end())
{
return it->second; return it->second;
return nullptr; }
return robot_sensor_msgs::LaserScan();
} }
void move_base::MoveBase::addPointCloud(const std::string &frame_id, robot_sensor_msgs::PointCloud *point_cloud) void move_base::MoveBase::addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud)
{ {
point_clouds_[frame_id] = point_cloud; auto it = point_clouds_.find(point_cloud_name);
if (it == point_clouds_.end())
{
point_clouds_[point_cloud_name] = point_cloud;
}
else
{
it->second = point_cloud;
}
updateLocalCostmap<robot_sensor_msgs::PointCloud>(point_cloud, point_cloud_name);
updateGlobalCostmap<robot_sensor_msgs::PointCloud>(point_cloud, point_cloud_name);
} }
robot_sensor_msgs::PointCloud* move_base::MoveBase::getPointCloud(const std::string &frame_id) robot_sensor_msgs::PointCloud move_base::MoveBase::getPointCloud(const std::string &point_cloud_name)
{ {
auto it = point_clouds_.find(frame_id); auto it = point_clouds_.find(point_cloud_name);
if (it != point_clouds_.end()) if (it != point_clouds_.end())
{
return it->second; return it->second;
return nullptr; }
return robot_sensor_msgs::PointCloud();
} }
void move_base::MoveBase::addPointCloud2(const std::string &frame_id, robot_sensor_msgs::PointCloud2 *point_cloud2) void move_base::MoveBase::addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2)
{ {
point_cloud2s_[frame_id] = point_cloud2; auto it = point_cloud2s_.find(point_cloud2_name);
if (it == point_cloud2s_.end())
{
point_cloud2s_[point_cloud2_name] = point_cloud2;
}
else
{
it->second = point_cloud2;
}
updateLocalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, point_cloud2_name);
updateGlobalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, point_cloud2_name);
} }
robot_sensor_msgs::PointCloud2* move_base::MoveBase::getPointCloud2(const std::string &frame_id) robot_sensor_msgs::PointCloud2 move_base::MoveBase::getPointCloud2(const std::string &point_cloud2_name)
{ {
auto it = point_cloud2s_.find(frame_id); auto it = point_cloud2s_.find(point_cloud2_name);
if (it != point_cloud2s_.end()) if (it != point_cloud2s_.end())
{
return it->second; return it->second;
return nullptr; }
return robot_sensor_msgs::PointCloud2();
} }
std::map<std::string, robot_nav_msgs::OccupancyGrid*> move_base::MoveBase::getAllStaticMaps() std::map<std::string, robot_nav_msgs::OccupancyGrid> move_base::MoveBase::getAllStaticMaps()
{ {
return static_maps_; return static_maps_;
} }
std::map<std::string, robot_sensor_msgs::LaserScan*> move_base::MoveBase::getAllLaserScans() std::map<std::string, robot_sensor_msgs::LaserScan> move_base::MoveBase::getAllLaserScans()
{ {
return laser_scans_; return laser_scans_;
} }
std::map<std::string, robot_sensor_msgs::PointCloud*> move_base::MoveBase::getAllPointClouds() std::map<std::string, robot_sensor_msgs::PointCloud> move_base::MoveBase::getAllPointClouds()
{ {
return point_clouds_; return point_clouds_;
} }
std::map<std::string, robot_sensor_msgs::PointCloud2*> move_base::MoveBase::getAllPointCloud2s() std::map<std::string, robot_sensor_msgs::PointCloud2> move_base::MoveBase::getAllPointCloud2s()
{ {
return point_cloud2s_; return point_cloud2s_;
} }
bool move_base::MoveBase::removeStaticMap(const std::string &frame_id) bool move_base::MoveBase::removeStaticMap(const std::string &map_name)
{ {
auto it = static_maps_.find(frame_id); auto it = static_maps_.find(map_name);
if (it != static_maps_.end()) if (it != static_maps_.end())
{ {
static_maps_.erase(it); static_maps_.erase(it);
@ -458,9 +492,9 @@ bool move_base::MoveBase::removeStaticMap(const std::string &frame_id)
return false; return false;
} }
bool move_base::MoveBase::removeLaserScan(const std::string &frame_id) bool move_base::MoveBase::removeLaserScan(const std::string &laser_scan_name)
{ {
auto it = laser_scans_.find(frame_id); auto it = laser_scans_.find(laser_scan_name);
if (it != laser_scans_.end()) if (it != laser_scans_.end())
{ {
laser_scans_.erase(it); laser_scans_.erase(it);
@ -469,9 +503,9 @@ bool move_base::MoveBase::removeLaserScan(const std::string &frame_id)
return false; return false;
} }
bool move_base::MoveBase::removePointCloud(const std::string &frame_id) bool move_base::MoveBase::removePointCloud(const std::string &point_cloud_name)
{ {
auto it = point_clouds_.find(frame_id); auto it = point_clouds_.find(point_cloud_name);
if (it != point_clouds_.end()) if (it != point_clouds_.end())
{ {
point_clouds_.erase(it); point_clouds_.erase(it);
@ -480,9 +514,9 @@ bool move_base::MoveBase::removePointCloud(const std::string &frame_id)
return false; return false;
} }
bool move_base::MoveBase::removePointCloud2(const std::string &frame_id) bool move_base::MoveBase::removePointCloud2(const std::string &point_cloud2_name)
{ {
auto it = point_cloud2s_.find(frame_id); auto it = point_cloud2s_.find(point_cloud2_name);
if (it != point_cloud2s_.end()) if (it != point_cloud2s_.end())
{ {
point_cloud2s_.erase(it); point_cloud2s_.erase(it);
@ -524,55 +558,77 @@ bool move_base::MoveBase::removeAllData()
return true; return true;
} }
bool move_base::MoveBase::updateGlobalCostmap(const std::string &frame_id) template<typename T>
void move_base::MoveBase::updateGlobalCostmap(const T& value, const std::string &name)
{ {
if (!planner_costmap_robot_) if (!planner_costmap_robot_)
return false; return;
// Tìm trong static_maps_ phần tử có header.frame_id == frame_id // Tìm map theo map_name (key trong static_maps_)
robot_nav_msgs::OccupancyGrid* found_map = nullptr; auto it = static_maps_.find(name);
for (auto it = static_maps_.begin(); it != static_maps_.end(); ++it) if (it == static_maps_.end())
{ return;
if (it->second && it->second->header.frame_id == frame_id)
{
found_map = it->second;
break;
}
}
// Nếu không tìm thấy, return false robot_nav_msgs::OccupancyGrid *found_map = &it->second;
if (!found_map) std::string frame_id = found_map->header.frame_id;
return false;
// Tìm StaticLayer và update với map đã tìm thấy // Tìm StaticLayer và update với map đã tìm thấy
try try
{ {
planner_costmap_robot_->pause(); for (const auto &layer : *planner_costmap_robot_->getLayeredCostmap()->getPlugins())
robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
layer != layered_costmap_->getPlugins()->end();
++layer)
{ {
boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer); if (layer->getType() != robot_costmap_2d::LayerType::STATIC_LAYER && layer->getName() != name)
if (!static_layer)
continue; continue;
// Update costmap với map đã tìm thấy // Update costmap với map đã tìm thấy
static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*found_map, frame_id); layer->dataCallBack<T>(value, frame_id);
} }
planner_costmap_robot_->resume(); return;
return true;
} }
catch (const std::exception &ex) catch (const std::exception &ex)
{ {
robot::log_error("[%s:%d]\n EXCEPTION in updateGlobalCostmap: %s", __FILE__, __LINE__, ex.what()); robot::log_error("[%s:%d]\n EXCEPTION in Update GlobalCostmap: %s", __FILE__, __LINE__, ex.what());
return false; return;
} }
} }
bool move_base::MoveBase::updateLocalCostmap(const std::string &frame_id) template<typename T>
void move_base::MoveBase::updateLocalCostmap(const T& value, const std::string &name)
{ {
return false; if (!controller_costmap_robot_)
return;
// Tìm map theo map_name (key trong static_maps_)
auto it = static_maps_.find(name);
if (it == static_maps_.end())
return;
robot_nav_msgs::OccupancyGrid *found_map = &it->second;
std::string frame_id = found_map->header.frame_id;
// Tìm StaticLayer và update với map đã tìm thấy
try
{
for (const auto &layer : *planner_costmap_robot_->getLayeredCostmap()->getPlugins())
{
if (layer->getType() != robot_costmap_2d::LayerType::STATIC_LAYER && layer->getName() != name)
continue;
// Update costmap với map đã tìm thấy
layer->dataCallBack<T>(value, frame_id);
}
return;
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d]\n EXCEPTION in Update LocalCostmap: %s", __FILE__, __LINE__, ex.what());
return;
}
}
void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry)
{
robot::log_info("[%s:%d] addOdometry called for: %s", __FILE__, __LINE__, odometry_name.c_str());
} }
bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
@ -642,7 +698,6 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
return false; return false;
} }
bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal, bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance) double xy_goal_tolerance, double yaw_goal_tolerance)
{ {
@ -1145,7 +1200,7 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors()
// first, we'll load a recovery behavior to clear the costmap (conservative reset) // first, we'll load a recovery behavior to clear the costmap (conservative reset)
try try
{ {
std::string path_file_so ; std::string path_file_so;
auto clear_costmap_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>( auto clear_costmap_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations); path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
std::shared_ptr<robot_nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader()); std::shared_ptr<robot_nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader());
@ -1465,7 +1520,8 @@ bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_
current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance()) current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
{ {
std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". " std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". "
"Current time: " << current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl; "Current time: "
<< current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl;
return false; return false;
} }
@ -1509,7 +1565,8 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro
bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist)
{ {
if (!twist_) { if (!twist_)
{
return false; return false;
} }
twist = *twist_; twist = *twist_;
@ -1518,7 +1575,8 @@ bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist)
robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback() robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback()
{ {
if (!nav_feedback_) { if (!nav_feedback_)
{
return nullptr; return nullptr;
} }
return nav_feedback_.get(); return nav_feedback_.get();