DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

hiep update

Browse Source
This commit is contained in:
HiepLM 2025-12-30 10:42:34 +07:00
parent 72b2f3c639
commit 6c682712fe
4 changed files with 23 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -61,9 +61,9 @@ target_link_libraries(robot_costmap_2d
geometry_msgs geometry_msgs
robot_nav_msgs robot_nav_msgs
robot_map_msgs robot_map_msgs
laser_geometry robot_laser_geometry
robot_visualization_msgs robot_visualization_msgs
voxel_grid robot_voxel_grid
tf3 tf3
robot_tf3_geometry_msgs robot_tf3_geometry_msgs
robot_tf3_sensor_msgs robot_tf3_sensor_msgs

4
include/robot_costmap_2d/obstacle_layer.h
View File

@ -46,7 +46,7 @@
#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 <laser_geometry/laser_geometry.hpp> #include <robot_laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/PointCloud.h> #include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h> #include <robot_sensor_msgs/point_cloud_conversion.h>
@ -149,7 +149,7 @@ protected:
std::string global_frame_; ///< @brief The global frame for the costmap std::string global_frame_; ///< @brief The global frame for the costmap
double max_obstacle_height_; ///< @brief Max Obstacle Height double max_obstacle_height_; ///< @brief Max Obstacle Height
laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds robot_laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles

8
include/robot_costmap_2d/voxel_layer.h
View File

@ -44,12 +44,12 @@
#include <robot_costmap_2d/voxel_grid.h> #include <robot_costmap_2d/voxel_grid.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 <laser_geometry/laser_geometry.hpp> #include <robot_laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/PointCloud.h> #include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h> #include <robot_sensor_msgs/point_cloud_conversion.h>
#include <robot_costmap_2d/obstacle_layer.h> #include <robot_costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h> #include <robot_voxel_grid/voxel_grid.h>
namespace robot_costmap_2d namespace robot_costmap_2d
{ {
@ -58,7 +58,7 @@ class VoxelLayer : public ObstacleLayer
{ {
public: public:
VoxelLayer() : VoxelLayer() :
voxel_grid_(0, 0, 0) robot_voxel_grid_(0, 0, 0)
{ {
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D. costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
} }
@ -90,7 +90,7 @@ private:
bool publish_voxel_; bool publish_voxel_;
voxel_grid::VoxelGrid voxel_grid_; robot_voxel_grid::VoxelGrid robot_voxel_grid_;
double z_resolution_, origin_z_; double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_; unsigned int unknown_threshold_, mark_threshold_, size_z_;
robot_sensor_msgs::PointCloud clearing_endpoints_; robot_sensor_msgs::PointCloud clearing_endpoints_;

30
plugins/voxel_layer.cpp
View File

@ -126,11 +126,11 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
void VoxelLayer::matchSize() void VoxelLayer::matchSize()
{ {
ObstacleLayer::matchSize(); ObstacleLayer::matchSize();
voxel_grid_.resize(size_x_, size_y_, size_z_); robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
if (!(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_)) if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_))
{ {
std::cerr << "[FATAL] Voxel grid size mismatch: " std::cerr << "[FATAL] Voxel grid size mismatch: "
<< "voxel(" << voxel_grid_.sizeX() << ", " << voxel_grid_.sizeY() << "voxel(" << robot_voxel_grid_.sizeX() << ", " << robot_voxel_grid_.sizeY()
<< ") but costmap(" << size_x_ << ", " << size_y_ << ")\n"; << ") but costmap(" << size_x_ << ", " << size_y_ << ")\n";
std::abort(); // dừng chương trình std::abort(); // dừng chương trình
} }
@ -140,14 +140,14 @@ void VoxelLayer::reset()
{ {
deactivate(); deactivate();
resetMaps(); resetMaps();
voxel_grid_.reset(); robot_voxel_grid_.reset();
activate(); activate();
} }
void VoxelLayer::resetMaps() void VoxelLayer::resetMaps()
{ {
Costmap2D::resetMaps(); Costmap2D::resetMaps();
voxel_grid_.reset(); robot_voxel_grid_.reset();
} }
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@ -216,7 +216,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
} }
// mark the cell in the voxel grid and check if we should also mark it in the costmap // mark the cell in the voxel grid and check if we should also mark it in the costmap
if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) if (robot_voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
{ {
unsigned int index = getIndex(mx, my); unsigned int index = getIndex(mx, my);
@ -229,12 +229,12 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// if (publish_voxel_) // if (publish_voxel_)
// { // {
// robot_costmap_2d::VoxelGrid grid_msg; // robot_costmap_2d::VoxelGrid grid_msg;
// unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); // unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY();
// grid_msg.size_x = voxel_grid_.sizeX(); // grid_msg.size_x = robot_voxel_grid_.sizeX();
// grid_msg.size_y = voxel_grid_.sizeY(); // grid_msg.size_y = robot_voxel_grid_.sizeY();
// grid_msg.size_z = voxel_grid_.sizeZ(); // grid_msg.size_z = robot_voxel_grid_.sizeZ();
// grid_msg.data.resize(size); // grid_msg.data.resize(size);
// memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); // memcpy(&grid_msg.data[0], robot_voxel_grid_.getData(), size * sizeof(unsigned int));
// grid_msg.origin.x = origin_x_; // grid_msg.origin.x = origin_x_;
// grid_msg.origin.y = origin_y_; // grid_msg.origin.y = origin_y_;
@ -291,7 +291,7 @@ void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_
if (clear_no_info || *current != NO_INFORMATION) if (clear_no_info || *current != NO_INFORMATION)
{ {
*current = FREE_SPACE; *current = FREE_SPACE;
voxel_grid_.clearVoxelColumn(index); robot_voxel_grid_.clearVoxelColumn(index);
} }
} }
current++; current++;
@ -397,8 +397,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
{ {
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
// voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z); // robot_voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
cell_raytrace_range); cell_raytrace_range);
@ -455,7 +455,7 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
// we need a map to store the obstacles in the window temporarily // we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y]; unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
unsigned int* voxel_map = voxel_grid_.getData(); unsigned int* voxel_map = robot_voxel_grid_.getData();
// copy the local window in the costmap to the local map // copy the local window in the costmap to the local map
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);