From f5e7e1f1e06189a181a9c099652e2c95f6166e49 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 9 Jan 2026 10:39:30 +0700 Subject: [PATCH] update --- config/costmap_local_params.yaml | 2 +- .../Packages/global_planners/custom_planner | 2 +- .../include/move_base_core/navigation.h | 74 ++-- .../move_base/include/move_base/move_base.h | 298 ++++++++-------- .../Packages/move_base/src/move_base.cpp | 320 +++++++++++------- 5 files changed, 385 insertions(+), 311 deletions(-) diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml index 2e74a01..6af3ebb 100755 --- a/config/costmap_local_params.yaml +++ b/config/costmap_local_params.yaml @@ -1,4 +1,4 @@ -local_costmap: +local_costmap: library_path: libplugins global_frame: odom robot_base_frame: base_footprint diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner index 540d793..a2bebb5 160000 --- a/src/Algorithms/Packages/global_planners/custom_planner +++ b/src/Algorithms/Packages/global_planners/custom_planner @@ -1 +1 @@ -Subproject commit 540d79321b53ef36a4979f15c6e36b759c134e07 +Subproject commit a2bebb5be969471a135601da78866cc9d1db84b1 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 d9d93a7..b9652b2 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 @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -199,115 +200,115 @@ namespace move_base_core * @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; + virtual std::vector getRobotFootprint() = 0; /** * @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. */ - 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. - * @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. */ - 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. - * @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. */ - 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. - * @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. */ - 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. - * @param frame_id The frame ID of the map. + * @param map_name The name of 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. - * @param frame_id The frame ID of the laser scan. + * @param laser_scan_name The name of 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. - * @param frame_id The frame ID of the point cloud. + * @param point_cloud_name The name of 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. - * @param frame_id The frame ID of the point cloud2. + * @param point_cloud2_name The name of 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. * @return The static maps. */ - virtual std::map getAllStaticMaps() = 0; + virtual std::map getAllStaticMaps() = 0; /** * @brief Get all laser scans from the navigation system. * @return The laser scans. */ - virtual std::map getAllLaserScans() = 0; + virtual std::map getAllLaserScans() = 0; /** * @brief Get all point clouds from the navigation system. * @return The point clouds. */ - virtual std::map getAllPointClouds() = 0; + virtual std::map getAllPointClouds() = 0; /** * @brief Get all point cloud2s from the navigation system. * @return The point cloud2s. */ - virtual std::map getAllPointCloud2s() = 0; + virtual std::map getAllPointCloud2s() = 0; /** * @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. */ - 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. - * @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. */ - 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. - * @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. */ - 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. - * @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. */ - 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. @@ -339,6 +340,13 @@ namespace move_base_core */ 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. * @param goal Target pose in the global frame. @@ -469,10 +477,10 @@ namespace move_base_core 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_; + std::map static_maps_; + std::map laser_scans_; + std::map point_clouds_; + std::map point_cloud2s_; BaseNavigation() = default; }; 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 dd814e9..6802ab6 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 @@ -88,146 +88,152 @@ namespace move_base * @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; + virtual std::vector getRobotFootprint() override; - /** + /** * @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. */ - 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 Add a laser scan to the navigation system. + * @param laser_scan_name The name of the laser scan. + * @param laser_scan The laser scan to add. + */ + 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. + * @param point_cloud_name The name of the point cloud. + * @param point_cloud The point cloud to add. + */ + 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. + * @param point_cloud2_name The name of the point cloud2. + * @param point_cloud2 The point cloud2 to add. + */ + 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. + * @param laser_scan_name The name of the laser scan. + * @return The laser scan. + */ + virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) override; + + /** + * @brief Get a point cloud from the navigation system. + * @param point_cloud_name The name of the point cloud. + * @return The point cloud. + */ + virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) override; + + /** + * @brief Get a point cloud2 from the navigation system. + * @param point_cloud2_name The name of the point cloud2. + * @return The point cloud2. + */ + virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) 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 map_name The name of the map. + * @return True if the map was removed successfully. + */ + virtual bool removeStaticMap(const std::string &map_name) override; + + /** + * @brief Remove a laser scan from the navigation system. + * @param laser_scan_name The name of the laser scan. + * @return True if the laser scan was removed successfully. + */ + virtual bool removeLaserScan(const std::string &laser_scan_name) override; + + /** + * @brief Remove a point cloud from the navigation system. + * @param point_cloud_name The name of the point cloud. + * @return True if the point cloud was removed successfully. + */ + virtual bool removePointCloud(const std::string &point_cloud_name) override; + + /** + * @brief Remove a point cloud2 from the navigation system. + * @param point_cloud2_name The name of the point cloud2. + * @return True if the point cloud2 was removed successfully. + */ + virtual bool removePointCloud2(const std::string &point_cloud2_name) 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 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 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. @@ -371,18 +377,20 @@ namespace move_base /** * @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. + * @param value The value to update the costmap. + * @param name The name of the costmap. */ - 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); + template + void updateGlobalCostmap(const T& value, const std::string &name); + /** + * @brief Update the local costmap. + * @param value The value to update the costmap. + * @param name The name of the costmap. + */ + template + void updateLocalCostmap(const T& value, const std::string &name); + /** * @brief Performs a control cycle * @param goal A reference to the goal to pursue @@ -444,7 +452,7 @@ namespace move_base */ void resetState(); - // void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal); + // void goalCB(const robot_geometry_msgs::PoseStamped &goal); void planThread(); diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 37d3412..d4055b9 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -24,20 +24,20 @@ #include move_base::MoveBase::MoveBase() -: initialized_(false), - planner_costmap_robot_(NULL), controller_costmap_robot_(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), - pause_ctr_(false), paused_(false) + : initialized_(false), + planner_costmap_robot_(NULL), controller_costmap_robot_(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), + pause_ctr_(false), paused_(false) { } move_base::MoveBase::MoveBase(robot::TFListenerPtr tf) -: initialized_(false), - planner_costmap_robot_(NULL), controller_costmap_robot_(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), - pause_ctr_(false), paused_(false) + : initialized_(false), + planner_costmap_robot_(NULL), controller_costmap_robot_(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), + pause_ctr_(false), paused_(false) { initialize(tf); } @@ -46,6 +46,18 @@ move_base::MoveBase::~MoveBase() { 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) { delete planner_costmap_robot_; @@ -98,9 +110,9 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) tf_ = tf; // NodeHandle("~") will automatically load YAML files from config directory - private_nh_ = robot::NodeHandle("~"); + private_nh_ = robot::NodeHandle("~"); recovery_trigger_ = PLANNING_R; - + // get some parameters that will be global to the move base node std::string global_planner, local_planner; private_nh_.getParam("base_global_planner", global_planner, std::string("")); @@ -164,7 +176,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) // From config param double inscribed_radius; private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2); - double circumscribed_radius; + double circumscribed_radius; private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3); inscribed_radius_ = inscribed_radius; circumscribed_radius_ = circumscribed_radius; @@ -175,41 +187,13 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false); private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true); private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true); - + // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map try { 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"); - // } + robot_costmap_2d::LayeredCostmap *layered_costmap_ = planner_costmap_robot_->getLayeredCostmap(); } catch (const std::exception &ex) { @@ -230,9 +214,9 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__); throw std::runtime_error("Failed to load global planner " + global_planner); } - 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__); - else + else robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__); } catch (const std::exception &ex) @@ -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_->pause(); - robot_costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap(); + 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) @@ -256,7 +240,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) // 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_min = -M_PI; // laser_scan->angle_max = M_PI; // laser_scan->angle_increment = M_PI / 180; // laser_scan->time_increment = 0.0; @@ -282,9 +266,9 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) { robot::PluginLoaderHelper loader; std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter"); - controller_loader_ = - boost::dll::import_alias( - path_file_so, local_planner, boost::dll::load_mode::append_decorations); + controller_loader_ = + boost::dll::import_alias( + path_file_so, local_planner, boost::dll::load_mode::append_decorations); tc_ = controller_loader_(); if (!tc_) { @@ -342,10 +326,10 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) nav_feedback_->is_ready = true; } else - { + { robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__); } - + initialized_ = true; robot::log_info("========== End: initialize() - SUCCESS =========="); } @@ -368,88 +352,138 @@ void move_base::MoveBase::setRobotFootprint(const std::vectorsetUnpaddedRobotFootprint(fprt); } -std::vector move_base::MoveBase::getRobotFootprint() const +std::vector move_base::MoveBase::getRobotFootprint() { 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) +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(map, map_name); + updateLocalCostmap(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()) + { 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(laser_scan, laser_scan_name); + updateGlobalCostmap(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()) + { 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(point_cloud, point_cloud_name); + updateGlobalCostmap(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()) + { 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(point_cloud2, point_cloud2_name); + updateGlobalCostmap(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()) + { return it->second; - return nullptr; + } + return robot_sensor_msgs::PointCloud2(); } -std::map move_base::MoveBase::getAllStaticMaps() +std::map move_base::MoveBase::getAllStaticMaps() { return static_maps_; } -std::map move_base::MoveBase::getAllLaserScans() +std::map move_base::MoveBase::getAllLaserScans() { return laser_scans_; } -std::map move_base::MoveBase::getAllPointClouds() +std::map move_base::MoveBase::getAllPointClouds() { return point_clouds_; } -std::map move_base::MoveBase::getAllPointCloud2s() +std::map move_base::MoveBase::getAllPointCloud2s() { 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()) { static_maps_.erase(it); @@ -458,9 +492,9 @@ bool move_base::MoveBase::removeStaticMap(const std::string &frame_id) 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()) { laser_scans_.erase(it); @@ -469,9 +503,9 @@ bool move_base::MoveBase::removeLaserScan(const std::string &frame_id) 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()) { point_clouds_.erase(it); @@ -480,9 +514,9 @@ bool move_base::MoveBase::removePointCloud(const std::string &frame_id) 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()) { point_cloud2s_.erase(it); @@ -524,55 +558,77 @@ bool move_base::MoveBase::removeAllData() return true; } -bool move_base::MoveBase::updateGlobalCostmap(const std::string &frame_id) +template +void move_base::MoveBase::updateGlobalCostmap(const T& value, const std::string &name) { if (!planner_costmap_robot_) - return false; + return; - // 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; - } - } + // Tìm map theo map_name (key trong static_maps_) + auto it = static_maps_.find(name); + if (it == static_maps_.end()) + return; - // Nếu không tìm thấy, return false - if (!found_map) - return false; + 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 { - 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) + for (const auto &layer : *planner_costmap_robot_->getLayeredCostmap()->getPlugins()) { - boost::shared_ptr static_layer = boost::dynamic_pointer_cast(*layer); - if (!static_layer) + if (layer->getType() != robot_costmap_2d::LayerType::STATIC_LAYER && layer->getName() != name) continue; - + // Update costmap với map đã tìm thấy - static_layer->dataCallBack(*found_map, frame_id); + layer->dataCallBack(value, frame_id); } - planner_costmap_robot_->resume(); - return true; + return; } catch (const std::exception &ex) { - robot::log_error("[%s:%d]\n EXCEPTION in updateGlobalCostmap: %s", __FILE__, __LINE__, ex.what()); - return false; + robot::log_error("[%s:%d]\n EXCEPTION in Update GlobalCostmap: %s", __FILE__, __LINE__, ex.what()); + return; } -} +} -bool move_base::MoveBase::updateLocalCostmap(const std::string &frame_id) +template +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(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) @@ -642,7 +698,6 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, return false; } - bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) { @@ -1056,13 +1111,13 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node) // robot::log_warning("Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead."); // return false; // } - + // if (!behavior["name"] || !behavior["type"]) // { // robot::log_warning("Recovery behaviors must have a name and a type. Using the default recovery behaviors instead."); // return false; // } - + // // check for recovery behaviors with the same name // std::string name_i = behavior["name"].as(); // for (size_t j = i + 1; j < behavior_list.size(); ++j) @@ -1094,10 +1149,10 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node) // // Load the factory function from the shared library // auto recovery_loader = boost::dll::import_alias( // path_file_so, behavior_type, boost::dll::load_mode::append_decorations); - + // // Create instance using the factory function // std::shared_ptr behavior_ptr(recovery_loader()); - + // // shouldn't be possible, but it won't hurt to check // if (behavior_ptr == nullptr) // { @@ -1111,7 +1166,7 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node) // } // catch (const std::exception &ex) // { - // std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as() + // std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as() // << ". Error: " << ex.what() << std::endl; // return false; // } @@ -1136,7 +1191,7 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors() { recovery_behaviors_.clear(); recovery_behavior_names_.clear(); - + try { // Note: Parameters should be set via YAML config file, not programmatically @@ -1145,7 +1200,7 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors() // first, we'll load a recovery behavior to clear the costmap (conservative reset) try { - std::string path_file_so ; + std::string path_file_so; auto clear_costmap_loader = boost::dll::import_alias( path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations); std::shared_ptr cons_clear(clear_costmap_loader()); @@ -1322,7 +1377,7 @@ void move_base::MoveBase::planThread() // planner_plan_->clear(); // // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y); // bool gotPlan = makePlan(temp_goal, *planner_plan_); - + // if (gotPlan) // { // std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl; @@ -1430,12 +1485,12 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap) { - + tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose); robot_geometry_msgs::PoseStamped robot_pose; tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); robot_pose.header.frame_id = robot_base_frame_; - robot_pose.header.stamp = robot::Time(); // latest available + robot_pose.header.stamp = robot::Time(); // latest available robot::Time current_time = robot::Time::now(); // save time for checking tf delay later // get robot pose on the given costmap frame @@ -1465,7 +1520,8 @@ bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_ current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance()) { 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; } @@ -1483,7 +1539,7 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro robot_geometry_msgs::PoseStamped goal_pose, global_pose; goal_pose = goal_pose_msg; - goal_pose.header.stamp = robot::Time(); // latest available + goal_pose.header.stamp = robot::Time(); // latest available try { tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, data_convert::convertTime(goal_pose.header.stamp)); @@ -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) { - if (!twist_) { + if (!twist_) + { return false; } 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() { - if (!nav_feedback_) { + if (!nav_feedback_) + { return nullptr; } return nav_feedback_.get();