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
Download Patch File Download Diff File Expand all files Collapse all files

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);