otimal

include/robot_costmap_2d/observation.h

@@ -48,16 +48,37 @@ 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;
+};
+
 class DepthCameraObservation
 {
 public:
   DepthCameraObservation()
     : data_handle_(),
       data_(nullptr),
-      topic_(),
+      topic_()
-      pixel_step_(0),
-      min_range_(0.0),
-      max_range_(0.0)
   {
   }
 
@@ -65,16 +86,12 @@ public:
       const robot_sensor_msgs::DepthCameraData& data,
       std::string topic,
       const robot::Time& received_time,
-      unsigned int pixel_step,
+      const DepthFrustumConfig& frustum)
-      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),
-      pixel_step_(pixel_step),
+      frustum_(frustum)
-      min_range_(min_range),
-      max_range_(max_range)
   {
   }
 
@@ -82,16 +99,12 @@ public:
       robot_sensor_msgs::DepthCameraData::ConstPtr data,
       std::string topic,
       const robot::Time& received_time,
-      unsigned int pixel_step,
+      const DepthFrustumConfig& frustum)
-      double min_range,
-      double max_range)
     : data_handle_(std::move(data)),
       data_(data_handle_.get()),
       topic_(std::move(topic)),
       received_time_(received_time),
-      pixel_step_(pixel_step),
+      frustum_(frustum)
-      min_range_(min_range),
-      max_range_(max_range)
   {
   }
 
@@ -100,9 +113,7 @@ public:
       data_(data_handle_.get()),
       topic_(other.topic_),
       received_time_(other.received_time_),
-      pixel_step_(other.pixel_step_),
+      frustum_(other.frustum_)
-      min_range_(other.min_range_),
-      max_range_(other.max_range_)
   {
   }
 
@@ -111,14 +122,10 @@ public:
       data_(data_handle_.get()),
       topic_(std::move(other.topic_)),
       received_time_(other.received_time_),
-      pixel_step_(other.pixel_step_),
+      frustum_(other.frustum_)
-      min_range_(other.min_range_),
-      max_range_(other.max_range_)
   {
     other.data_ = nullptr;
-    other.pixel_step_ = 0;
+    other.frustum_ = DepthFrustumConfig();
-    other.min_range_ = 0.0;
-    other.max_range_ = 0.0;
   }
 
   DepthCameraObservation& operator=(const DepthCameraObservation& other)
@@ -130,9 +137,7 @@ public:
     data_ = data_handle_.get();
     topic_ = other.topic_;
     received_time_ = other.received_time_;
-    pixel_step_ = other.pixel_step_;
+    frustum_ = other.frustum_;
-    min_range_ = other.min_range_;
-    max_range_ = other.max_range_;
     return *this;
   }
 
@@ -145,14 +150,10 @@ public:
     data_ = data_handle_.get();
     topic_ = std::move(other.topic_);
     received_time_ = other.received_time_;
-    pixel_step_ = other.pixel_step_;
+    frustum_ = other.frustum_;
-    min_range_ = other.min_range_;
-    max_range_ = other.max_range_;
 
     other.data_ = nullptr;
-    other.pixel_step_ = 0;
+    other.frustum_ = DepthFrustumConfig();
-    other.min_range_ = 0.0;
-    other.max_range_ = 0.0;
 
     return *this;
   }
@@ -163,9 +164,7 @@ public:
   const robot_sensor_msgs::DepthCameraData* data_;
   std::string topic_;
   robot::Time received_time_;
-  unsigned int pixel_step_;
+  DepthFrustumConfig frustum_;
-  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, unsigned int frustum_pixel_step, double frustum_min_range,
+                    double raytrace_range, const DepthFrustumConfig& frustum_config,
-                    double frustum_max_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
+                    tf3::BufferCore& tf3_buffer, std::string global_frame,
                     std::string sensor_frame, double tf_tolerance);
 
 
@@ -171,9 +171,7 @@ private:
   boost::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
   double obstacle_range_, raytrace_range_;
   double tf_tolerance_;
-  unsigned int frustum_pixel_step_;
+  DepthFrustumConfig frustum_config_;
-  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

@@ -114,25 +114,12 @@ private:
   bool publish_voxel_;
   robot_voxel_grid::VoxelGrid robot_voxel_grid_;
   double z_resolution_, origin_z_;
-  /// Clearing rays stop this far [m] before the measured surface.
+  /// Scratch for the full-column clearing pass (config lives per observation
-  /// Negative keeps the legacy 2 * resolution behaviour.
+  /// source in DepthFrustumConfig): per depth-image pixel column, the nearest
-  double frustum_skip_distance_ = -1.0;
+  /// return inside the obstacle height band certifies "no obstacle in this
-
+  /// direction closer than d". Cells along that 2D beam get their whole voxel
-  /// Full-column clearing: per depth-image pixel column, the nearest return
+  /// column cleared, removing marked voxels the per-pixel 3D rays cannot
-  /// inside the obstacle height band certifies "no obstacle in this direction
+  /// reach (above the vertical FOV at close range).
-  /// 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).
-  bool frustum_column_clearing_ = false;
-  /// Height band [m] used to detect the nearest in-band return. Points below
-  /// the band (floor) do not shorten the beam. max < 0: use max_obstacle_height.
-  double column_clear_min_height_ = 0.10;
-  double column_clear_max_height_ = -1.0;
-  /// Full columns are only cleared beyond this distance [m], where the
-  /// vertical FOV covers the whole height band. Negative: derive each frame
-  /// from the camera intrinsics and mounting pose.
-  double column_cover_distance_ = -1.0;
-
   struct DepthColumnStat
   {
     double min_band_dist = -1.0;  ///< horizontal distance of nearest in-band return; < 0 = none
@@ -150,11 +137,13 @@ private:
   {
     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;

plugins/obstacle_layer.cpp

@@ -133,8 +133,7 @@ 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;
-        double frustum_min_range = 0.2;
+        DepthFrustumConfig frustum_config;
-        double frustum_max_range = 3.0;
 
         robot::NodeHandle priv_nh(nh, source);
         topic = loadParam(layer[source],"topic", topic);
@@ -147,10 +146,28 @@ 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_clearing_enabled = loadParam(layer, "frustum_clearing_enabled", false);
+        // frustum params are per-source; the layer-level key is kept as a
-        frustum_pixel_step = loadParam(layer, "frustum_clearing_pixel_step", 8);
+        // fallback for older YAMLs
-        frustum_min_range = loadParam(layer, "frustum_min_range", 0.2);
+        frustum_clearing_enabled = loadParam(layer[source], "frustum_clearing_enabled",
-        frustum_max_range = loadParam(layer, "frustum_max_range", 3.0);
+                                             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);
 
         if (priv_nh.hasParam("topic"))
           priv_nh.getParam("topic", topic);
@@ -180,13 +197,34 @@ 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_min_range);
+          priv_nh.getParam("frustum_min_range", frustum_config.min_range);
         if (priv_nh.hasParam("frustum_max_range"))
-          priv_nh.getParam("frustum_max_range", 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_depth_camera_topic"))
           priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
 
-        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);
+        frustum_config.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
+
+        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\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);
 
         double obstacle_range = 2.5;
         obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
@@ -244,8 +282,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_pixel_step,
+                                          max_obstacle_height, obstacle_range, raytrace_range, frustum_config,
-                                          frustum_min_range, frustum_max_range, *tf_, global_frame_,
+                                          *tf_, global_frame_,
                                           sensor_frame, transform_tolerance)));
           
         }

plugins/voxel_layer.cpp

@@ -98,11 +98,6 @@ 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);
-    frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0);
-    frustum_column_clearing_ = loadParam(layer, "frustum_column_clearing", false);
-    column_clear_min_height_ = loadParam(layer, "column_clear_min_height", 0.10);
-    column_clear_max_height_ = loadParam(layer, "column_clear_max_height", -1.0);
-    column_cover_distance_ = loadParam(layer, "column_cover_distance", -1.0);
 
     int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
     if (nh.hasParam("enabled"))
@@ -130,26 +125,6 @@ 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("frustum_skip_distance"))
-      nh.getParam("frustum_skip_distance", frustum_skip_distance_);
-    if (nh.hasParam("frustum_column_clearing"))
-      nh.getParam("frustum_column_clearing", frustum_column_clearing_);
-    if (nh.hasParam("column_clear_min_height"))
-      nh.getParam("column_clear_min_height", column_clear_min_height_);
-    if (nh.hasParam("column_clear_max_height"))
-      nh.getParam("column_clear_max_height", column_clear_max_height_);
-    if (nh.hasParam("column_cover_distance"))
-      nh.getParam("column_cover_distance", column_cover_distance_);
-
-    robot::log_info("VoxelLayer frustum_skip_distance: %.3f m%s\n",
-                    frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_,
-                    frustum_skip_distance_ >= 0.0 ? "" : " (legacy 2 * resolution)");
-    robot::log_info("VoxelLayer frustum_column_clearing: %s, band: [%.2f, %.2f] m, cover_distance: %.2f m%s\n",
-                    frustum_column_clearing_ ? "true" : "false",
-                    column_clear_min_height_,
-                    column_clear_max_height_ >= 0.0 ? column_clear_max_height_ : max_obstacle_height_,
-                    column_cover_distance_,
-                    column_cover_distance_ >= 0.0 ? "" : " (auto from camera pose)");
 
     this->matchSize();
   } 
@@ -500,20 +475,35 @@ void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
   cached_cx_ = cx;
   cached_cy_ = cy;
 
-  const std::size_t rows = (height + pixel_step - 1) / pixel_step;
+  // Sample every pixel_step-th row/column and always include the last image
-  const std::size_t columns = (width + pixel_step - 1) / pixel_step;
+  // row/column, so cells marked from border pixels stay inside the swept
-  depth_ray_cache_.clear();
+  // clearing fan.
-  depth_ray_cache_.reserve(rows * columns);
+  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());
+  depth_ray_cache_.clear();
+  depth_ray_cache_.reserve(u_samples.size() * v_samples.size());
+
+  for (const unsigned int v : v_samples)
   {
-    for (unsigned int u = 0; u < width; u += pixel_step)
+    for (unsigned int col = 0; col < u_samples.size(); ++col)
     {
+      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, x * inverse_norm, y * inverse_norm, inverse_norm});
+          DepthRay{u, v, col, x * inverse_norm, y * inverse_norm, inverse_norm});
     }
   }
 }
@@ -640,11 +630,12 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     return false;
   }
 
-  const unsigned int step = std::max(1u, observation.pixel_step_);
+  const DepthFrustumConfig& frustum = observation.frustum_;
-  const double min_range = observation.min_range_;
+  const unsigned int step = std::max(1u, frustum.pixel_step);
-  const double max_range = observation.max_range_;
+  const double min_range = frustum.min_range;
+  const double max_range = frustum.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);
@@ -677,12 +668,12 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
   // 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 = column_clear_min_height_;
+  const double band_min_h = frustum.column_min_height;
   const double band_max_h =
-      column_clear_max_height_ >= 0.0 ? column_clear_max_height_ : max_obstacle_height_;
+      frustum.column_max_height >= 0.0 ? frustum.column_max_height : max_obstacle_height_;
-  double cover_dist = column_cover_distance_;
+  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;
+  bool column_pass = frustum.column_clearing && band_max_h > band_min_h;
 
   if (column_pass && cover_dist < 0.0)
   {
@@ -728,9 +719,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     }
   }
 
-  const unsigned int column_count = (width + step - 1) / step;
   if (column_pass)
-    depth_column_stats_.assign(column_count, DepthColumnStat());
+    depth_column_stats_.assign(cached_column_count_, DepthColumnStat());
 
   for (const DepthRay& local_ray : depth_ray_cache_)
   {
@@ -739,18 +729,21 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
       continue;
 
+    // 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;
+
     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)
+    if (column_pass && local_ray.col < depth_column_stats_.size())
     {
       const double horiz_norm = std::hypot(global_ray.x, global_ray.y);
-      const unsigned int col = local_ray.u / step;
+      if (horiz_norm > 1e-6)
-      if (horiz_norm > 1e-6 && col < column_count)
       {
-        DepthColumnStat& stat = depth_column_stats_[col];
+        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)
         {
@@ -763,9 +756,6 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
 
         if (valid)
         {
-          // 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 = depth_m / local_ray.z;
           const double pz = oz + global_ray.z * euclid_range;
           if (pz >= band_min_h && pz <= band_max_h)
           {
@@ -781,8 +771,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     }
 
     double ray_len = max_range;
-    if (valid && depth_m < max_range)
+    if (valid && euclid_range < max_range)
-      ray_len = std::max(0.0, depth_m - skip_dist);
+      ray_len = std::max(0.0, euclid_range - skip_dist);
 
     if (ray_len <= min_range)
       continue;
@@ -801,7 +791,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
   if (column_pass)
   {
     cleared_any = clearDepthColumns(ox, oy, cover_dist, far_dist, min_range, max_range,
-                                    skip_dist, min_x, min_y, max_x, max_y) ||
+                                    std::max(0.0, frustum.column_skip_distance),
+                                    min_x, min_y, max_x, max_y) ||
                   cleared_any;
   }
 

src/costmap_2d_robot.cpp

@@ -400,6 +400,12 @@ 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;
 
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
@@ -417,6 +423,12 @@ 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);
         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_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
       }
@@ -459,6 +471,12 @@ 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;
 
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
         move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
@@ -476,6 +494,12 @@ 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);
         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_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, unsigned int frustum_pixel_step,
+                                     double raytrace_range, const DepthFrustumConfig& frustum_config,
-                                     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),
-    frustum_pixel_step_(std::max(1u, frustum_pixel_step)),
+    tf_tolerance_(tf_tolerance),
-    frustum_min_range_(std::max(0.0, frustum_min_range)),
+    frustum_config_(frustum_config)
-    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,8 +240,7 @@ void ObservationBuffer::bufferDepthCamera(robot_sensor_msgs::DepthCameraData::Co
     return;
 
   DepthCameraObservation observation(
-      std::move(depth_camera_data), topic_name_, robot::Time::now(),
+      std::move(depth_camera_data), topic_name_, robot::Time::now(), frustum_config_);
-      frustum_pixel_step_, frustum_min_range_, frustum_max_range_);
 
   if (observation_keep_time_ == robot::Duration(0.0) && !depth_observation_list_.empty())
   {