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
robot_nav_msgs
robot_map_msgs
laser_geometry
robot_laser_geometry
robot_visualization_msgs
voxel_grid
robot_voxel_grid
tf3
robot_tf3_geometry_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_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/PointCloud2.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
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> > 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_nav_msgs/OccupancyGrid.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/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.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
{
@ -58,7 +58,7 @@ class VoxelLayer : public ObstacleLayer
{
public:
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.
}
@ -90,7 +90,7 @@ private:
bool publish_voxel_;
voxel_grid::VoxelGrid voxel_grid_;
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_;
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()
{
ObstacleLayer::matchSize();
voxel_grid_.resize(size_x_, size_y_, size_z_);
if (!(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_))
robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_))
{
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";
std::abort(); // dừng chương trình
}
@ -140,14 +140,14 @@ void VoxelLayer::reset()
{
deactivate();
resetMaps();
voxel_grid_.reset();
robot_voxel_grid_.reset();
activate();
}
void VoxelLayer::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,
@ -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
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);
@ -229,12 +229,12 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// if (publish_voxel_)
// {
// robot_costmap_2d::VoxelGrid grid_msg;
// unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
// grid_msg.size_x = voxel_grid_.sizeX();
// grid_msg.size_y = voxel_grid_.sizeY();
// grid_msg.size_z = voxel_grid_.sizeZ();
// unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY();
// grid_msg.size_x = robot_voxel_grid_.sizeX();
// grid_msg.size_y = robot_voxel_grid_.sizeY();
// grid_msg.size_z = robot_voxel_grid_.sizeZ();
// 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.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)
{
*current = FREE_SPACE;
voxel_grid_.clearVoxelColumn(index);
robot_voxel_grid_.clearVoxelColumn(index);
}
}
current++;
@ -397,8 +397,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
{
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);
voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
// robot_voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
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,
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
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* voxel_map = voxel_grid_.getData();
unsigned int* voxel_map = robot_voxel_grid_.getData();
// 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);