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_(),
-      pixel_step_(0),
-      min_range_(0.0),
-      max_range_(0.0)
+      topic_()
   {
   }
 
@@ -65,16 +86,12 @@ public:
       const robot_sensor_msgs::DepthCameraData& data,
       std::string topic,
       const robot::Time& received_time,
-      unsigned int pixel_step,
-      double min_range,
-      double max_range)
+      const DepthFrustumConfig& frustum)
     : 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),
-      min_range_(min_range),
-      max_range_(max_range)
+      frustum_(frustum)
   {
   }
 
@@ -82,16 +99,12 @@ public:
       robot_sensor_msgs::DepthCameraData::ConstPtr data,
       std::string topic,
       const robot::Time& received_time,
-      unsigned int pixel_step,
-      double min_range,
-      double max_range)
+      const DepthFrustumConfig& frustum)
     : data_handle_(std::move(data)),
       data_(data_handle_.get()),
       topic_(std::move(topic)),
       received_time_(received_time),
-      pixel_step_(pixel_step),
-      min_range_(min_range),
-      max_range_(max_range)
+      frustum_(frustum)
   {
   }
 
@@ -100,9 +113,7 @@ public:
       data_(data_handle_.get()),
       topic_(other.topic_),
       received_time_(other.received_time_),
-      pixel_step_(other.pixel_step_),
-      min_range_(other.min_range_),
-      max_range_(other.max_range_)
+      frustum_(other.frustum_)
   {
   }
 
@@ -111,14 +122,10 @@ public:
       data_(data_handle_.get()),
       topic_(std::move(other.topic_)),
       received_time_(other.received_time_),
-      pixel_step_(other.pixel_step_),
-      min_range_(other.min_range_),
-      max_range_(other.max_range_)
+      frustum_(other.frustum_)
   {
     other.data_ = nullptr;
-    other.pixel_step_ = 0;
-    other.min_range_ = 0.0;
-    other.max_range_ = 0.0;
+    other.frustum_ = DepthFrustumConfig();
   }
 
   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_;
-    min_range_ = other.min_range_;
-    max_range_ = other.max_range_;
+    frustum_ = other.frustum_;
     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_;
-    min_range_ = other.min_range_;
-    max_range_ = other.max_range_;
+    frustum_ = other.frustum_;
 
     other.data_ = nullptr;
-    other.pixel_step_ = 0;
-    other.min_range_ = 0.0;
-    other.max_range_ = 0.0;
+    other.frustum_ = DepthFrustumConfig();
 
     return *this;
   }
@@ -163,9 +164,7 @@ public:
   const robot_sensor_msgs::DepthCameraData* data_;
   std::string topic_;
   robot::Time received_time_;
-  unsigned int pixel_step_;
-  double min_range_;
-  double max_range_;
+  DepthFrustumConfig frustum_;
 };
 
 /**

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 frustum_max_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
+                    double raytrace_range, const DepthFrustumConfig& frustum_config,
+                    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_;
-  double frustum_min_range_;
-  double frustum_max_range_;
+  DepthFrustumConfig frustum_config_;
 };
 }  // 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.
-  /// Negative keeps the legacy 2 * resolution behaviour.
-  double frustum_skip_distance_ = -1.0;
-
-  /// Full-column clearing: 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).
-  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;
-
+  /// 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
@@ -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;