From 28a32ee67cd42da8718b46d837cb556759d92c9e Mon Sep 17 00:00:00 2001 From: duongtd Date: Thu, 23 Jul 2026 11:18:04 +0700 Subject: [PATCH] optimal voxel layer --- include/robot_costmap_2d/voxel_layer.h | 16 ++++++ plugins/voxel_layer.cpp | 70 +++++++++++++++++++++++++- 2 files changed, 85 insertions(+), 1 deletion(-) diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index 1601dc4..8fcb9b9 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -102,15 +102,31 @@ private: bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz, double raytrace_range, unsigned int cell_raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y); + /// Frees LETHAL cells this layer marked that have not been re-marked within + /// obstacle_decay_time. Handles ghost cells frustum clearing can never + /// reach: cells that left the camera FOV, occlusion shadows, and cells + /// inside the skip band in front of a measured surface. + void decayStaleObstacles(double now_sec, double* min_x, double* min_y, + double* max_x, double* max_y); bool publish_voxel_; robot_voxel_grid::VoxelGrid robot_voxel_grid_; double z_resolution_, origin_z_; unsigned int unknown_threshold_, mark_threshold_, size_z_; + /// Seconds a marked cell survives without being re-observed before it is + /// freed. <= 0 disables decay (default). Only enable on camera-only + /// costmaps: decay clears obstacles the sensor cannot currently see. + double obstacle_decay_time_{0.0}; + /// Clearing rays stop this far [m] before the measured surface. < 0 keeps + /// the legacy 2 * resolution behaviour. + double frustum_skip_distance_{-1.0}; + /// Per-cell robot::Time seconds of the last marking; 0 = no valid stamp. + std::vector cell_last_marked_; robot_sensor_msgs::PointCloud clearing_endpoints_; std::vector rolling_costmap_scratch_; std::vector rolling_voxel_scratch_; + std::vector rolling_stamp_scratch_; struct DepthRay { diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index f21d910..d93924f 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -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(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(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::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(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(index)); + stamp = 0.0; + + unsigned int mx, my; + indexToCells(static_cast(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(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); } }