From a3e5168d8878630da4cc8b5c24c1a32ca16be9a7 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 8 Jan 2026 10:36:05 +0700 Subject: [PATCH] update from HiepLM --- src/Libraries/costmap_2d | 2 +- .../Cores/move_base_core/CMakeLists.txt | 2 + .../include/move_base_core/navigation.h | 173 +++++++++- .../Cores/move_base_core/package.xml | 5 +- .../move_base/include/move_base/move_base.h | 160 +++++++++ .../Packages/move_base/src/move_base.cpp | 315 +++++++++++++++--- 6 files changed, 592 insertions(+), 65 deletions(-) diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index f052dac..384897b 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit f052dac1421e7e2bedba4088780526a8775b0c29 +Subproject commit 384897b7504103468a5c0501157485c12d8289d8 diff --git a/src/Navigations/Cores/move_base_core/CMakeLists.txt b/src/Navigations/Cores/move_base_core/CMakeLists.txt index 8b1bd4e..65b0efc 100644 --- a/src/Navigations/Cores/move_base_core/CMakeLists.txt +++ b/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() diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index f0a86ac..d9d93a7 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -3,20 +3,23 @@ #include -// geometry msgs +// msgs #include #include #include #include #include - -// robot protocol msgs +#include +#include +#include +#include #include // tf #include #include #include +#include #include #include @@ -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.stamp = robot::Time::now(); - // goal.pose.position.x = pose.x + offset_distance * cos(pose.theta); - // goal.pose.position.y = pose.y + offset_distance * sin(pose.theta); + goal.header.frame_id = frame_id; + goal.header.stamp = robot::Time::now(); + goal.pose.position.x = pose.x + offset_distance * cos(pose.theta); + goal.pose.position.y = pose.y + offset_distance * sin(pose.theta); - // tf3::Quaternion q; - // q.setRPY(0, 0, pose.theta); - // goal.pose.orientation = tf3::convert(q); + tf3::Quaternion q; + q.setRPY(0, 0, pose.theta); + goal.pose.orientation = tf3::toMsg(q); return goal; } @@ -192,6 +195,150 @@ namespace move_base_core */ virtual void setRobotFootprint(const std::vector &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 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 getAllStaticMaps() = 0; + + /** + * @brief Get all laser scans from the navigation system. + * @return The laser scans. + */ + virtual std::map getAllLaserScans() = 0; + + /** + * @brief Get all point clouds from the navigation system. + * @return The point clouds. + */ + virtual std::map getAllPointClouds() = 0; + + /** + * @brief Get all point cloud2s from the navigation system. + * @return The point cloud2s. + */ + virtual std::map 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. @@ -315,11 +462,17 @@ namespace move_base_core * @return Pointer to the navigation feedback. */ virtual NavFeedback *getFeedback() = 0; + protected: // Shared feedback data for navigation status tracking std::shared_ptr nav_feedback_; std::shared_ptr twist_; + + std::map static_maps_; + std::map laser_scans_; + std::map point_clouds_; + std::map point_cloud2s_; BaseNavigation() = default; }; diff --git a/src/Navigations/Cores/move_base_core/package.xml b/src/Navigations/Cores/move_base_core/package.xml index ba67c66..8fe0e4b 100644 --- a/src/Navigations/Cores/move_base_core/package.xml +++ b/src/Navigations/Cores/move_base_core/package.xml @@ -24,11 +24,14 @@ robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs + robot_nav_msgs + robot_sensor_msgs tf3 robot_time robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs - + robot_nav_msgs + robot_sensor_msgs \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index 810b532..dd814e9 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -84,6 +84,151 @@ namespace move_base */ virtual void setRobotFootprint(const std::vector &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 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 getAllStaticMaps() override; + + /** + * @brief Get all laser scans from the navigation system. + * @return The laser scans. + */ + virtual std::map getAllLaserScans() override; + + /** + * @brief Get all point clouds from the navigation system. + * @return The point clouds. + */ + virtual std::map getAllPointClouds() override; + + /** + * @brief Get all point cloud2s from the navigation system. + * @return The point cloud2s. + */ + virtual std::map 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 diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 3836b57..37d3412 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include 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>::const_iterator layer = layered_costmap_->getPlugins()->begin(); - layer != layered_costmap_->getPlugins()->end(); - ++layer) - { - boost::shared_ptr static_layer = boost::dynamic_pointer_cast(*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(*occupancy_grid, "map"); - } + // for (std::vector>::const_iterator layer = layered_costmap_->getPlugins()->begin(); + // layer != layered_costmap_->getPlugins()->end(); + // ++layer) + // { + // boost::shared_ptr static_layer = boost::dynamic_pointer_cast(*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(*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>::const_iterator layer = layered_costmap_->getPlugins()->begin(); - layer != layered_costmap_->getPlugins()->end(); - ++layer) - { - boost::shared_ptr obstacle_layer = boost::dynamic_pointer_cast(*layer); - if (!obstacle_layer) - continue; - robot_sensor_msgs::LaserScan *laser_scan = new robot_sensor_msgs::LaserScan(); - laser_scan->header.frame_id = "laser"; - laser_scan->header.stamp = robot::Time::now(); - laser_scan->angle_min = -M_PI; - laser_scan->angle_max = M_PI; - laser_scan->angle_increment = M_PI / 180; - laser_scan->time_increment = 0.0; - laser_scan->scan_time = 0.0; - laser_scan->range_min = 0.0; - laser_scan->range_max = 10.0; - laser_scan->ranges.resize(180); - for (int i = 0; i < 180; i++) - { - laser_scan->ranges[i] = 10.0; - } - if (laser_scan) - obstacle_layer->dataCallBack(*laser_scan, "laser"); - } + // for (std::vector>::const_iterator layer = layered_costmap_->getPlugins()->begin(); + // layer != layered_costmap_->getPlugins()->end(); + // ++layer) + // { + // boost::shared_ptr obstacle_layer = boost::dynamic_pointer_cast(*layer); + // if (!obstacle_layer) + // continue; + // robot_sensor_msgs::LaserScan *laser_scan = new robot_sensor_msgs::LaserScan(); + // laser_scan->header.frame_id = "laser"; + // laser_scan->header.stamp = robot::Time::now(); + // laser_scan->angle_min = -M_PI; + // laser_scan->angle_max = M_PI; + // laser_scan->angle_increment = M_PI / 180; + // laser_scan->time_increment = 0.0; + // laser_scan->scan_time = 0.0; + // laser_scan->range_min = 0.0; + // laser_scan->range_max = 10.0; + // laser_scan->ranges.resize(180); + // for (int i = 0; i < 180; i++) + // { + // laser_scan->ranges[i] = 10.0; + // } + // if (laser_scan) + // obstacle_layer->dataCallBack(*laser_scan, "laser"); + // } } catch (const std::exception &ex) { @@ -366,6 +368,213 @@ void move_base::MoveBase::setRobotFootprint(const std::vectorsetUnpaddedRobotFootprint(fprt); } +std::vector move_base::MoveBase::getRobotFootprint() const +{ + if (planner_costmap_robot_) + return planner_costmap_robot_->getUnpaddedRobotFootprint(); + return std::vector(); +} + +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 move_base::MoveBase::getAllStaticMaps() +{ + return static_maps_; +} + +std::map move_base::MoveBase::getAllLaserScans() +{ + return laser_scans_; +} + +std::map move_base::MoveBase::getAllPointClouds() +{ + return point_clouds_; +} + +std::map 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>::const_iterator layer = layered_costmap_->getPlugins()->begin(); + layer != layered_costmap_->getPlugins()->end(); + ++layer) + { + boost::shared_ptr static_layer = boost::dynamic_pointer_cast(*layer); + if (!static_layer) + continue; + + // Update costmap với map đã tìm thấy + static_layer->dataCallBack(*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_)