add fillter depth raw

src/depth_local_costmap_noetic_test_node.cpp

@@ -8,6 +8,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/OccupancyGrid.h>
+#include <robot_depth_image_proc/depth_frame_filter.h>
 #include <robot_depth_image_proc/ros_message_conversions.h>
 #include <robot/robot.h>
 #include <robot_geometry_msgs/PoseStamped.h>
@@ -335,12 +336,15 @@ class DepthCameraDataFeeder
 public:
   DepthCameraDataFeeder(ros::NodeHandle& private_nh, robot_costmap_2d::Costmap2DROBOT& local_costmap)
     : local_costmap_(local_costmap)
+    , clearing_filter_config_(loadClearingFilterConfig(private_nh))
+    , clearing_frame_filter_(clearing_filter_config_)
   {
     private_nh.param("depth_image_topic", depth_image_topic_, std::string("/camera/depth/image_raw"));
     private_nh.param("camera_info_topic", camera_info_topic_, std::string("/camera/depth/camera_info"));
     private_nh.param("depth_camera_data_topic",
                      depth_camera_data_topic_,
                      std::string("/camera/depth/data"));
+    private_nh.param("enable_depth_clearing_filter", enable_clearing_filter_, enable_clearing_filter_);
 
     camera_info_sub_ =
         private_nh.subscribe(camera_info_topic_, 1, &DepthCameraDataFeeder::cameraInfoCallback, this);
@@ -349,6 +353,48 @@ public:
   }
 
 private:
+  // Pre-filter config for the frustum-clearing depth image. The clearing path
+  // runs the edge / flying-pixel filter but keeps the temporal filter OFF:
+  // invalidating a freshly appeared real obstacle would let the clearing ray
+  // pass straight through it (clearing to max_range on invalid pixels).
+  static depth_image_proc::DepthFilterConfig loadClearingFilterConfig(ros::NodeHandle& private_nh)
+  {
+    depth_image_proc::DepthFilterConfig config;
+    config.temporal_min_frames = 1;  // temporal filter disabled for clearing
+
+    private_nh.param("clearing_filter/edge_max_delta", config.edge_max_delta, config.edge_max_delta);
+    private_nh.param("clearing_filter/edge_dilation", config.edge_dilation, config.edge_dilation);
+    private_nh.param("clearing_filter/edge_window", config.edge_window, config.edge_window);
+    private_nh.param("clearing_filter/edge_invalid_border",
+                     config.edge_invalid_border,
+                     config.edge_invalid_border);
+    private_nh.param("clearing_filter/temporal_max_delta",
+                     config.temporal_max_delta,
+                     config.temporal_max_delta);
+    private_nh.param("clearing_filter/temporal_min_frames",
+                     config.temporal_min_frames,
+                     config.temporal_min_frames);
+
+    if (!config.valid())
+    {
+      ROS_ERROR("Invalid clearing_filter config: edge_max_delta=%.3f edge_dilation=%d "
+                "edge_window=%d temporal_max_delta=%.3f temporal_min_frames=%d. "
+                "Falling back to defaults (edge filter on, temporal off).",
+                config.edge_max_delta, config.edge_dilation, config.edge_window,
+                config.temporal_max_delta, config.temporal_min_frames);
+      depth_image_proc::DepthFilterConfig fallback;
+      fallback.temporal_min_frames = 1;
+      return fallback;
+    }
+
+    ROS_INFO("Depth clearing filter: edge_max_delta=%.3f m edge_dilation=%d edge_window=%d "
+             "edge_invalid_border=%d temporal_max_delta=%.3f m temporal_min_frames=%d",
+             config.edge_max_delta, config.edge_dilation, config.edge_window,
+             config.edge_invalid_border ? 1 : 0, config.temporal_max_delta,
+             config.temporal_min_frames);
+    return config;
+  }
+
   void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
   {
     if (!msg)
@@ -399,6 +445,18 @@ private:
 
     robot_sensor_msgs::DepthCameraData::Ptr depth_camera_data(new robot_sensor_msgs::DepthCameraData());
     depth_camera_data->depth = depth_image_proc::toRobotImage(*msg);
+
+    // Filter the depth image feeding frustum clearing. Raw flying pixels /
+    // speckle read shorter than the true surface and truncate the clearing ray,
+    // so stale obstacles never get cleared once the object leaves. Invalidated
+    // pixels instead let that ray clear out to max_range. Serialized because
+    // DepthFrameFilter is stateful and the node runs a 2-thread AsyncSpinner.
+    if (enable_clearing_filter_)
+    {
+      std::lock_guard<std::mutex> lock(clearing_filter_mutex_);
+      clearing_frame_filter_.apply(depth_camera_data->depth);
+    }
+
     depth_camera_data->camera_info = camera_info;
     depth_camera_data->header = depth_camera_data->depth.header;
     if (depth_camera_data->header.frame_id.empty())
@@ -419,9 +477,13 @@ private:
   std::string depth_image_topic_;
   std::string camera_info_topic_;
   std::string depth_camera_data_topic_;
+  bool enable_clearing_filter_{true};
+  depth_image_proc::DepthFilterConfig clearing_filter_config_;
+  depth_image_proc::DepthFrameFilter clearing_frame_filter_;
   robot_sensor_msgs::CameraInfo camera_info_;
   bool has_camera_info_{false};
   std::mutex camera_info_mutex_;
+  std::mutex clearing_filter_mutex_;
   ros::Subscriber depth_image_sub_;
   ros::Subscriber camera_info_sub_;
 };