update from HiepLM

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit f052dac1421e7e2bedba4088780526a8775b0c29
+Subproject commit 384897b7504103468a5c0501157485c12d8289d8

src/Navigations/Cores/move_base_core/CMakeLists.txt

@@ -31,6 +31,8 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_geometry_msgs
     robot_protocol_msgs
     robot_nav_2d_msgs
+    robot_nav_msgs
+    robot_sensor_msgs
   )
 
 else()

src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h

@@ -3,20 +3,23 @@
 
 #include <memory>
 
-// geometry msgs
+// msgs
 #include <robot_geometry_msgs/Twist.h>
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_geometry_msgs/Pose2D.h>
 #include <robot_geometry_msgs/Vector3.h>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
-
+#include <robot_nav_msgs/OccupancyGrid.h>
-// robot protocol msgs
+#include <robot_sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_protocol_msgs/Order.h>
 
 // tf
 #include <tf3/buffer_core.h>
 #include <tf3/utils.h>
 #include <tf3/convert.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 #include <robot/time.h>
 #include <move_base_core/common.h>
 
@@ -114,14 +117,14 @@ namespace move_base_core
 	inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
 	{
 		robot_geometry_msgs::PoseStamped goal;
-		// goal.header.frame_id = frame_id;
+		goal.header.frame_id = frame_id;
-		// goal.header.stamp = robot::Time::now();
+		goal.header.stamp = robot::Time::now();
-		// goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
+		goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
-		// goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
+		goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
 
-		// tf3::Quaternion q;
+		tf3::Quaternion q;
-		// q.setRPY(0, 0, pose.theta);
+		q.setRPY(0, 0, pose.theta);
-		// goal.pose.orientation = tf3::convert(q);
+		goal.pose.orientation = tf3::toMsg(q);
 		return goal;
 	}
 
@@ -192,6 +195,150 @@ namespace move_base_core
 		 */
 		virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) = 0;
 
+		/**
+		 * @brief Get the robot's footprint (outline shape) in the global frame.
+		 * @return A vector of points representing the robot's footprint polygon.
+		 */
+		virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() const = 0;
+
+		/**
+		 * @brief Add a static map to the navigation system.
+		 * @param frame_id The frame ID of the map.
+		 * @param map The map to add.
+		 */
+		 virtual void addStaticMap(const std::string &frame_id, robot_nav_msgs::OccupancyGrid *map) = 0;
+
+		 /**
+			* @brief Add a laser scan to the navigation system.
+			* @param frame_id The frame ID of the laser scan.
+			* @param laser_scan The laser scan to add.
+			*/
+		 virtual void addLaserScan(const std::string &frame_id, robot_sensor_msgs::LaserScan *laser_scan) = 0;
+ 
+		 /**
+			* @brief Add a point cloud to the navigation system.
+			* @param frame_id The frame ID of the point cloud.
+			* @param point_cloud The point cloud to add.
+			*/
+		 virtual void addPointCloud(const std::string &frame_id, robot_sensor_msgs::PointCloud *point_cloud) = 0;
+ 
+		 /**
+			* @brief Add a point cloud2 to the navigation system.
+			* @param frame_id The frame ID of the point cloud2.
+			* @param point_cloud2 The point cloud2 to add.
+			*/
+		 virtual void addPointCloud2(const std::string &frame_id, robot_sensor_msgs::PointCloud2 *point_cloud2) = 0;
+ 
+		 /**
+			* @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) = 0;
+ 
+		 /**
+			* @brief Get a laser scan from the navigation system.
+			* @param frame_id The frame ID of the laser scan.
+			* @return The laser scan.
+			*/
+		 virtual robot_sensor_msgs::LaserScan* getLaserScan(const std::string &frame_id) = 0;
+ 
+		 /**
+			* @brief Get a point cloud from the navigation system.
+			* @param frame_id The frame ID of the point cloud.
+			* @return The point cloud.
+			*/
+		 virtual robot_sensor_msgs::PointCloud* getPointCloud(const std::string &frame_id) = 0;
+ 
+		 /**
+			* @brief Get a point cloud2 from the navigation system.
+			* @param frame_id The frame ID of the point cloud2.
+			* @return The point cloud2.
+			*/
+		 virtual robot_sensor_msgs::PointCloud2* getPointCloud2(const std::string &frame_id) = 0;
+ 
+		 /**
+			* @brief Get all static maps from the navigation system.
+			* @return The static maps.
+			*/
+		 virtual std::map<std::string, robot_nav_msgs::OccupancyGrid*> getAllStaticMaps() = 0;
+ 
+		 /**
+			* @brief Get all laser scans from the navigation system.
+			* @return The laser scans.
+			*/
+		 virtual std::map<std::string, robot_sensor_msgs::LaserScan*> getAllLaserScans() = 0;
+ 
+		 /**
+			* @brief Get all point clouds from the navigation system.
+			* @return The point clouds.
+			*/
+		 virtual std::map<std::string, robot_sensor_msgs::PointCloud*> getAllPointClouds() = 0;
+ 
+		 /**
+			* @brief Get all point cloud2s from the navigation system.
+			* @return The point cloud2s.
+			*/
+		 virtual std::map<std::string, robot_sensor_msgs::PointCloud2*> getAllPointCloud2s() = 0;
+ 
+		 /**
+			* @brief Remove a static map from the navigation system.
+			* @param frame_id The frame ID of the map.
+			* @return True if the map was removed successfully.
+			*/
+		 virtual bool removeStaticMap(const std::string &frame_id) = 0;
+ 
+		 /**
+			* @brief Remove a laser scan from the navigation system.
+			* @param frame_id The frame ID of the laser scan.
+			* @return True if the laser scan was removed successfully.
+			*/
+		 virtual bool removeLaserScan(const std::string &frame_id) = 0;
+ 
+		 /**
+			* @brief Remove a point cloud from the navigation system.
+			* @param frame_id The frame ID of the point cloud.
+			* @return True if the point cloud was removed successfully.
+			*/
+		 virtual bool removePointCloud(const std::string &frame_id) = 0;
+ 
+		 /**
+			* @brief Remove a point cloud2 from the navigation system.
+			* @param frame_id The frame ID of the point cloud2.
+			* @return True if the point cloud2 was removed successfully.
+			*/
+		 virtual bool removePointCloud2(const std::string &frame_id) = 0;
+ 
+		 /**
+			* @brief Remove all static maps from the navigation system.
+			* @return True if the static maps were removed successfully.
+			*/
+		 virtual bool removeAllStaticMaps() = 0;
+ 
+		 /**
+			* @brief Remove all laser scans from the navigation system.
+			* @return True if the laser scans were removed successfully.
+			*/
+		 virtual bool removeAllLaserScans() = 0;
+ 
+		 /**
+			* @brief Remove all point clouds from the navigation system.
+			* @return True if the point clouds were removed successfully.
+			*/
+		 virtual bool removeAllPointClouds() = 0;
+ 
+		 /**
+			* @brief Remove all point cloud2s from the navigation system.
+			* @return True if the point cloud2s were removed successfully.
+			*/
+		 virtual bool removeAllPointCloud2s() = 0;
+ 
+		 /**
+			* @brief Remove all data from the navigation system.
+			* @return True if the data was removed successfully.
+			*/
+		 virtual bool removeAllData() = 0;
+
 		/**
 		 * @brief Send a goal for the robot to navigate to.
 		 * @param goal              Target pose in the global frame.
@@ -316,10 +463,16 @@ namespace move_base_core
 		 */
 		virtual NavFeedback *getFeedback() = 0;
 		
+
 	protected:
 		// Shared feedback data for navigation status tracking
 		std::shared_ptr<NavFeedback> nav_feedback_;
 		std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
+
+		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::PointCloud*> point_clouds_;
+		std::map<std::string, robot_sensor_msgs::PointCloud2*> point_cloud2s_;
 		BaseNavigation() = default;
 	};
 

src/Navigations/Cores/move_base_core/package.xml

@@ -24,11 +24,14 @@
   <build_depend>robot_geometry_msgs</build_depend>
   <build_depend>robot_protocol_msgs</build_depend>
   <build_depend>robot_nav_2d_msgs</build_depend>
+  <build_depend>robot_nav_msgs</build_depend>
+  <build_depend>robot_sensor_msgs</build_depend>
 
   <run_depend>tf3</run_depend>
   <run_depend>robot_time</run_depend>
   <run_depend>robot_geometry_msgs</run_depend>
   <run_depend>robot_protocol_msgs</run_depend>
   <run_depend>robot_nav_2d_msgs</run_depend>
-
+  <run_depend>robot_nav_msgs</run_depend>
+  <run_depend>robot_sensor_msgs</run_depend>
 </package>

src/Navigations/Packages/move_base/include/move_base/move_base.h

@@ -84,6 +84,151 @@ namespace move_base
      */
     virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) override;
 
+    /**
+     * @brief Get the robot's footprint (outline shape) in the global frame.
+     * @return A vector of points representing the robot's footprint polygon.
+     */
+    virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() const override;
+
+    /**
+		 * @brief Add a static map to the navigation system.
+		 * @param frame_id The frame ID of the map.
+		 * @param map The map to add.
+		 */
+     virtual void addStaticMap(const std::string &frame_id, 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.
+      * @param frame_id The frame ID of the laser scan.
+      * @param laser_scan The laser scan to add.
+      */
+     virtual void addLaserScan(const std::string &frame_id, robot_sensor_msgs::LaserScan *laser_scan) override;
+ 
+     /**
+      * @brief Add a point cloud to the navigation system.
+      * @param frame_id The frame ID of the point cloud.
+      * @param point_cloud The point cloud to add.
+      */
+     virtual void addPointCloud(const std::string &frame_id, robot_sensor_msgs::PointCloud *point_cloud) override;
+ 
+     /**
+      * @brief Add a point cloud2 to the navigation system.
+      * @param frame_id The frame ID of the point cloud2.
+      * @param point_cloud2 The point cloud2 to add.
+      */
+     virtual void addPointCloud2(const std::string &frame_id, robot_sensor_msgs::PointCloud2 *point_cloud2) override;
+ 
+     /**
+      * @brief Get a laser scan from the navigation system.
+      * @param frame_id The frame ID of the laser scan.
+      * @return The laser scan.
+      */
+     virtual robot_sensor_msgs::LaserScan* getLaserScan(const std::string &frame_id) override;
+ 
+     /**
+      * @brief Get a point cloud from the navigation system.
+      * @param frame_id The frame ID of the point cloud.
+      * @return The point cloud.
+      */
+     virtual robot_sensor_msgs::PointCloud* getPointCloud(const std::string &frame_id) override;
+ 
+     /**
+      * @brief Get a point cloud2 from the navigation system.
+      * @param frame_id The frame ID of the point cloud2.
+      * @return The point cloud2.
+      */
+     virtual robot_sensor_msgs::PointCloud2* getPointCloud2(const std::string &frame_id) override;
+ 
+     /**
+      * @brief Get all static maps from the navigation system.
+      * @return The static maps.
+      */
+     virtual std::map<std::string, robot_nav_msgs::OccupancyGrid*> getAllStaticMaps() override;
+ 
+     /**
+      * @brief Get all laser scans from the navigation system.
+      * @return The laser scans.
+      */
+     virtual std::map<std::string, robot_sensor_msgs::LaserScan*> getAllLaserScans() override;
+ 
+     /**
+      * @brief Get all point clouds from the navigation system.
+      * @return The point clouds.
+      */
+     virtual std::map<std::string, robot_sensor_msgs::PointCloud*> getAllPointClouds() override;
+ 
+     /**
+      * @brief Get all point cloud2s from the navigation system.
+      * @return The point cloud2s.
+      */
+     virtual std::map<std::string, robot_sensor_msgs::PointCloud2*> getAllPointCloud2s() override;
+ 
+     /**
+      * @brief Remove a static map from the navigation system.
+      * @param frame_id The frame ID of the map.
+      * @return True if the map was removed successfully.
+      */
+     virtual bool removeStaticMap(const std::string &frame_id) override;
+ 
+     /**
+      * @brief Remove a laser scan from the navigation system.
+      * @param frame_id The frame ID of the laser scan.
+      * @return True if the laser scan was removed successfully.
+      */
+     virtual bool removeLaserScan(const std::string &frame_id) override;
+ 
+     /**
+      * @brief Remove a point cloud from the navigation system.
+      * @param frame_id The frame ID of the point cloud.
+      * @return True if the point cloud was removed successfully.
+      */
+     virtual bool removePointCloud(const std::string &frame_id) override;
+ 
+     /**
+      * @brief Remove a point cloud2 from the navigation system.
+      * @param frame_id The frame ID of the point cloud2.
+      * @return True if the point cloud2 was removed successfully.
+      */
+     virtual bool removePointCloud2(const std::string &frame_id) override;
+ 
+     /**
+      * @brief Remove all static maps from the navigation system.
+      * @return True if the static maps were removed successfully.
+      */
+     virtual bool removeAllStaticMaps() override;
+ 
+     /**
+      * @brief Remove all laser scans from the navigation system.
+      * @return True if the laser scans were removed successfully.
+      */
+     virtual bool removeAllLaserScans() override;
+ 
+     /**
+      * @brief Remove all point clouds from the navigation system.
+      * @return True if the point clouds were removed successfully.
+      */
+     virtual bool removeAllPointClouds() override;
+ 
+     /**
+      * @brief Remove all point cloud2s from the navigation system.
+      * @return True if the point cloud2s were removed successfully.
+      */
+     virtual bool removeAllPointCloud2s() override;
+ 
+     /**
+      * @brief Remove all data from the navigation system.
+      * @return True if the data was removed successfully.
+      */
+     virtual bool removeAllData() override;
+
+
     /**
      * @brief Send a goal for the robot to navigate to.
      * @param goal              Target pose in the global frame.
@@ -210,6 +355,7 @@ namespace move_base
      */
     virtual robot::move_base_core::NavFeedback *getFeedback() override;
 
+
     /**
      * @brief  Destructor - Cleans up
      */
@@ -223,6 +369,20 @@ namespace move_base
 
   private:
 
+    /**
+    * @brief Update the global costmap.
+    * @param frame_id The frame ID of the costmap.
+    * @return True if the global costmap was updated successfully, false otherwise.
+    */
+    bool updateGlobalCostmap(const std::string &frame_id);
+    
+    /**
+     * @brief Update the local costmap.
+     * @param frame_id The frame ID of the costmap.
+     * @return True if the local costmap was updated successfully, false otherwise.
+     */
+    bool updateLocalCostmap(const std::string &frame_id);
+
     /**
      * @brief  Performs a control cycle
      * @param goal A reference to the goal to pursue

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -20,6 +20,8 @@
 #include <robot_costmap_2d/voxel_layer.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud2.h>
 
 move_base::MoveBase::MoveBase()
 : initialized_(false),
@@ -180,34 +182,34 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
       planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
       planner_costmap_robot_->pause();
       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();
+      // 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)
-      {
+      // {
-        boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
+      //   boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
-        if (!static_layer)
+      //   if (!static_layer)
-          continue;
+      //     continue;
-        robot_nav_msgs::OccupancyGrid *occupancy_grid = new robot_nav_msgs::OccupancyGrid();
+      //   robot_nav_msgs::OccupancyGrid *occupancy_grid = new robot_nav_msgs::OccupancyGrid();
-        occupancy_grid->header.frame_id = "map";
+      //   occupancy_grid->header.frame_id = "map";
-        occupancy_grid->header.stamp = robot::Time::now();
+      //   occupancy_grid->header.stamp = robot::Time::now();
-        occupancy_grid->info.resolution = 0.05;
+      //   occupancy_grid->info.resolution = 0.05;
-        occupancy_grid->info.width = 100;
+      //   occupancy_grid->info.width = 100;
-        occupancy_grid->info.height = 100;
+      //   occupancy_grid->info.height = 100;
-        occupancy_grid->info.origin.position.x = 0.0;
+      //   occupancy_grid->info.origin.position.x = 0.0;
-        occupancy_grid->info.origin.position.y = 0.0;
+      //   occupancy_grid->info.origin.position.y = 0.0;
-        occupancy_grid->info.origin.position.z = 0.0;
+      //   occupancy_grid->info.origin.position.z = 0.0;
-        occupancy_grid->info.origin.orientation.x = 0.0;
+      //   occupancy_grid->info.origin.orientation.x = 0.0;
-        occupancy_grid->info.origin.orientation.y = 0.0;
+      //   occupancy_grid->info.origin.orientation.y = 0.0;
-        occupancy_grid->info.origin.orientation.z = 0.0;
+      //   occupancy_grid->info.origin.orientation.z = 0.0;
-        occupancy_grid->info.origin.orientation.w = 1.0;
+      //   occupancy_grid->info.origin.orientation.w = 1.0;
-        occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height);
+      //   occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height);
-        for (int i = 0; i < occupancy_grid->data.size(); i++)
+      //   for (int i = 0; i < occupancy_grid->data.size(); i++)
-        {
+      //   {
-          occupancy_grid->data[i] = robot_costmap_2d::NO_INFORMATION;
+      //     occupancy_grid->data[i] = robot_costmap_2d::NO_INFORMATION;
-        }
+      //   }
-        if (occupancy_grid)
+      //   if (occupancy_grid)
-          static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
+      //     static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
-      }
+      // }
     }
     catch (const std::exception &ex)
     {
@@ -244,31 +246,31 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
       controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
       controller_costmap_robot_->pause();
       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)
-      {
+      // {
-        boost::shared_ptr<robot_costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<robot_costmap_2d::VoxelLayer>(*layer);
+      //   boost::shared_ptr<robot_costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<robot_costmap_2d::VoxelLayer>(*layer);
-        if (!obstacle_layer)
+      //   if (!obstacle_layer)
-          continue;
+      //     continue;
-        robot_sensor_msgs::LaserScan *laser_scan = new robot_sensor_msgs::LaserScan();
+      //   robot_sensor_msgs::LaserScan *laser_scan = new robot_sensor_msgs::LaserScan();
-        laser_scan->header.frame_id = "laser";
+      //   laser_scan->header.frame_id = "laser";
-        laser_scan->header.stamp = robot::Time::now();
+      //   laser_scan->header.stamp = robot::Time::now();
-        laser_scan->angle_min = -M_PI;  
+      //   laser_scan->angle_min = -M_PI;  
-        laser_scan->angle_max = M_PI;
+      //   laser_scan->angle_max = M_PI;
-        laser_scan->angle_increment = M_PI / 180;
+      //   laser_scan->angle_increment = M_PI / 180;
-        laser_scan->time_increment = 0.0;
+      //   laser_scan->time_increment = 0.0;
-        laser_scan->scan_time = 0.0;
+      //   laser_scan->scan_time = 0.0;
-        laser_scan->range_min = 0.0;
+      //   laser_scan->range_min = 0.0;
-        laser_scan->range_max = 10.0;
+      //   laser_scan->range_max = 10.0;
-        laser_scan->ranges.resize(180);
+      //   laser_scan->ranges.resize(180);
-        for (int i = 0; i < 180; i++)
+      //   for (int i = 0; i < 180; i++)
-        {
+      //   {
-          laser_scan->ranges[i] = 10.0;
+      //     laser_scan->ranges[i] = 10.0;
-        }
+      //   }
-        if (laser_scan)
+      //   if (laser_scan)
-          obstacle_layer->dataCallBack<robot_sensor_msgs::LaserScan>(*laser_scan, "laser");
+      //     obstacle_layer->dataCallBack<robot_sensor_msgs::LaserScan>(*laser_scan, "laser");
-      }
+      // }
     }
     catch (const std::exception &ex)
     {
@@ -366,6 +368,213 @@ void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msg
     controller_costmap_robot_->setUnpaddedRobotFootprint(fprt);
 }
 
+std::vector<robot_geometry_msgs::Point> move_base::MoveBase::getRobotFootprint() const
+{
+  if (planner_costmap_robot_)
+    return planner_costmap_robot_->getUnpaddedRobotFootprint();
+  return std::vector<robot_geometry_msgs::Point>();
+}
+
+void move_base::MoveBase::addStaticMap(const std::string &frame_id, robot_nav_msgs::OccupancyGrid *map)
+{
+  static_maps_[frame_id] = map;
+}
+
+robot_nav_msgs::OccupancyGrid* move_base::MoveBase::getStaticMap(const std::string &frame_id)
+{
+  auto it = static_maps_.find(frame_id);
+  if (it != static_maps_.end())
+    return it->second;
+  return nullptr;
+}
+
+void move_base::MoveBase::addLaserScan(const std::string &frame_id, robot_sensor_msgs::LaserScan *laser_scan)
+{
+  laser_scans_[frame_id] = laser_scan;
+}
+
+robot_sensor_msgs::LaserScan* move_base::MoveBase::getLaserScan(const std::string &frame_id)
+{
+  auto it = laser_scans_.find(frame_id);
+  if (it != laser_scans_.end())
+    return it->second;
+  return nullptr;
+}
+
+void move_base::MoveBase::addPointCloud(const std::string &frame_id, robot_sensor_msgs::PointCloud *point_cloud)
+{
+  point_clouds_[frame_id] = point_cloud;
+}
+
+robot_sensor_msgs::PointCloud* move_base::MoveBase::getPointCloud(const std::string &frame_id)
+{
+  auto it = point_clouds_.find(frame_id);
+  if (it != point_clouds_.end())
+    return it->second;
+  return nullptr;
+}
+
+void move_base::MoveBase::addPointCloud2(const std::string &frame_id, robot_sensor_msgs::PointCloud2 *point_cloud2)
+{
+  point_cloud2s_[frame_id] = point_cloud2;
+}
+
+robot_sensor_msgs::PointCloud2* move_base::MoveBase::getPointCloud2(const std::string &frame_id)
+{
+  auto it = point_cloud2s_.find(frame_id);
+  if (it != point_cloud2s_.end())
+    return it->second;
+  return nullptr;
+}
+
+std::map<std::string, robot_nav_msgs::OccupancyGrid*> move_base::MoveBase::getAllStaticMaps()
+{
+  return static_maps_;
+}
+
+std::map<std::string, robot_sensor_msgs::LaserScan*> move_base::MoveBase::getAllLaserScans()
+{
+  return laser_scans_;
+}
+
+std::map<std::string, robot_sensor_msgs::PointCloud*> move_base::MoveBase::getAllPointClouds()
+{
+  return point_clouds_;
+}
+
+std::map<std::string, robot_sensor_msgs::PointCloud2*> move_base::MoveBase::getAllPointCloud2s()
+{
+  return point_cloud2s_;
+}
+
+bool move_base::MoveBase::removeStaticMap(const std::string &frame_id)
+{
+  auto it = static_maps_.find(frame_id);
+  if (it != static_maps_.end())
+  {
+    static_maps_.erase(it);
+    return true;
+  }
+  return false;
+}
+
+bool move_base::MoveBase::removeLaserScan(const std::string &frame_id)
+{
+  auto it = laser_scans_.find(frame_id);
+  if (it != laser_scans_.end())
+  {
+    laser_scans_.erase(it);
+    return true;
+  }
+  return false;
+}
+
+bool move_base::MoveBase::removePointCloud(const std::string &frame_id)
+{
+  auto it = point_clouds_.find(frame_id);
+  if (it != point_clouds_.end())
+  {
+    point_clouds_.erase(it);
+    return true;
+  }
+  return false;
+}
+
+bool move_base::MoveBase::removePointCloud2(const std::string &frame_id)
+{
+  auto it = point_cloud2s_.find(frame_id);
+  if (it != point_cloud2s_.end())
+  {
+    point_cloud2s_.erase(it);
+    return true;
+  }
+  return false;
+}
+
+bool move_base::MoveBase::removeAllStaticMaps()
+{
+  static_maps_.clear();
+  return true;
+}
+
+bool move_base::MoveBase::removeAllLaserScans()
+{
+  laser_scans_.clear();
+  return true;
+}
+
+bool move_base::MoveBase::removeAllPointClouds()
+{
+  point_clouds_.clear();
+  return true;
+}
+
+bool move_base::MoveBase::removeAllPointCloud2s()
+{
+  point_cloud2s_.clear();
+  return true;
+}
+
+bool move_base::MoveBase::removeAllData()
+{
+  static_maps_.clear();
+  laser_scans_.clear();
+  point_clouds_.clear();
+  point_cloud2s_.clear();
+  return true;
+}
+
+bool move_base::MoveBase::updateGlobalCostmap(const std::string &frame_id)
+{
+  if (!planner_costmap_robot_)
+    return false;
+
+  // Tìm trong static_maps_ phần tử có header.frame_id == frame_id
+  robot_nav_msgs::OccupancyGrid* found_map = nullptr;
+  for (auto it = static_maps_.begin(); it != static_maps_.end(); ++it)
+  {
+    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
+  if (!found_map)
+    return false;
+
+  // Tìm StaticLayer và update với map đã tìm thấy
+  try
+  {
+    planner_costmap_robot_->pause();
+    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;
+      
+      // Update costmap với map đã tìm thấy
+      static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*found_map, frame_id);
+    }
+    planner_costmap_robot_->resume();
+    return true;
+  }
+  catch (const std::exception &ex)
+  {
+    robot::log_error("[%s:%d]\n EXCEPTION in updateGlobalCostmap: %s", __FILE__, __LINE__, ex.what());
+    return false;
+  }
+} 
+
+bool move_base::MoveBase::updateLocalCostmap(const std::string &frame_id)
+{
+  return false;
+}
+
 bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   if (setup_)