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

update

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

2
config/costmap_local_params.yaml
View File

@ -1,4 +1,4 @@
local_costmap: local_costmap:
library_path: libplugins library_path: libplugins
global_frame: odom global_frame: odom
robot_base_frame: base_footprint robot_base_frame: base_footprint

2
src/Algorithms/Packages/global_planners/custom_planner

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

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

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

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

@ -88,146 +88,152 @@ namespace move_base
* @brief Get the robot's footprint (outline shape) in the global frame. * @brief Get the robot's footprint (outline shape) in the global frame.
* @return A vector of points representing the robot's footprint polygon. * @return A vector of points representing the robot's footprint polygon.
*/ */
virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() const override; virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() override;
/** /**
* @brief Add a static map to the navigation system. * @brief Add a static map to the navigation system.
* @param frame_id The frame ID of the map. * @param map_name The name of the map.
* @param map The map to add. * @param map The map to add.
*/ */
virtual void addStaticMap(const std::string &frame_id, robot_nav_msgs::OccupancyGrid *map) override; virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) override;
/**
* @brief 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<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 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. * @brief Add an odometry to the navigation system.
* @param frame_id The frame ID of the map. * @param odometry_name The name of the odometry.
* @return The map. * @param odometry The odometry to add.
*/ */
virtual robot_nav_msgs::OccupancyGrid* getStaticMap(const std::string &frame_id) override; virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) 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. * @brief Send a goal for the robot to navigate to.
@ -371,18 +377,20 @@ namespace move_base
/** /**
* @brief Update the global costmap. * @brief Update the global costmap.
* @param frame_id The frame ID of the costmap. * @param value The value to update the costmap.
* @return True if the global costmap was updated successfully, false otherwise. * @param name The name of the costmap.
*/ */
bool updateGlobalCostmap(const std::string &frame_id); template<typename T>
void updateGlobalCostmap(const T& value, const std::string &name);
/**
* @brief Update the local costmap.
* @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 Update the local costmap.
* @param value The value to update the costmap.
* @param name The name of the costmap.
*/
template<typename T>
void updateLocalCostmap(const T& value, const std::string &name);
/** /**
* @brief Performs a control cycle * @brief Performs a control cycle
* @param goal A reference to the goal to pursue * @param goal A reference to the goal to pursue
@ -444,7 +452,7 @@ namespace move_base
*/ */
void resetState(); void resetState();
// void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal); // void goalCB(const robot_geometry_msgs::PoseStamped &goal);
void planThread(); void planThread();

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

@ -24,20 +24,20 @@
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
move_base::MoveBase::MoveBase() move_base::MoveBase::MoveBase()
: initialized_(false), : initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false) pause_ctr_(false), paused_(false)
{ {
} }
move_base::MoveBase::MoveBase(robot::TFListenerPtr tf) move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
: initialized_(false), : initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false) pause_ctr_(false), paused_(false)
{ {
initialize(tf); initialize(tf);
} }
@ -46,6 +46,18 @@ move_base::MoveBase::~MoveBase()
{ {
recovery_behaviors_.clear(); recovery_behaviors_.clear();
// Cleanup static maps (shared_ptr tự động cleanup)
static_maps_.clear();
// Cleanup laser scans (shared_ptr tự động cleanup)
laser_scans_.clear();
// Cleanup point clouds (shared_ptr tự động cleanup)
point_clouds_.clear();
// Cleanup point cloud2s (shared_ptr tự động cleanup)
point_cloud2s_.clear();
if (planner_costmap_robot_ != NULL) if (planner_costmap_robot_ != NULL)
{ {
delete planner_costmap_robot_; delete planner_costmap_robot_;
@ -98,9 +110,9 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
tf_ = tf; tf_ = tf;
// NodeHandle("~") will automatically load YAML files from config directory // NodeHandle("~") will automatically load YAML files from config directory
private_nh_ = robot::NodeHandle("~"); private_nh_ = robot::NodeHandle("~");
recovery_trigger_ = PLANNING_R; recovery_trigger_ = PLANNING_R;
// get some parameters that will be global to the move base node // get some parameters that will be global to the move base node
std::string global_planner, local_planner; std::string global_planner, local_planner;
private_nh_.getParam("base_global_planner", global_planner, std::string("")); 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 // From config param
double inscribed_radius; double inscribed_radius;
private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2); 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); private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
inscribed_radius_ = inscribed_radius; inscribed_radius_ = inscribed_radius;
circumscribed_radius_ = circumscribed_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("shutdown_costmaps", shutdown_costmaps_, false);
private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true); private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, 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 // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
try try
{ {
planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_); planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
planner_costmap_robot_->pause(); planner_costmap_robot_->pause();
robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap(); robot_costmap_2d::LayeredCostmap *layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
// for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
// layer != layered_costmap_->getPlugins()->end();
// ++layer)
// {
// boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
// if (!static_layer)
// continue;
// robot_nav_msgs::OccupancyGrid *occupancy_grid = new robot_nav_msgs::OccupancyGrid();
// occupancy_grid->header.frame_id = "map";
// occupancy_grid->header.stamp = robot::Time::now();
// occupancy_grid->info.resolution = 0.05;
// occupancy_grid->info.width = 100;
// occupancy_grid->info.height = 100;
// occupancy_grid->info.origin.position.x = 0.0;
// occupancy_grid->info.origin.position.y = 0.0;
// occupancy_grid->info.origin.position.z = 0.0;
// occupancy_grid->info.origin.orientation.x = 0.0;
// occupancy_grid->info.origin.orientation.y = 0.0;
// occupancy_grid->info.origin.orientation.z = 0.0;
// occupancy_grid->info.origin.orientation.w = 1.0;
// occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height);
// for (int i = 0; i < occupancy_grid->data.size(); i++)
// {
// occupancy_grid->data[i] = robot_costmap_2d::NO_INFORMATION;
// }
// if (occupancy_grid)
// static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
// }
} }
catch (const std::exception &ex) catch (const std::exception &ex)
{ {
@ -230,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__); robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
throw std::runtime_error("Failed to load global planner " + global_planner); throw std::runtime_error("Failed to load global planner " + global_planner);
} }
if(planner_->initialize(global_planner, planner_costmap_robot_)) if (planner_->initialize(global_planner, planner_costmap_robot_))
robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__); robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__);
else else
robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__); robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__);
} }
catch (const std::exception &ex) 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_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
controller_costmap_robot_->pause(); controller_costmap_robot_->pause();
robot_costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap(); robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
// for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin(); // for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
// layer != layered_costmap_->getPlugins()->end(); // layer != layered_costmap_->getPlugins()->end();
// ++layer) // ++layer)
@ -256,7 +240,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
// 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;
@ -282,9 +266,9 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{ {
robot::PluginLoaderHelper loader; robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter"); std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter");
controller_loader_ = controller_loader_ =
boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>( boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
path_file_so, local_planner, boost::dll::load_mode::append_decorations); path_file_so, local_planner, boost::dll::load_mode::append_decorations);
tc_ = controller_loader_(); tc_ = controller_loader_();
if (!tc_) if (!tc_)
{ {
@ -342,10 +326,10 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
nav_feedback_->is_ready = true; nav_feedback_->is_ready = true;
} }
else else
{ {
robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__); robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
} }
initialized_ = true; initialized_ = true;
robot::log_info("========== End: initialize() - SUCCESS =========="); robot::log_info("========== End: initialize() - SUCCESS ==========");
} }
@ -368,88 +352,138 @@ void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msg
controller_costmap_robot_->setUnpaddedRobotFootprint(fprt); controller_costmap_robot_->setUnpaddedRobotFootprint(fprt);
} }
std::vector<robot_geometry_msgs::Point> move_base::MoveBase::getRobotFootprint() const std::vector<robot_geometry_msgs::Point> move_base::MoveBase::getRobotFootprint()
{ {
if (planner_costmap_robot_) if (planner_costmap_robot_)
return planner_costmap_robot_->getUnpaddedRobotFootprint(); return planner_costmap_robot_->getUnpaddedRobotFootprint();
return std::vector<robot_geometry_msgs::Point>(); return std::vector<robot_geometry_msgs::Point>();
} }
void move_base::MoveBase::addStaticMap(const std::string &frame_id, robot_nav_msgs::OccupancyGrid *map) void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map)
{ {
static_maps_[frame_id] = map; // Kiểm tra xem map_name đã tồn tại trong static_maps_ chưa
auto it = static_maps_.find(map_name);
if (it == static_maps_.end())
{
static_maps_[map_name] = map;
}
else
{
it->second = map;
}
updateGlobalCostmap<robot_nav_msgs::OccupancyGrid>(map, map_name);
updateLocalCostmap<robot_nav_msgs::OccupancyGrid>(map, map_name);
} }
robot_nav_msgs::OccupancyGrid* move_base::MoveBase::getStaticMap(const std::string &frame_id) robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::string &map_name)
{ {
auto it = static_maps_.find(frame_id); auto it = static_maps_.find(map_name);
if (it != static_maps_.end()) if (it != static_maps_.end())
{
return it->second; return it->second;
return nullptr; }
return robot_nav_msgs::OccupancyGrid();
} }
void move_base::MoveBase::addLaserScan(const std::string &frame_id, robot_sensor_msgs::LaserScan *laser_scan) void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
{ {
laser_scans_[frame_id] = laser_scan;
auto it = laser_scans_.find(laser_scan_name);
if (it == laser_scans_.end())
{
laser_scans_[laser_scan_name] = laser_scan;
}
else
{
it->second = laser_scan;
}
updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, laser_scan_name);
updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, laser_scan_name);
} }
robot_sensor_msgs::LaserScan* move_base::MoveBase::getLaserScan(const std::string &frame_id) robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)
{ {
auto it = laser_scans_.find(frame_id); auto it = laser_scans_.find(laser_scan_name);
if (it != laser_scans_.end()) if (it != laser_scans_.end())
{
return it->second; return it->second;
return nullptr; }
return robot_sensor_msgs::LaserScan();
} }
void move_base::MoveBase::addPointCloud(const std::string &frame_id, robot_sensor_msgs::PointCloud *point_cloud) void move_base::MoveBase::addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud)
{ {
point_clouds_[frame_id] = point_cloud; auto it = point_clouds_.find(point_cloud_name);
if (it == point_clouds_.end())
{
point_clouds_[point_cloud_name] = point_cloud;
}
else
{
it->second = point_cloud;
}
updateLocalCostmap<robot_sensor_msgs::PointCloud>(point_cloud, point_cloud_name);
updateGlobalCostmap<robot_sensor_msgs::PointCloud>(point_cloud, point_cloud_name);
} }
robot_sensor_msgs::PointCloud* move_base::MoveBase::getPointCloud(const std::string &frame_id) robot_sensor_msgs::PointCloud move_base::MoveBase::getPointCloud(const std::string &point_cloud_name)
{ {
auto it = point_clouds_.find(frame_id); auto it = point_clouds_.find(point_cloud_name);
if (it != point_clouds_.end()) if (it != point_clouds_.end())
{
return it->second; return it->second;
return nullptr; }
return robot_sensor_msgs::PointCloud();
} }
void move_base::MoveBase::addPointCloud2(const std::string &frame_id, robot_sensor_msgs::PointCloud2 *point_cloud2) void move_base::MoveBase::addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2)
{ {
point_cloud2s_[frame_id] = point_cloud2; auto it = point_cloud2s_.find(point_cloud2_name);
if (it == point_cloud2s_.end())
{
point_cloud2s_[point_cloud2_name] = point_cloud2;
}
else
{
it->second = point_cloud2;
}
updateLocalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, point_cloud2_name);
updateGlobalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, point_cloud2_name);
} }
robot_sensor_msgs::PointCloud2* move_base::MoveBase::getPointCloud2(const std::string &frame_id) robot_sensor_msgs::PointCloud2 move_base::MoveBase::getPointCloud2(const std::string &point_cloud2_name)
{ {
auto it = point_cloud2s_.find(frame_id); auto it = point_cloud2s_.find(point_cloud2_name);
if (it != point_cloud2s_.end()) if (it != point_cloud2s_.end())
{
return it->second; return it->second;
return nullptr; }
return robot_sensor_msgs::PointCloud2();
} }
std::map<std::string, robot_nav_msgs::OccupancyGrid*> move_base::MoveBase::getAllStaticMaps() std::map<std::string, robot_nav_msgs::OccupancyGrid> move_base::MoveBase::getAllStaticMaps()
{ {
return static_maps_; return static_maps_;
} }
std::map<std::string, robot_sensor_msgs::LaserScan*> move_base::MoveBase::getAllLaserScans() std::map<std::string, robot_sensor_msgs::LaserScan> move_base::MoveBase::getAllLaserScans()
{ {
return laser_scans_; return laser_scans_;
} }
std::map<std::string, robot_sensor_msgs::PointCloud*> move_base::MoveBase::getAllPointClouds() std::map<std::string, robot_sensor_msgs::PointCloud> move_base::MoveBase::getAllPointClouds()
{ {
return point_clouds_; return point_clouds_;
} }
std::map<std::string, robot_sensor_msgs::PointCloud2*> move_base::MoveBase::getAllPointCloud2s() std::map<std::string, robot_sensor_msgs::PointCloud2> move_base::MoveBase::getAllPointCloud2s()
{ {
return point_cloud2s_; return point_cloud2s_;
} }
bool move_base::MoveBase::removeStaticMap(const std::string &frame_id) bool move_base::MoveBase::removeStaticMap(const std::string &map_name)
{ {
auto it = static_maps_.find(frame_id); auto it = static_maps_.find(map_name);
if (it != static_maps_.end()) if (it != static_maps_.end())
{ {
static_maps_.erase(it); static_maps_.erase(it);
@ -458,9 +492,9 @@ bool move_base::MoveBase::removeStaticMap(const std::string &frame_id)
return false; return false;
} }
bool move_base::MoveBase::removeLaserScan(const std::string &frame_id) bool move_base::MoveBase::removeLaserScan(const std::string &laser_scan_name)
{ {
auto it = laser_scans_.find(frame_id); auto it = laser_scans_.find(laser_scan_name);
if (it != laser_scans_.end()) if (it != laser_scans_.end())
{ {
laser_scans_.erase(it); laser_scans_.erase(it);
@ -469,9 +503,9 @@ bool move_base::MoveBase::removeLaserScan(const std::string &frame_id)
return false; return false;
} }
bool move_base::MoveBase::removePointCloud(const std::string &frame_id) bool move_base::MoveBase::removePointCloud(const std::string &point_cloud_name)
{ {
auto it = point_clouds_.find(frame_id); auto it = point_clouds_.find(point_cloud_name);
if (it != point_clouds_.end()) if (it != point_clouds_.end())
{ {
point_clouds_.erase(it); point_clouds_.erase(it);
@ -480,9 +514,9 @@ bool move_base::MoveBase::removePointCloud(const std::string &frame_id)
return false; return false;
} }
bool move_base::MoveBase::removePointCloud2(const std::string &frame_id) bool move_base::MoveBase::removePointCloud2(const std::string &point_cloud2_name)
{ {
auto it = point_cloud2s_.find(frame_id); auto it = point_cloud2s_.find(point_cloud2_name);
if (it != point_cloud2s_.end()) if (it != point_cloud2s_.end())
{ {
point_cloud2s_.erase(it); point_cloud2s_.erase(it);
@ -524,55 +558,77 @@ bool move_base::MoveBase::removeAllData()
return true; return true;
} }
bool move_base::MoveBase::updateGlobalCostmap(const std::string &frame_id) template<typename T>
void move_base::MoveBase::updateGlobalCostmap(const T& value, const std::string &name)
{ {
if (!planner_costmap_robot_) if (!planner_costmap_robot_)
return false; return;
// Tìm trong static_maps_ phần tử có header.frame_id == frame_id // Tìm map theo map_name (key trong static_maps_)
robot_nav_msgs::OccupancyGrid* found_map = nullptr; auto it = static_maps_.find(name);
for (auto it = static_maps_.begin(); it != static_maps_.end(); ++it) if (it == static_maps_.end())
{ return;
if (it->second && it->second->header.frame_id == frame_id)
{
found_map = it->second;
break;
}
}
// Nếu không tìm thấy, return false robot_nav_msgs::OccupancyGrid *found_map = &it->second;
if (!found_map) std::string frame_id = found_map->header.frame_id;
return false;
// Tìm StaticLayer và update với map đã tìm thấy // Tìm StaticLayer và update với map đã tìm thấy
try try
{ {
planner_costmap_robot_->pause(); for (const auto &layer : *planner_costmap_robot_->getLayeredCostmap()->getPlugins())
robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
layer != layered_costmap_->getPlugins()->end();
++layer)
{ {
boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer); if (layer->getType() != robot_costmap_2d::LayerType::STATIC_LAYER && layer->getName() != name)
if (!static_layer)
continue; continue;
// Update costmap với map đã tìm thấy // Update costmap với map đã tìm thấy
static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*found_map, frame_id); layer->dataCallBack<T>(value, frame_id);
} }
planner_costmap_robot_->resume(); return;
return true;
} }
catch (const std::exception &ex) catch (const std::exception &ex)
{ {
robot::log_error("[%s:%d]\n EXCEPTION in updateGlobalCostmap: %s", __FILE__, __LINE__, ex.what()); robot::log_error("[%s:%d]\n EXCEPTION in Update GlobalCostmap: %s", __FILE__, __LINE__, ex.what());
return false; return;
} }
} }
bool move_base::MoveBase::updateLocalCostmap(const std::string &frame_id) template<typename T>
void move_base::MoveBase::updateLocalCostmap(const T& value, const std::string &name)
{ {
return false; if (!controller_costmap_robot_)
return;
// Tìm map theo map_name (key trong static_maps_)
auto it = static_maps_.find(name);
if (it == static_maps_.end())
return;
robot_nav_msgs::OccupancyGrid *found_map = &it->second;
std::string frame_id = found_map->header.frame_id;
// Tìm StaticLayer và update với map đã tìm thấy
try
{
for (const auto &layer : *planner_costmap_robot_->getLayeredCostmap()->getPlugins())
{
if (layer->getType() != robot_costmap_2d::LayerType::STATIC_LAYER && layer->getName() != name)
continue;
// Update costmap với map đã tìm thấy
layer->dataCallBack<T>(value, frame_id);
}
return;
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d]\n EXCEPTION in Update LocalCostmap: %s", __FILE__, __LINE__, ex.what());
return;
}
}
void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry)
{
robot::log_info("[%s:%d] addOdometry called for: %s", __FILE__, __LINE__, odometry_name.c_str());
} }
bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
@ -642,7 +698,6 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
return false; return false;
} }
bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal, bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance) double xy_goal_tolerance, double yaw_goal_tolerance)
{ {
@ -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."); // robot::log_warning("Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead.");
// return false; // return false;
// } // }
// if (!behavior["name"] || !behavior["type"]) // if (!behavior["name"] || !behavior["type"])
// { // {
// robot::log_warning("Recovery behaviors must have a name and a type. Using the default recovery behaviors instead."); // robot::log_warning("Recovery behaviors must have a name and a type. Using the default recovery behaviors instead.");
// return false; // return false;
// } // }
// // check for recovery behaviors with the same name // // check for recovery behaviors with the same name
// std::string name_i = behavior["name"].as<std::string>(); // std::string name_i = behavior["name"].as<std::string>();
// for (size_t j = i + 1; j < behavior_list.size(); ++j) // 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 // // Load the factory function from the shared library
// auto recovery_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>( // auto recovery_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
// path_file_so, behavior_type, boost::dll::load_mode::append_decorations); // path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
// // Create instance using the factory function // // Create instance using the factory function
// std::shared_ptr<robot_nav_core::RecoveryBehavior> behavior_ptr(recovery_loader()); // std::shared_ptr<robot_nav_core::RecoveryBehavior> behavior_ptr(recovery_loader());
// // shouldn't be possible, but it won't hurt to check // // shouldn't be possible, but it won't hurt to check
// if (behavior_ptr == nullptr) // if (behavior_ptr == nullptr)
// { // {
@ -1111,7 +1166,7 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
// } // }
// catch (const std::exception &ex) // catch (const std::exception &ex)
// { // {
// std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as<std::string>() // std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as<std::string>()
// << ". Error: " << ex.what() << std::endl; // << ". Error: " << ex.what() << std::endl;
// return false; // return false;
// } // }
@ -1136,7 +1191,7 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors()
{ {
recovery_behaviors_.clear(); recovery_behaviors_.clear();
recovery_behavior_names_.clear(); recovery_behavior_names_.clear();
try try
{ {
// Note: Parameters should be set via YAML config file, not programmatically // 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) // first, we'll load a recovery behavior to clear the costmap (conservative reset)
try try
{ {
std::string path_file_so ; std::string path_file_so;
auto clear_costmap_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>( auto clear_costmap_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations); path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
std::shared_ptr<robot_nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader()); std::shared_ptr<robot_nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader());
@ -1322,7 +1377,7 @@ void move_base::MoveBase::planThread()
// planner_plan_->clear(); // planner_plan_->clear();
// // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y); // // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y);
// bool gotPlan = makePlan(temp_goal, *planner_plan_); // bool gotPlan = makePlan(temp_goal, *planner_plan_);
// if (gotPlan) // if (gotPlan)
// { // {
// std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl; // 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) bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap)
{ {
tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose); tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
robot_geometry_msgs::PoseStamped robot_pose; robot_geometry_msgs::PoseStamped robot_pose;
tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
robot_pose.header.frame_id = robot_base_frame_; 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 robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
// get robot pose on the given costmap frame // 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()) current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
{ {
std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". " std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". "
"Current time: " << current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl; "Current time: "
<< current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl;
return false; return false;
} }
@ -1483,7 +1539,7 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro
robot_geometry_msgs::PoseStamped goal_pose, global_pose; robot_geometry_msgs::PoseStamped goal_pose, global_pose;
goal_pose = goal_pose_msg; goal_pose = goal_pose_msg;
goal_pose.header.stamp = robot::Time(); // latest available goal_pose.header.stamp = robot::Time(); // latest available
try try
{ {
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, data_convert::convertTime(goal_pose.header.stamp)); 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) bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist)
{ {
if (!twist_) { if (!twist_)
{
return false; return false;
} }
twist = *twist_; twist = *twist_;
@ -1518,7 +1575,8 @@ bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist)
robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback() robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback()
{ {
if (!nav_feedback_) { if (!nav_feedback_)
{
return nullptr; return nullptr;
} }
return nav_feedback_.get(); return nav_feedback_.get();