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

optimal voxel layer

Browse Source
This commit is contained in:
duongtd
2026-07-23 11:18:04 +07:00
parent 03b13f6936
commit 28a32ee67c
2 changed files with 85 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
plugins/voxel_layer.cpp
View File

@@ -98,6 +98,8 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
combination_method_ = loadParam(layer, "combination_method", 0.0);
obstacle_decay_time_ = loadParam(layer, "obstacle_decay_time", 0.0);
frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0);
int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
if (nh.hasParam("enabled"))
@@ -125,6 +127,18 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
}
if (nh.hasParam("combination_method"))
nh.getParam("combination_method", combination_method_);
if (nh.hasParam("obstacle_decay_time"))
nh.getParam("obstacle_decay_time", obstacle_decay_time_);
if (nh.hasParam("frustum_skip_distance"))
nh.getParam("frustum_skip_distance", frustum_skip_distance_);
if (obstacle_decay_time_ > 0.0)
{
robot::log_info(
"VoxelLayer obstacle decay enabled: LETHAL cells not re-observed for %.1f s are freed. "
"Intended for camera-only costmaps; decay clears obstacles outside the current FOV.\n",
obstacle_decay_time_);
}
this->matchSize();
}
@@ -140,6 +154,7 @@ void VoxelLayer::matchSize()
{
ObstacleLayer::matchSize();
robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_))
{
std::cerr << "[FATAL] Voxel grid size mismatch: "
@@ -161,6 +176,7 @@ void VoxelLayer::resetMaps()
{
Costmap2D::resetMaps();
robot_voxel_grid_.reset();
cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
}
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@@ -197,6 +213,8 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
}
// place the new obstacles into a priority queue... each with a priority of zero to begin with
const double now_sec =
obstacle_decay_time_ > 0.0 ? robot::Time::now().toSec() : 0.0;
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const Observation& obs = *it;
@@ -242,13 +260,56 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
if (obstacle_decay_time_ > 0.0)
cell_last_marked_[index] = now_sec;
touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
}
}
}
if (obstacle_decay_time_ > 0.0)
decayStaleObstacles(now_sec, min_x, min_y, max_x, max_y);
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
void VoxelLayer::decayStaleObstacles(double now_sec, double* min_x, double* min_y,
double* max_x, double* max_y)
{
const std::size_t map_size = static_cast<std::size_t>(size_x_) * size_y_;
if (cell_last_marked_.size() != map_size)
cell_last_marked_.assign(map_size, 0.0);
for (std::size_t index = 0; index < map_size; ++index)
{
if (costmap_[index] != LETHAL_OBSTACLE)
continue;
double& stamp = cell_last_marked_[index];
// A LETHAL cell without a stamp was marked before decay tracking covered
// it (e.g. right after a resize). Start its timer now instead of freeing
// an obstacle we have no age evidence for.
if (stamp <= 0.0)
{
stamp = now_sec;
continue;
}
if (now_sec - stamp <= obstacle_decay_time_)
continue;
costmap_[index] = FREE_SPACE;
robot_voxel_grid_.clearVoxelColumn(static_cast<unsigned int>(index));
stamp = 0.0;
unsigned int mx, my;
indexToCells(static_cast<unsigned int>(index), mx, my);
double wx, wy;
mapToWorld(mx, my, wx, wy);
touch(wx, wy, min_x, min_y, max_x, max_y);
}
}
void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
{
// get the cell coordinates of the center point of the window
@@ -618,7 +679,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
const unsigned int step = std::max(1u, observation.pixel_step_);
const double min_range = observation.min_range_;
const double max_range = observation.max_range_;
const double skip_dist = 2.0 * resolution_;
const double skip_dist =
frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_;
const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
updateDepthRayCache(width, height, step, fx, fy, cx, cy);
@@ -715,8 +777,10 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
rolling_costmap_scratch_.resize(overlap_size);
rolling_voxel_scratch_.resize(overlap_size);
rolling_stamp_scratch_.resize(overlap_size);
unsigned char* local_map = rolling_costmap_scratch_.data();
unsigned int* local_voxel_map = rolling_voxel_scratch_.data();
double* local_stamp_map = rolling_stamp_scratch_.data();
unsigned int* voxel_map = robot_voxel_grid_.getData();
if (overlap_size > 0)
@@ -725,6 +789,8 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
cell_size_x, cell_size_x, cell_size_y);
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0,
cell_size_x, cell_size_x, cell_size_y);
copyMapRegion(cell_last_marked_.data(), lower_left_x, lower_left_y, size_x_,
local_stamp_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
}
// we'll reset our maps to unknown space if appropriate
@@ -745,6 +811,8 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
size_x_, cell_size_x, cell_size_y);
copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y,
size_x_, cell_size_x, cell_size_y);
copyMapRegion(local_stamp_map, 0, 0, cell_size_x, cell_last_marked_.data(),
start_x, start_y, size_x_, cell_size_x, cell_size_y);
}
}