update
This commit is contained in:
parent
a3e5168d88
commit
f5e7e1f1e0
|
|
@ -1 +1 @@
|
|||
Subproject commit 540d79321b53ef36a4979f15c6e36b759c134e07
|
||||
Subproject commit a2bebb5be969471a135601da78866cc9d1db84b1
|
||||
|
|
@ -9,6 +9,7 @@
|
|||
#include <robot_geometry_msgs/Pose2D.h>
|
||||
#include <robot_geometry_msgs/Vector3.h>
|
||||
#include <robot_nav_2d_msgs/Twist2DStamped.h>
|
||||
#include <robot_nav_msgs/Odometry.h>
|
||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||
#include <robot_sensor_msgs/LaserScan.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.
|
||||
* @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.
|
||||
* @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<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.
|
||||
* @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.
|
||||
* @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.
|
||||
* @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.
|
||||
* @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<NavFeedback> nav_feedback_;
|
||||
std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
|
||||
|
||||
std::map<std::string, robot_nav_msgs::OccupancyGrid*> static_maps_;
|
||||
std::map<std::string, robot_sensor_msgs::LaserScan*> laser_scans_;
|
||||
std::map<std::string, robot_sensor_msgs::PointCloud*> point_clouds_;
|
||||
std::map<std::string, robot_sensor_msgs::PointCloud2*> point_cloud2s_;
|
||||
std::map<std::string, robot_nav_msgs::OccupancyGrid> static_maps_;
|
||||
std::map<std::string, robot_sensor_msgs::LaserScan> laser_scans_;
|
||||
std::map<std::string, robot_sensor_msgs::PointCloud> point_clouds_;
|
||||
std::map<std::string, robot_sensor_msgs::PointCloud2> point_cloud2s_;
|
||||
BaseNavigation() = default;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -88,115 +88,115 @@ 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<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.
|
||||
* @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;
|
||||
|
||||
/**
|
||||
* @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;
|
||||
virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) override;
|
||||
|
||||
/**
|
||||
* @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) override;
|
||||
virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) override;
|
||||
|
||||
/**
|
||||
* @brief Add a point cloud to the navigation system.
|
||||
* @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) override;
|
||||
virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) override;
|
||||
|
||||
/**
|
||||
* @brief Add a point cloud2 to the navigation system.
|
||||
* @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) override;
|
||||
virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) override;
|
||||
|
||||
/**
|
||||
* @brief Get a static map from the navigation system.
|
||||
* @param map_name The name of the map.
|
||||
* @return The map.
|
||||
*/
|
||||
virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) override;
|
||||
|
||||
/**
|
||||
* @brief Get a laser scan from the navigation system.
|
||||
* @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) override;
|
||||
virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) override;
|
||||
|
||||
/**
|
||||
* @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) override;
|
||||
virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) override;
|
||||
|
||||
/**
|
||||
* @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) override;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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.
|
||||
* @param map_name The name of the map.
|
||||
* @return True if the map was removed successfully.
|
||||
*/
|
||||
virtual bool removeStaticMap(const std::string &frame_id) override;
|
||||
virtual bool removeStaticMap(const std::string &map_name) override;
|
||||
|
||||
/**
|
||||
* @brief Remove a laser scan from the navigation system.
|
||||
* @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) override;
|
||||
virtual bool removeLaserScan(const std::string &laser_scan_name) override;
|
||||
|
||||
/**
|
||||
* @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) override;
|
||||
virtual bool removePointCloud(const std::string &point_cloud_name) override;
|
||||
|
||||
/**
|
||||
* @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) override;
|
||||
virtual bool removePointCloud2(const std::string &point_cloud2_name) override;
|
||||
|
||||
/**
|
||||
* @brief Remove all static maps from the navigation system.
|
||||
|
|
@ -228,6 +228,12 @@ namespace move_base
|
|||
*/
|
||||
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,17 +377,19 @@ 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);
|
||||
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.
|
||||
* @param value The value to update the costmap.
|
||||
* @param name The name of the costmap.
|
||||
*/
|
||||
bool updateLocalCostmap(const std::string &frame_id);
|
||||
template<typename T>
|
||||
void updateLocalCostmap(const T& value, const std::string &name);
|
||||
|
||||
/**
|
||||
* @brief Performs a control cycle
|
||||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@
|
|||
#include <robot_sensor_msgs/PointCloud2.h>
|
||||
|
||||
move_base::MoveBase::MoveBase()
|
||||
: initialized_(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),
|
||||
|
|
@ -33,7 +33,7 @@ move_base::MoveBase::MoveBase()
|
|||
}
|
||||
|
||||
move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
|
||||
: initialized_(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),
|
||||
|
|
@ -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_;
|
||||
|
|
@ -181,35 +193,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
|
|||
{
|
||||
planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
|
||||
planner_costmap_robot_->pause();
|
||||
robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
|
||||
// for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
|
||||
// layer != layered_costmap_->getPlugins()->end();
|
||||
// ++layer)
|
||||
// {
|
||||
// boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
|
||||
// if (!static_layer)
|
||||
// continue;
|
||||
// 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");
|
||||
// }
|
||||
robot_costmap_2d::LayeredCostmap *layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
|
||||
}
|
||||
catch (const std::exception &ex)
|
||||
{
|
||||
|
|
@ -230,7 +214,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
|
|||
robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
|
||||
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
|
||||
robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__);
|
||||
|
|
@ -245,7 +229,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
|
|||
{
|
||||
controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
|
||||
controller_costmap_robot_->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();
|
||||
// layer != layered_costmap_->getPlugins()->end();
|
||||
// ++layer)
|
||||
|
|
@ -368,88 +352,138 @@ void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msg
|
|||
controller_costmap_robot_->setUnpaddedRobotFootprint(fprt);
|
||||
}
|
||||
|
||||
std::vector<robot_geometry_msgs::Point> move_base::MoveBase::getRobotFootprint() const
|
||||
std::vector<robot_geometry_msgs::Point> move_base::MoveBase::getRobotFootprint()
|
||||
{
|
||||
if (planner_costmap_robot_)
|
||||
return planner_costmap_robot_->getUnpaddedRobotFootprint();
|
||||
return std::vector<robot_geometry_msgs::Point>();
|
||||
}
|
||||
|
||||
void move_base::MoveBase::addStaticMap(const std::string &frame_id, robot_nav_msgs::OccupancyGrid *map)
|
||||
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())
|
||||
{
|
||||
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())
|
||||
{
|
||||
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())
|
||||
{
|
||||
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())
|
||||
{
|
||||
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_;
|
||||
}
|
||||
|
||||
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_;
|
||||
}
|
||||
|
||||
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_;
|
||||
}
|
||||
|
||||
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_;
|
||||
}
|
||||
|
||||
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<typename T>
|
||||
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<boost::shared_ptr<robot_costmap_2d::Layer>>::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<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*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<robot_nav_msgs::OccupancyGrid>(*found_map, frame_id);
|
||||
layer->dataCallBack<T>(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<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)
|
||||
|
|
@ -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)
|
||||
{
|
||||
|
|
@ -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<robot_nav_core::RecoveryBehavior::Ptr()>(
|
||||
path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
|
||||
std::shared_ptr<robot_nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader());
|
||||
|
|
@ -1465,7 +1520,8 @@ bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_
|
|||
current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user