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

update from HiepLM

Browse Source
This commit is contained in:
HiepLM 2026-01-08 10:36:05 +07:00
parent c7e65910eb
commit a3e5168d88
6 changed files with 592 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/Libraries/costmap_2d

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

2
src/Navigations/Cores/move_base_core/CMakeLists.txt
View File

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

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

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

5
src/Navigations/Cores/move_base_core/package.xml
View File

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

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

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

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

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