optimal voxel layer

config/voxel_layer_params.yaml

@@ -7,4 +7,9 @@ voxel_layer:
   z_voxels: 16
   unknown_threshold: 15.0
   mark_threshold: 0
-  combination_method: 1
+  combination_method: 1
+  frustum_clearing_enabled: true
+  frustum_clearing_pixel_step: 8
+  frustum_min_range: 0.20
+  frustum_max_range: 3.0
+  frustum_depth_camera_topic: /camera/depth/data

include/robot_costmap_2d/observation.h

@@ -48,48 +48,16 @@ namespace robot_costmap_2d
  * The message is shared so returning buffered observations does not copy the
  * full depth image on every costmap update.
  */
-/// Per-observation-source configuration of the depth-image frustum clearing,
-/// loaded by ObstacleLayer from the source's YAML/ROS params and carried with
-/// each DepthCameraObservation.
-struct DepthFrustumConfig
-{
-  unsigned int pixel_step = 0;
-  double min_range = 0.0;
-  double max_range = 0.0;
-  /// 3D clearing rays stop this far [m] before the measured surface.
-  /// Negative: legacy 2 * costmap resolution.
-  double skip_distance = -1.0;
-  /// Full-column clearing from the per-pixel-column nearest in-band return.
-  bool column_clearing = false;
-  /// Height band [m] used for the in-band test; floor returns below min do
-  /// not shorten the beam. max < 0: use the layer max_obstacle_height.
-  double column_min_height = 0.10;
-  double column_max_height = -1.0;
-  /// Column beams stop this far [m] before the nearest in-band return.
-  double column_skip_distance = 0.02;
-  /// Full columns are only cleared beyond this distance [m]. Negative:
-  /// derive each frame from the camera intrinsics and mounting pose.
-  double column_cover_distance = -1.0;
-  /// Depth-image columns at the LEFT edge excluded from clearing [px]. Covers
-  /// the stereo no-disparity strip that is permanently invalid there: those
-  /// pixels carry no free-space evidence, so clearing through them erases
-  /// obstacles that rotate out of the FOV on that side. Set to the measured
-  /// width of the black strip in the raw depth image (a few px margin). 0
-  /// disables. Invalid pixels ELSEWHERE still clear (needed for ghost removal).
-  unsigned int clear_left_border_px = 0;
-  /// Same as clear_left_border_px but for the RIGHT edge, for cameras whose
-  /// stereo no-disparity strip sits on the right instead of the left. Measured
-  /// from the last image column inward. 0 disables.
-  unsigned int clear_right_border_px = 0;
-};
-
 class DepthCameraObservation
 {
 public:
   DepthCameraObservation()
     : data_handle_(),
       data_(nullptr),
-      topic_()
+      topic_(),
+      pixel_step_(0),
+      min_range_(0.0),
+      max_range_(0.0)
   {
   }
 
@@ -97,12 +65,16 @@ public:
       const robot_sensor_msgs::DepthCameraData& data,
       std::string topic,
       const robot::Time& received_time,
-      const DepthFrustumConfig& frustum)
+      unsigned int pixel_step,
+      double min_range,
+      double max_range)
     : data_handle_(boost::make_shared<robot_sensor_msgs::DepthCameraData>(data)),
       data_(data_handle_.get()),
       topic_(std::move(topic)),
       received_time_(received_time),
-      frustum_(frustum)
+      pixel_step_(pixel_step),
+      min_range_(min_range),
+      max_range_(max_range)
   {
   }
 
@@ -110,12 +82,16 @@ public:
       robot_sensor_msgs::DepthCameraData::ConstPtr data,
       std::string topic,
       const robot::Time& received_time,
-      const DepthFrustumConfig& frustum)
+      unsigned int pixel_step,
+      double min_range,
+      double max_range)
     : data_handle_(std::move(data)),
       data_(data_handle_.get()),
       topic_(std::move(topic)),
       received_time_(received_time),
-      frustum_(frustum)
+      pixel_step_(pixel_step),
+      min_range_(min_range),
+      max_range_(max_range)
   {
   }
 
@@ -124,7 +100,9 @@ public:
       data_(data_handle_.get()),
       topic_(other.topic_),
       received_time_(other.received_time_),
-      frustum_(other.frustum_)
+      pixel_step_(other.pixel_step_),
+      min_range_(other.min_range_),
+      max_range_(other.max_range_)
   {
   }
 
@@ -133,10 +111,14 @@ public:
       data_(data_handle_.get()),
       topic_(std::move(other.topic_)),
       received_time_(other.received_time_),
-      frustum_(other.frustum_)
+      pixel_step_(other.pixel_step_),
+      min_range_(other.min_range_),
+      max_range_(other.max_range_)
   {
     other.data_ = nullptr;
-    other.frustum_ = DepthFrustumConfig();
+    other.pixel_step_ = 0;
+    other.min_range_ = 0.0;
+    other.max_range_ = 0.0;
   }
 
   DepthCameraObservation& operator=(const DepthCameraObservation& other)
@@ -148,7 +130,9 @@ public:
     data_ = data_handle_.get();
     topic_ = other.topic_;
     received_time_ = other.received_time_;
-    frustum_ = other.frustum_;
+    pixel_step_ = other.pixel_step_;
+    min_range_ = other.min_range_;
+    max_range_ = other.max_range_;
     return *this;
   }
 
@@ -161,10 +145,14 @@ public:
     data_ = data_handle_.get();
     topic_ = std::move(other.topic_);
     received_time_ = other.received_time_;
-    frustum_ = other.frustum_;
+    pixel_step_ = other.pixel_step_;
+    min_range_ = other.min_range_;
+    max_range_ = other.max_range_;
 
     other.data_ = nullptr;
-    other.frustum_ = DepthFrustumConfig();
+    other.pixel_step_ = 0;
+    other.min_range_ = 0.0;
+    other.max_range_ = 0.0;
 
     return *this;
   }
@@ -175,7 +163,9 @@ public:
   const robot_sensor_msgs::DepthCameraData* data_;
   std::string topic_;
   robot::Time received_time_;
-  DepthFrustumConfig frustum_;
+  unsigned int pixel_step_;
+  double min_range_;
+  double max_range_;
 };
 
 /**

include/robot_costmap_2d/observation_buffer.h

@@ -78,8 +78,8 @@ public:
   
   ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
-                    double raytrace_range, const DepthFrustumConfig& frustum_config,
-                    tf3::BufferCore& tf3_buffer, std::string global_frame,
+                    double raytrace_range, unsigned int frustum_pixel_step, double frustum_min_range,
+                    double frustum_max_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
                     std::string sensor_frame, double tf_tolerance);
 
 
@@ -171,7 +171,9 @@ private:
   boost::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
   double obstacle_range_, raytrace_range_;
   double tf_tolerance_;
-  DepthFrustumConfig frustum_config_;
+  unsigned int frustum_pixel_step_;
+  double frustum_min_range_;
+  double frustum_max_range_;
 };
 }  // namespace robot_costmap_2d
 #endif  // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

include/robot_costmap_2d/voxel_layer.h

@@ -51,9 +51,6 @@
 #include <robot_costmap_2d/obstacle_layer.h>
 #include <robot_voxel_grid/voxel_grid.h>
 
-#include <limits>
-#include <vector>
-
 namespace robot_costmap_2d
 {
 
@@ -100,51 +97,47 @@ private:
   bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
                        double& depth_m, bool& is_valid) const;
   void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
-                           double fx, double fy, double cx, double cy);
+                           double fx, double fy, double cx, double cy,
+                           unsigned int u_offset, unsigned int v_offset);
   bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
   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);
-  bool clearDepthColumns(double ox, double oy, double cover_distance, double far_distance,
-                         double min_range, double max_range, double skip_dist,
-                         double* min_x, double* min_y, double* max_x, double* max_y);
-  bool clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const;
+  /// 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_;
-  /// Scratch for the full-column clearing pass (config lives per observation
-  /// source in DepthFrustumConfig): per depth-image pixel column, the nearest
-  /// return inside the obstacle height band certifies "no obstacle in this
-  /// direction closer than d". Cells along that 2D beam get their whole voxel
-  /// column cleared, removing marked voxels the per-pixel 3D rays cannot
-  /// reach (above the vertical FOV at close range).
-  struct DepthColumnStat
-  {
-    double min_band_dist = -1.0;  ///< horizontal distance of nearest in-band return; < 0 = none
-    double azimuth = 0.0;         ///< beam direction in the global frame
-    double best_row_delta = std::numeric_limits<double>::infinity();
-    bool has_ray = false;         ///< column had at least one readable pixel
-    bool in_border = false;       ///< column lies in a left/right no-disparity strip
-  };
-  std::vector<DepthColumnStat> depth_column_stats_;
   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<double> cell_last_marked_;
   robot_sensor_msgs::PointCloud clearing_endpoints_;
   std::vector<unsigned char> rolling_costmap_scratch_;
   std::vector<unsigned int> rolling_voxel_scratch_;
+  std::vector<double> rolling_stamp_scratch_;
 
   struct DepthRay
   {
     unsigned int u;
     unsigned int v;
-    unsigned int col;  ///< pixel-column index in the cache (border column included)
     double x;
     double y;
     double z;
   };
   std::vector<DepthRay> depth_ray_cache_;
-  unsigned int cached_column_count_ = 0;
   unsigned int cached_depth_width_ = 0;
   unsigned int cached_depth_height_ = 0;
   unsigned int cached_depth_pixel_step_ = 0;
@@ -152,6 +145,14 @@ private:
   double cached_fy_ = 0.0;
   double cached_cx_ = 0.0;
   double cached_cy_ = 0.0;
+  unsigned int cached_u_offset_ = 0;
+  unsigned int cached_v_offset_ = 0;
+  /// Advances every frustum pass to dither the sampled pixel grid. A static
+  /// camera otherwise re-traces the same fixed pixel_step subgrid each frame,
+  /// so cells between adjacent rays (angular gap pixel_step / fx) are never
+  /// crossed and stay marked until the robot moves. Cycling the grid phase
+  /// sweeps every pixel over pixel_step^2 frames at unchanged per-frame cost.
+  unsigned int frustum_phase_ = 0;
 
   inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
   {

plugins/obstacle_layer.cpp

@@ -133,7 +133,8 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
         bool inf_is_valid = false, clearing=false, marking=true;
         bool frustum_clearing_enabled = false;
         int frustum_pixel_step = 8;
-        DepthFrustumConfig frustum_config;
+        double frustum_min_range = 0.2;
+        double frustum_max_range = 3.0;
 
         robot::NodeHandle priv_nh(nh, source);
         topic = loadParam(layer[source],"topic", topic);
@@ -146,34 +147,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
         inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
         clearing = loadParam(layer[source],"clearing", false);
         marking = loadParam(layer[source],"marking", true);
-        // frustum params are per-source; the layer-level key is kept as a
-        // fallback for older YAMLs
-        frustum_clearing_enabled = loadParam(layer[source], "frustum_clearing_enabled",
-                                             loadParam(layer, "frustum_clearing_enabled", false));
-        frustum_pixel_step = loadParam(layer[source], "frustum_clearing_pixel_step",
-                                       loadParam(layer, "frustum_clearing_pixel_step", 8));
-        frustum_config.min_range = loadParam(layer[source], "frustum_min_range",
-                                             loadParam(layer, "frustum_min_range", 0.2));
-        frustum_config.max_range = loadParam(layer[source], "frustum_max_range",
-                                             loadParam(layer, "frustum_max_range", 3.0));
-        frustum_config.skip_distance =
-            loadParam(layer[source], "frustum_skip_distance", frustum_config.skip_distance);
-        frustum_config.column_clearing =
-            loadParam(layer[source], "frustum_column_clearing", frustum_config.column_clearing);
-        frustum_config.column_min_height =
-            loadParam(layer[source], "column_clear_min_height", frustum_config.column_min_height);
-        frustum_config.column_max_height =
-            loadParam(layer[source], "column_clear_max_height", frustum_config.column_max_height);
-        frustum_config.column_skip_distance =
-            loadParam(layer[source], "column_skip_distance", frustum_config.column_skip_distance);
-        frustum_config.column_cover_distance =
-            loadParam(layer[source], "column_cover_distance", frustum_config.column_cover_distance);
-        int frustum_clear_left_border =
-            loadParam(layer[source], "frustum_clear_left_border_px",
-                      loadParam(layer, "frustum_clear_left_border_px", 0));
-        int frustum_clear_right_border =
-            loadParam(layer[source], "frustum_clear_right_border_px",
-                      loadParam(layer, "frustum_clear_right_border_px", 0));
+        frustum_clearing_enabled = loadParam(layer, "frustum_clearing_enabled", false);
+        frustum_pixel_step = loadParam(layer, "frustum_clearing_pixel_step", 8);
+        frustum_min_range = loadParam(layer, "frustum_min_range", 0.2);
+        frustum_max_range = loadParam(layer, "frustum_max_range", 3.0);
 
         if (priv_nh.hasParam("topic"))
           priv_nh.getParam("topic", topic);
@@ -203,44 +180,13 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
           frustum_pixel_step = std::max(1, frustum_pixel_step);
         }
         if (priv_nh.hasParam("frustum_min_range"))
-          priv_nh.getParam("frustum_min_range", frustum_config.min_range);
+          priv_nh.getParam("frustum_min_range", frustum_min_range);
         if (priv_nh.hasParam("frustum_max_range"))
-          priv_nh.getParam("frustum_max_range", frustum_config.max_range);
-        if (priv_nh.hasParam("frustum_skip_distance"))
-          priv_nh.getParam("frustum_skip_distance", frustum_config.skip_distance);
-        if (priv_nh.hasParam("frustum_column_clearing"))
-          priv_nh.getParam("frustum_column_clearing", frustum_config.column_clearing);
-        if (priv_nh.hasParam("column_clear_min_height"))
-          priv_nh.getParam("column_clear_min_height", frustum_config.column_min_height);
-        if (priv_nh.hasParam("column_clear_max_height"))
-          priv_nh.getParam("column_clear_max_height", frustum_config.column_max_height);
-        if (priv_nh.hasParam("column_skip_distance"))
-          priv_nh.getParam("column_skip_distance", frustum_config.column_skip_distance);
-        if (priv_nh.hasParam("column_cover_distance"))
-          priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance);
-        if (priv_nh.hasParam("frustum_clear_left_border_px"))
-          priv_nh.getParam("frustum_clear_left_border_px", frustum_clear_left_border);
-        if (priv_nh.hasParam("frustum_clear_right_border_px"))
-          priv_nh.getParam("frustum_clear_right_border_px", frustum_clear_right_border);
+          priv_nh.getParam("frustum_max_range", frustum_max_range);
         if (priv_nh.hasParam("frustum_depth_camera_topic"))
           priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
 
-        frustum_config.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
-        frustum_config.clear_left_border_px =
-            static_cast<unsigned int>(std::max(0, frustum_clear_left_border));
-        frustum_config.clear_right_border_px =
-            static_cast<unsigned int>(std::max(0, frustum_clear_right_border));
-
-        robot::log_info("source %s: frustum_clearing_enabled: %s, pixel_step: %u, range: [%.2f, %.2f] m, "
-                        "skip: %.3f m, column_clearing: %s, column_band: [%.2f, %.2f] m, "
-                        "column_skip: %.3f m, column_cover: %.2f m, clear_left_border_px: %u px, "
-                        "clear_right_border_px: %u px\n",
-                        source.c_str(), frustum_clearing_enabled ? "true" : "false",
-                        frustum_config.pixel_step, frustum_config.min_range, frustum_config.max_range,
-                        frustum_config.skip_distance, frustum_config.column_clearing ? "true" : "false",
-                        frustum_config.column_min_height, frustum_config.column_max_height,
-                        frustum_config.column_skip_distance, frustum_config.column_cover_distance,
-                        frustum_config.clear_left_border_px, frustum_config.clear_right_border_px);
+        robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_pixel_step, frustum_min_range, frustum_max_range);
 
         double obstacle_range = 2.5;
         obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
@@ -298,8 +244,8 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
           depth_observation_buffers_.push_back(
               boost::shared_ptr < ObservationBuffer
                   > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
-                                          max_obstacle_height, obstacle_range, raytrace_range, frustum_config,
-                                          *tf_, global_frame_,
+                                          max_obstacle_height, obstacle_range, raytrace_range, frustum_pixel_step,
+                                          frustum_min_range, frustum_max_range, *tf_, global_frame_,
                                           sensor_frame, transform_tolerance)));
           
         }

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<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
@@ -458,11 +519,13 @@ bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned
 
 void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
                                      unsigned int pixel_step, double fx, double fy,
-                                     double cx, double cy)
+                                     double cx, double cy,
+                                     unsigned int u_offset, unsigned int v_offset)
 {
   if (cached_depth_width_ == width && cached_depth_height_ == height &&
       cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy &&
-      cached_cx_ == cx && cached_cy_ == cy)
+      cached_cx_ == cx && cached_cy_ == cy &&
+      cached_u_offset_ == u_offset && cached_v_offset_ == v_offset)
   {
     return;
   }
@@ -474,36 +537,23 @@ void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
   cached_fy_ = fy;
   cached_cx_ = cx;
   cached_cy_ = cy;
+  cached_u_offset_ = u_offset;
+  cached_v_offset_ = v_offset;
 
-  // Sample every pixel_step-th row/column and always include the last image
-  // row/column, so cells marked from border pixels stay inside the swept
-  // clearing fan.
-  std::vector<unsigned int> u_samples, v_samples;
-  u_samples.reserve(width / pixel_step + 2);
-  v_samples.reserve(height / pixel_step + 2);
-  for (unsigned int u = 0; u < width; u += pixel_step)
-    u_samples.push_back(u);
-  if (width > 0 && u_samples.back() != width - 1)
-    u_samples.push_back(width - 1);
-  for (unsigned int v = 0; v < height; v += pixel_step)
-    v_samples.push_back(v);
-  if (height > 0 && v_samples.back() != height - 1)
-    v_samples.push_back(height - 1);
-
-  cached_column_count_ = static_cast<unsigned int>(u_samples.size());
+  const std::size_t rows = (height + pixel_step - 1) / pixel_step;
+  const std::size_t columns = (width + pixel_step - 1) / pixel_step;
   depth_ray_cache_.clear();
-  depth_ray_cache_.reserve(u_samples.size() * v_samples.size());
+  depth_ray_cache_.reserve(rows * columns);
 
-  for (const unsigned int v : v_samples)
+  for (unsigned int v = v_offset; v < height; v += pixel_step)
   {
-    for (unsigned int col = 0; col < u_samples.size(); ++col)
+    for (unsigned int u = u_offset; u < width; u += pixel_step)
     {
-      const unsigned int u = u_samples[col];
       const double x = (static_cast<double>(u) - cx) / fx;
       const double y = (static_cast<double>(v) - cy) / fy;
       const double inverse_norm = 1.0 / std::sqrt(x * x + y * y + 1.0);
       depth_ray_cache_.push_back(
-          DepthRay{u, v, col, x * inverse_norm, y * inverse_norm, inverse_norm});
+          DepthRay{u, v, x * inverse_norm, y * inverse_norm, inverse_norm});
     }
   }
 }
@@ -602,33 +652,18 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
   local_origin.point.y = 0.0;
   local_origin.point.z = 0.0;
 
-  // Look up the sensor pose at the depth image's CAPTURE time, not the latest
-  // transform. The costmap update runs later than the frame was captured, so
-  // during rotation the latest pose orients the clearing frustum where the depth
-  // pixels were never measured from; the fan's free rays then sweep across and
-  // erase freshly marked cells, and the trailing side that gets erased flips
-  // with rotation direction. A stamped lookup keeps the frustum geometrically
-  // consistent with its own pixels. If the transform at that stamp is
-  // unavailable (stale / would extrapolate), skip clearing this cycle instead of
-  // clearing from a wrong pose. Falls back to latest only when the frame carries
-  // no stamp.
-  const robot::Time& depth_stamp = local_origin.header.stamp;
-  const tf3::Time query_time =
-      depth_stamp.isZero() ? tf3::Time() : tf3::Time(depth_stamp.sec, depth_stamp.nsec);
-
   robot_geometry_msgs::PointStamped global_origin;
   tf3::TransformStampedMsg tfm;
   try
   {
-    tfm = tf_->lookupTransform(global_frame_, depth_frame, query_time);
+    tfm = tf_->lookupTransform(global_frame_, depth_frame, tf3::Time());
     tf3::doTransform(local_origin, global_origin, tfm);
   }
   catch (tf3::TransformException& ex)
   {
     robot::log_error_throttle(
-        5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s at t=%.3f: %s\n",
-        observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(),
-        query_time.toSec(), ex.what());
+        5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s: %s\n",
+        observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(), ex.what());
     return false;
   }
 
@@ -645,15 +680,23 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     return false;
   }
 
-  const DepthFrustumConfig& frustum = observation.frustum_;
-  const unsigned int step = std::max(1u, frustum.pixel_step);
-  const double min_range = frustum.min_range;
-  const double max_range = frustum.max_range;
+  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 =
-      frustum.skip_distance >= 0.0 ? frustum.skip_distance : 2.0 * resolution_;
+      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);
+
+  // Dither the sampled pixel grid every pass. With a fixed subgrid a static
+  // camera re-traces identical rays each frame; cells between adjacent rays
+  // (angular gap step / fx, ~5 cm at 2.5 m for step 8) are never crossed and
+  // stay marked until the robot moves. Cycling the phase sweeps every pixel
+  // column/row over step^2 frames at the same per-frame ray count.
+  const unsigned int u_offset = step > 1 ? frustum_phase_ % step : 0;
+  const unsigned int v_offset = step > 1 ? (frustum_phase_ / step) % step : 0;
+  ++frustum_phase_;
+  updateDepthRayCache(width, height, step, fx, fy, cx, cy, u_offset, v_offset);
 
   double qx = tfm.transform.rotation.x;
   double qy = tfm.transform.rotation.y;
@@ -679,64 +722,6 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
   const unsigned int cell_raytrace_range = cellDistance(max_range);
   bool cleared_any = false;
 
-  // Column clearing: certify the beam length per pixel column and the distance
-  // window [cover, far] where the vertical FOV spans the whole height band.
-  // Outside that window a real obstacle could sit above/below the FOV, so only
-  // the per-pixel 3D rays may clear there.
-  const double band_min_h = frustum.column_min_height;
-  const double band_max_h =
-      frustum.column_max_height >= 0.0 ? frustum.column_max_height : max_obstacle_height_;
-  double cover_dist = frustum.column_cover_distance;
-  double far_dist = std::numeric_limits<double>::infinity();
-  bool column_pass = frustum.column_clearing && band_max_h > band_min_h;
-
-  if (column_pass && cover_dist < 0.0)
-  {
-    const double up_half = std::atan2(cy, fy);
-    const double down_half = std::atan2(static_cast<double>(height) - 1.0 - cy, fy);
-    const double axis_elev = std::atan2(r22, std::hypot(r02, r12));
-    const double alpha_top = axis_elev + up_half;
-    const double alpha_bot = axis_elev - down_half;
-    const double band_top = band_max_h - oz;
-    const double band_bot = band_min_h - oz;
-    constexpr double kMinSlope = 1e-3;
-
-    cover_dist = 0.0;
-    if (band_top > 0.0)
-    {
-      if (alpha_top <= kMinSlope)
-        column_pass = false;  // camera can never look up to the band top
-      else
-        cover_dist = std::max(cover_dist, band_top / std::tan(alpha_top));
-    }
-    else if (alpha_top < -kMinSlope)
-    {
-      far_dist = std::min(far_dist, band_top / std::tan(alpha_top));
-    }
-    if (band_bot < 0.0)
-    {
-      if (alpha_bot >= -kMinSlope)
-        column_pass = false;  // camera can never look down to the band bottom
-      else
-        cover_dist = std::max(cover_dist, band_bot / std::tan(alpha_bot));
-    }
-    else if (alpha_bot > kMinSlope)
-    {
-      far_dist = std::min(far_dist, band_bot / std::tan(alpha_bot));
-    }
-
-    if (!column_pass)
-    {
-      robot::log_warning_throttle(
-          10.0, "VoxelLayer column clearing disabled: vertical FOV [%.1f, %.1f] deg at camera "
-          "height %.2f m never covers band [%.2f, %.2f] m\n",
-          alpha_bot * 180.0 / M_PI, alpha_top * 180.0 / M_PI, oz, band_min_h, band_max_h);
-    }
-  }
-
-  if (column_pass)
-    depth_column_stats_.assign(cached_column_count_, DepthColumnStat());
-
   for (const DepthRay& local_ray : depth_ray_cache_)
   {
     double depth_m = 0.0;
@@ -744,73 +729,18 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
       continue;
 
-    // Edge stereo no-disparity strips (left and/or right, depending on the
-    // camera). An INVALID pixel in such a strip is structurally invalid (carries
-    // no free-space evidence), so clearing it out to max_range erases obstacles
-    // rotating out of the FOV on that side (the turn bug) — skip only those. A
-    // VALID return there is a real measured surface, so it must still clear
-    // normally; otherwise the border becomes a clearing dead zone and obstacles
-    // there never get cleared. Invalid pixels OUTSIDE the strips still clear to
-    // max_range (ghost removal). Right edge measured inward from the last column;
-    // the unsigned test avoids underflow when the border exceeds the width.
-    const bool in_left_border = local_ray.u < frustum.clear_left_border_px;
-    const bool in_right_border =
-        frustum.clear_right_border_px > 0 &&
-        local_ray.u + frustum.clear_right_border_px >= width;
-    const bool in_border = in_left_border || in_right_border;
-    if (in_border && !valid)
-      continue;
+    double ray_len = max_range;
+    if (valid && depth_m < max_range)
+      ray_len = std::max(0.0, depth_m - skip_dist);
 
-    // depth images store z-depth; local_ray.z is the unit ray's optical axis
-    // component, so depth / z is the Euclidean range
-    const double euclid_range = valid ? depth_m / local_ray.z : 0.0;
+    if (ray_len <= min_range)
+      continue;
 
     robot_geometry_msgs::Vector3 global_ray;
     global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
     global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
     global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
 
-    if (column_pass && local_ray.col < depth_column_stats_.size())
-    {
-      const double horiz_norm = std::hypot(global_ray.x, global_ray.y);
-      if (horiz_norm > 1e-6)
-      {
-        DepthColumnStat& stat = depth_column_stats_[local_ray.col];
-        const double row_delta = std::fabs(static_cast<double>(local_ray.v) - cy);
-        if (stat.min_band_dist < 0.0 && row_delta < stat.best_row_delta)
-        {
-          // no in-band return yet: aim the beam along the ray nearest the
-          // principal row
-          stat.azimuth = std::atan2(global_ray.y, global_ray.x);
-          stat.best_row_delta = row_delta;
-        }
-        stat.has_ray = true;
-        if (in_border)
-          stat.in_border = true;
-
-        if (valid)
-        {
-          const double pz = oz + global_ray.z * euclid_range;
-          if (pz >= band_min_h && pz <= band_max_h)
-          {
-            const double dist_h = horiz_norm * euclid_range;
-            if (stat.min_band_dist < 0.0 || dist_h < stat.min_band_dist)
-            {
-              stat.min_band_dist = dist_h;
-              stat.azimuth = std::atan2(global_ray.y, global_ray.x);
-            }
-          }
-        }
-      }
-    }
-
-    double ray_len = max_range;
-    if (valid && euclid_range < max_range)
-      ray_len = std::max(0.0, euclid_range - skip_dist);
-
-    if (ray_len <= min_range)
-      continue;
-
     const double sx = ox + global_ray.x * min_range;
     const double sy = oy + global_ray.y * min_range;
     const double sz = oz + global_ray.z * min_range;
@@ -822,126 +752,6 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
                                 min_x, min_y, max_x, max_y) || cleared_any;
   }
 
-  if (column_pass)
-  {
-    cleared_any = clearDepthColumns(ox, oy, cover_dist, far_dist, min_range, max_range,
-                                    std::max(0.0, frustum.column_skip_distance),
-                                    min_x, min_y, max_x, max_y) ||
-                  cleared_any;
-  }
-
-  return cleared_any;
-}
-
-namespace
-{
-/// raytraceLine action: frees the 2D cell and wipes its whole voxel column.
-class ClearFullColumn
-{
-public:
-  ClearFullColumn(unsigned char* costmap, robot_voxel_grid::VoxelGrid& voxel_grid)
-    : costmap_(costmap), voxel_grid_(voxel_grid)
-  {
-  }
-
-  inline void operator()(unsigned int offset)
-  {
-    costmap_[offset] = FREE_SPACE;
-    voxel_grid_.clearVoxelColumn(offset);
-  }
-
-private:
-  unsigned char* costmap_;
-  robot_voxel_grid::VoxelGrid& voxel_grid_;
-};
-}  // namespace
-
-bool VoxelLayer::clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const
-{
-  // Liang-Barsky clip against the map interior; the half-resolution margin
-  // keeps clipped endpoints valid for worldToMap.
-  const double min_wx = origin_x_;
-  const double min_wy = origin_y_;
-  const double max_wx = origin_x_ + getSizeInMetersX() - 0.5 * resolution_;
-  const double max_wy = origin_y_ + getSizeInMetersY() - 0.5 * resolution_;
-  const double dx = ex - sx;
-  const double dy = ey - sy;
-  const double p[4] = {-dx, dx, -dy, dy};
-  const double q[4] = {sx - min_wx, max_wx - sx, sy - min_wy, max_wy - sy};
-
-  double t0 = 0.0;
-  double t1 = 1.0;
-  for (int i = 0; i < 4; ++i)
-  {
-    if (std::fabs(p[i]) < 1e-12)
-    {
-      if (q[i] < 0.0)
-        return false;
-      continue;
-    }
-    const double r = q[i] / p[i];
-    if (p[i] < 0.0)
-      t0 = std::max(t0, r);
-    else
-      t1 = std::min(t1, r);
-  }
-  if (t0 > t1)
-    return false;
-
-  const double bx = sx;
-  const double by = sy;
-  sx = bx + t0 * dx;
-  sy = by + t0 * dy;
-  ex = bx + t1 * dx;
-  ey = by + t1 * dy;
-  return true;
-}
-
-bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance,
-                                   double far_distance, double min_range, double max_range,
-                                   double skip_dist, double* min_x, double* min_y,
-                                   double* max_x, double* max_y)
-{
-  const double start_dist = std::max(cover_distance, min_range);
-  bool cleared_any = false;
-
-  for (const DepthColumnStat& stat : depth_column_stats_)
-  {
-    if (!stat.has_ray)
-      continue;
-
-    // In an edge stereo strip, never clear a whole column out to max_range on a
-    // missing in-band return: that is exactly the no-free-space-evidence case that
-    // erases obstacles turning out of view. Only an in-band measured surface may
-    // shorten (and thus clear) a border column.
-    if (stat.in_border && stat.min_band_dist < 0.0)
-      continue;
-
-    double end_dist = stat.min_band_dist >= 0.0 ? stat.min_band_dist - skip_dist : max_range;
-    end_dist = std::min(std::min(end_dist, max_range), far_distance);
-    if (end_dist <= start_dist)
-      continue;
-
-    const double cos_az = std::cos(stat.azimuth);
-    const double sin_az = std::sin(stat.azimuth);
-    double sx = ox + cos_az * start_dist;
-    double sy = oy + sin_az * start_dist;
-    double ex = ox + cos_az * end_dist;
-    double ey = oy + sin_az * end_dist;
-    if (!clipColumnSegment(sx, sy, ex, ey))
-      continue;
-
-    unsigned int sx_m, sy_m, ex_m, ey_m;
-    if (!worldToMap(sx, sy, sx_m, sy_m) || !worldToMap(ex, ey, ex_m, ey_m))
-      continue;
-
-    ClearFullColumn clearer(costmap_, robot_voxel_grid_);
-    raytraceLine(clearer, sx_m, sy_m, ex_m, ey_m);
-    touch(sx, sy, min_x, min_y, max_x, max_y);
-    touch(ex, ey, min_x, min_y, max_x, max_y);
-    cleared_any = true;
-  }
-
   return cleared_any;
 }
 
@@ -980,8 +790,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)
@@ -990,6 +802,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
@@ -1010,6 +824,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);
   }
 
 }

src/costmap_2d_robot.cpp

@@ -400,14 +400,6 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
         int frustum_clearing_pixel_step = 8;
         double frustum_min_range = 0.2;
         double frustum_max_range = 3.0;
-        double frustum_skip_distance = -1.0;
-        bool frustum_column_clearing = false;
-        double column_clear_min_height = 0.10;
-        double column_clear_max_height = -1.0;
-        double column_skip_distance = 0.02;
-        double column_cover_distance = -1.0;
-        int frustum_clear_left_border_px = 0;
-        int frustum_clear_right_border_px = 0;
 
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
@@ -425,16 +417,8 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_skip_distance", frustum_skip_distance);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_column_clearing", frustum_column_clearing);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_min_height", column_clear_min_height);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_max_height", column_clear_max_height);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_left_border_px", frustum_clear_left_border_px);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_right_border_px", frustum_clear_right_border_px);
         robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
-        robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f, frustum_clear_left_border_px: %d, frustum_clear_right_border_px: %d", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range, frustum_clear_left_border_px, frustum_clear_right_border_px);
+        robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
       }
     }
   }
@@ -475,14 +459,6 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
         int frustum_clearing_pixel_step = 8;
         double frustum_min_range = 0.2;
         double frustum_max_range = 3.0;
-        double frustum_skip_distance = -1.0;
-        bool frustum_column_clearing = false;
-        double column_clear_min_height = 0.10;
-        double column_clear_max_height = -1.0;
-        double column_skip_distance = 0.02;
-        double column_cover_distance = -1.0;
-        int frustum_clear_left_border_px = 0;
-        int frustum_clear_right_border_px = 0;
 
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
@@ -500,16 +476,8 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_skip_distance", frustum_skip_distance);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_column_clearing", frustum_column_clearing);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_min_height", column_clear_min_height);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_max_height", column_clear_max_height);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_left_border_px", frustum_clear_left_border_px);
-        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_right_border_px", frustum_clear_right_border_px);
         robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
-        robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f, frustum_clear_left_border_px: %d, frustum_clear_right_border_px: %d", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range, frustum_clear_left_border_px, frustum_clear_right_border_px);
+        robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
       }
     }
   }

src/observation_buffer.cpp

@@ -60,19 +60,19 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_
 
 ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
                                      double min_obstacle_height, double max_obstacle_height, double obstacle_range,
-                                     double raytrace_range, const DepthFrustumConfig& frustum_config,
+                                     double raytrace_range, unsigned int frustum_pixel_step,
+                                     double frustum_min_range, double frustum_max_range,
                                      tf3::BufferCore& tf3_buffer, string global_frame,
                                      string sensor_frame, double tf_tolerance) :
     tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
     last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
     min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
     obstacle_range_(obstacle_range), raytrace_range_(raytrace_range),
-    tf_tolerance_(tf_tolerance),
-    frustum_config_(frustum_config)
+    frustum_pixel_step_(std::max(1u, frustum_pixel_step)),
+    frustum_min_range_(std::max(0.0, frustum_min_range)),
+    frustum_max_range_(std::max(frustum_max_range, frustum_min_range_)),
+    tf_tolerance_(tf_tolerance)
 {
-  frustum_config_.pixel_step = std::max(1u, frustum_config_.pixel_step);
-  frustum_config_.min_range = std::max(0.0, frustum_config_.min_range);
-  frustum_config_.max_range = std::max(frustum_config_.max_range, frustum_config_.min_range);
 }
 
 ObservationBuffer::~ObservationBuffer()
@@ -240,7 +240,8 @@ void ObservationBuffer::bufferDepthCamera(robot_sensor_msgs::DepthCameraData::Co
     return;
 
   DepthCameraObservation observation(
-      std::move(depth_camera_data), topic_name_, robot::Time::now(), frustum_config_);
+      std::move(depth_camera_data), topic_name_, robot::Time::now(),
+      frustum_pixel_step_, frustum_min_range_, frustum_max_range_);
 
   if (observation_keep_time_ == robot::Duration(0.0) && !depth_observation_list_.empty())
   {

test/coordinates_test.cpp

@@ -182,24 +182,24 @@ TEST(CostmapPerformanceRegression, observation_copy_shares_cloud_payload)
 
 TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once)
 {
-  // tf3::BufferCore tf_buffer(tf3::Duration(10.0));
-  // ObservationBuffer buffer(
-  //     "/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0,
-  //     8, 0.2, 3.0, tf_buffer, "odom", "", 0.2);
+  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
+  ObservationBuffer buffer(
+      "/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0,
+      8, 0.2, 3.0, tf_buffer, "odom", "", 0.2);
 
-  // robot_sensor_msgs::DepthCameraData::ConstPtr depth =
-  //     boost::make_shared<robot_sensor_msgs::DepthCameraData>();
-  // buffer.bufferDepthCamera(depth);
+  robot_sensor_msgs::DepthCameraData::ConstPtr depth =
+      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
+  buffer.bufferDepthCamera(depth);
 
-  // std::vector<DepthCameraObservation> first_snapshot;
-  // buffer.getDepthObservations(first_snapshot);
-  // ASSERT_EQ(first_snapshot.size(), 1u);
-  // EXPECT_EQ(first_snapshot.front().data_, depth.get());
-  // EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data");
+  std::vector<DepthCameraObservation> first_snapshot;
+  buffer.getDepthObservations(first_snapshot);
+  ASSERT_EQ(first_snapshot.size(), 1u);
+  EXPECT_EQ(first_snapshot.front().data_, depth.get());
+  EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data");
 
-  // std::vector<DepthCameraObservation> second_snapshot;
-  // buffer.getDepthObservations(second_snapshot);
-  // EXPECT_TRUE(second_snapshot.empty());
+  std::vector<DepthCameraObservation> second_snapshot;
+  buffer.getDepthObservations(second_snapshot);
+  EXPECT_TRUE(second_snapshot.empty());
 }
 
 TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs)