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_;
 };
 
 /**