otimal

include/robot_depth_image_proc/processing_rate_limiter.h

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <chrono>
+#include <cmath>
+
+namespace depth_image_proc
+{
+
+// Wall-clock limiter for CPU-bound sensor callbacks. It deliberately does not
+// catch up after a delayed callback: processing a burst of old frames would
+// increase latency and CPU without helping a latest-sample costmap consumer.
+class ProcessingRateLimiter
+{
+public:
+  using Clock = std::chrono::steady_clock;
+  using TimePoint = Clock::time_point;
+
+  explicit ProcessingRateLimiter(double rate_hz = 0.0)
+  {
+    setRate(rate_hz);
+  }
+
+  void setRate(double rate_hz)
+  {
+    enabled_ = std::isfinite(rate_hz) && rate_hz > 0.0;
+    if (enabled_)
+    {
+      period_ = std::chrono::duration_cast<Clock::duration>(
+          std::chrono::duration<double>(1.0 / rate_hz));
+      early_tolerance_ = period_ / 20;
+    }
+    reset();
+  }
+
+  bool shouldProcess()
+  {
+    return shouldProcessAt(Clock::now());
+  }
+
+  bool shouldProcessAt(const TimePoint now)
+  {
+    if (!enabled_)
+      return true;
+
+    if (!initialized_)
+    {
+      initialized_ = true;
+      next_process_time_ = now + period_;
+      return true;
+    }
+
+    if (now + early_tolerance_ >= next_process_time_)
+    {
+      next_process_time_ += period_;
+      if (now + early_tolerance_ >= next_process_time_)
+        next_process_time_ = now + period_;
+      return true;
+    }
+    return false;
+  }
+
+  void reset()
+  {
+    initialized_ = false;
+    next_process_time_ = TimePoint{};
+  }
+
+  bool enabled() const
+  {
+    return enabled_;
+  }
+
+private:
+  bool enabled_{false};
+  bool initialized_{false};
+  Clock::duration period_{Clock::duration::zero()};
+  Clock::duration early_tolerance_{Clock::duration::zero()};
+  TimePoint next_process_time_{};
+};
+
+}  // namespace depth_image_proc

launch/depth_image_proc_gazebo.launch

@@ -1,11 +1,15 @@
 <?xml version="1.0"?>
 <launch>
   <!-- Use with Gazebo T800: depth topics come from URDF depth camera plugin -->
-  <arg name="multi_cam" default="true"/>
+  <arg name="multi_cam" default="false"/>
   <arg name="depth_topic" default="/camera/depth/image_raw"/>
   <arg name="camera_info_topic" default="/camera/depth/camera_info"/>
   <arg name="cloud_topic" default="/camera/depth/points_proc"/>
   <arg name="fixed_frame" default="odom"/>
+  <!-- Limit CPU-heavy filter/projection work while preserving 30 Hz camera input. -->
+  <arg name="processing_rate" default="15.0"/>
+  <arg name="performance_metrics_enabled" default="true"/>
+  <arg name="performance_metrics_period" default="5.0"/>
 
   <!-- Noise filter + downsampling (applied before publishing the cloud) -->
   <arg name="filter_decimation" default="4"/>          <!-- keep 1 of NxN pixels -->
@@ -19,7 +23,7 @@
   <arg name="filter_edge_invalid_border" default="true"/> <!-- drop pixels hugging no-data holes at occlusion edges -->
   <arg name="filter_temporal_max_delta" default="0.06"/> <!-- [m] per-frame stability tolerance -->
   <arg name="filter_temporal_min_frames" default="2"/>  <!-- stable frames required; 0/1 disables -->
-  <arg name="rviz" default="true"/>
+  <arg name="rviz" default="false"/>
 
   <node pkg="robot_depth_image_proc"
         type="depth_image_proc_node"
@@ -31,6 +35,9 @@
     <param name="cloud_topic" value="$(arg cloud_topic)"/>
     <param name="fixed_frame" value="$(arg fixed_frame)"/>
     <param name="publish_tf" value="false"/>
+    <param name="processing_rate" value="$(arg processing_rate)"/>
+    <param name="performance_metrics_enabled" value="$(arg performance_metrics_enabled)"/>
+    <param name="performance_metrics_period" value="$(arg performance_metrics_period)"/>
     <param name="filter/decimation" value="$(arg filter_decimation)"/>
     <param name="filter/range_min" value="$(arg filter_range_min)"/>
     <param name="filter/range_max" value="$(arg filter_range_max)"/>

src/depth_image_proc_node.cpp

@@ -1,3 +1,6 @@
+#include <algorithm>
+#include <chrono>
+#include <cstdint>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -12,6 +15,7 @@
 
 #include <robot_depth_image_proc/depth_frame_filter.h>
 #include <robot_depth_image_proc/point_cloud_xyz.h>
+#include <robot_depth_image_proc/processing_rate_limiter.h>
 #include <robot_depth_image_proc/ros_message_conversions.h>
 
 struct CameraConfig
@@ -30,12 +34,20 @@ public:
       const CameraConfig& config,
       const depth_image_proc::DepthFilterConfig& filter_config,
       const std::string& fixed_frame,
-      bool publish_tf)
+      bool publish_tf,
+      double processing_rate_hz,
+      bool performance_metrics_enabled,
+      double performance_metrics_period)
       : config_(config),
         filter_config_(filter_config),
         frame_filter_(filter_config),
         fixed_frame_(fixed_frame),
-        publish_tf_(publish_tf)
+        publish_tf_(publish_tf),
+        processing_rate_hz_(processing_rate_hz),
+        processing_rate_limiter_(processing_rate_hz),
+        performance_metrics_enabled_(performance_metrics_enabled),
+        performance_metrics_period_(std::max(1.0, performance_metrics_period)),
+        metrics_window_start_(ros::WallTime::now())
   {
     cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>(config_.cloud_topic, 1);
 
@@ -55,11 +67,12 @@ public:
     }
 
     ROS_INFO(
-        "[%s] depth_image_proc listening on [%s] + [%s], publishing [%s]",
+        "[%s] depth_image_proc listening on [%s] + [%s], publishing [%s] at <= %.1f Hz",
         config_.name.c_str(),
         config_.depth_topic.c_str(),
         config_.camera_info_topic.c_str(),
-        config_.cloud_topic.c_str());
+        config_.cloud_topic.c_str(),
+        processing_rate_hz_ > 0.0 ? processing_rate_hz_ : 0.0);
   }
 
 private:
@@ -106,11 +119,21 @@ private:
       return;
     }
 
+    ++received_frames_;
+    if (!processing_rate_limiter_.shouldProcess())
+    {
+      ++skipped_frames_;
+      reportPerformanceIfDue();
+      return;
+    }
+
+    const auto processing_start = std::chrono::steady_clock::now();
     robot_sensor_msgs::Image depth = depth_image_proc::toRobotImage(*msg);
     frame_filter_.apply(depth);
     const robot_sensor_msgs::PointCloud2 cloud =
         depth_image_proc::convertDepthToPointCloudFiltered(
             depth, camera_info, filter_config_);
+    recordProcessedFrame(processing_start);
 
     // A fully filtered-out frame (nothing in range) is valid: publish the
     // empty cloud so costmap_2d observation buffers do not go stale.
@@ -120,15 +143,57 @@ private:
           5.0,
           "[%s] depth_image_proc conversion failed (bad encoding or filter config)",
           config_.name.c_str());
+      reportPerformanceIfDue();
       return;
     }
 
-    ROS_WARN("cloud: %d", (int)cloud.data.size());
-
     sensor_msgs::PointCloud2 ros_cloud = depth_image_proc::toRosPointCloud(cloud);
     ros_cloud.header.stamp = msg->header.stamp;
     ros_cloud.header.frame_id = msg->header.frame_id;
     cloud_pub_.publish(ros_cloud);
+    reportPerformanceIfDue();
+  }
+
+  void recordProcessedFrame(const std::chrono::steady_clock::time_point processing_start)
+  {
+    const double elapsed_ms =
+        std::chrono::duration<double, std::milli>(
+            std::chrono::steady_clock::now() - processing_start).count();
+    ++processed_frames_;
+    processing_time_ms_ += elapsed_ms;
+    max_processing_time_ms_ = std::max(max_processing_time_ms_, elapsed_ms);
+  }
+
+  void reportPerformanceIfDue()
+  {
+    if (!performance_metrics_enabled_)
+      return;
+
+    const ros::WallTime now = ros::WallTime::now();
+    const double window_seconds = (now - metrics_window_start_).toSec();
+    if (window_seconds < performance_metrics_period_)
+      return;
+
+    const double input_hz = static_cast<double>(received_frames_) / window_seconds;
+    const double processed_hz = static_cast<double>(processed_frames_) / window_seconds;
+    const double average_ms =
+        processed_frames_ > 0 ? processing_time_ms_ / static_cast<double>(processed_frames_) : 0.0;
+    ROS_INFO(
+        "[%s] depth performance: input=%.1f Hz processed=%.1f Hz "
+        "skipped=%llu avg=%.2f ms max=%.2f ms",
+        config_.name.c_str(),
+        input_hz,
+        processed_hz,
+        static_cast<unsigned long long>(skipped_frames_),
+        average_ms,
+        max_processing_time_ms_);
+
+    metrics_window_start_ = now;
+    received_frames_ = 0;
+    processed_frames_ = 0;
+    skipped_frames_ = 0;
+    processing_time_ms_ = 0.0;
+    max_processing_time_ms_ = 0.0;
   }
 
   void publishStaticTransform()
@@ -151,6 +216,16 @@ private:
   depth_image_proc::DepthFrameFilter frame_filter_;
   const std::string fixed_frame_;
   const bool publish_tf_;
+  const double processing_rate_hz_;
+  depth_image_proc::ProcessingRateLimiter processing_rate_limiter_;
+  const bool performance_metrics_enabled_;
+  const double performance_metrics_period_;
+  ros::WallTime metrics_window_start_;
+  std::uint64_t received_frames_{0};
+  std::uint64_t processed_frames_{0};
+  std::uint64_t skipped_frames_{0};
+  double processing_time_ms_{0.0};
+  double max_processing_time_ms_{0.0};
 
   std::mutex mutex_;
   robot_sensor_msgs::CameraInfo camera_info_;
@@ -170,6 +245,9 @@ public:
   {
     pnh.param("fixed_frame", fixed_frame_, std::string("map"));
     pnh.param("publish_tf", publish_tf_, true);
+    pnh.param("processing_rate", processing_rate_hz_, 15.0);
+    pnh.param("performance_metrics_enabled", performance_metrics_enabled_, true);
+    pnh.param("performance_metrics_period", performance_metrics_period_, 5.0);
 
     const depth_image_proc::DepthFilterConfig filter_config = loadFilterConfig(pnh);
 
@@ -184,7 +262,14 @@ public:
     for (const CameraConfig& config : configs)
     {
       pipelines_.push_back(std::make_unique<DepthCameraPipeline>(
-          nh, config, filter_config, fixed_frame_, publish_tf_));
+          nh,
+          config,
+          filter_config,
+          fixed_frame_,
+          publish_tf_,
+          processing_rate_hz_,
+          performance_metrics_enabled_,
+          performance_metrics_period_));
     }
 
     ROS_INFO("depth_image_proc_node started with %zu camera(s)", pipelines_.size());
@@ -336,6 +421,9 @@ private:
 
   std::string fixed_frame_;
   bool publish_tf_{true};
+  double processing_rate_hz_{15.0};
+  bool performance_metrics_enabled_{true};
+  double performance_metrics_period_{5.0};
   std::vector<std::unique_ptr<DepthCameraPipeline>> pipelines_;
 };
 

test/test_point_cloud_xyz.cpp

@@ -5,6 +5,7 @@
 
 #include <robot_depth_image_proc/depth_frame_filter.h>
 #include <robot_depth_image_proc/point_cloud_xyz.h>
+#include <robot_depth_image_proc/processing_rate_limiter.h>
 #include <robot_sensor_msgs/image_encodings.h>
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
 
@@ -59,6 +60,50 @@ robot_sensor_msgs::Image makeFlatDepthImage(
 
 }  // namespace
 
+TEST(ProcessingRateLimiter, LimitsWorkWithoutCatchUpBursts)
+{
+  using Clock = depth_image_proc::ProcessingRateLimiter::Clock;
+  using namespace std::chrono_literals;
+
+  depth_image_proc::ProcessingRateLimiter limiter(10.0);
+  const Clock::time_point start{};
+
+  EXPECT_TRUE(limiter.shouldProcessAt(start));
+  EXPECT_FALSE(limiter.shouldProcessAt(start + 50ms));
+  EXPECT_TRUE(limiter.shouldProcessAt(start + 100ms));
+
+  // A late callback schedules from "now"; it does not create a catch-up burst.
+  EXPECT_TRUE(limiter.shouldProcessAt(start + 350ms));
+  EXPECT_FALSE(limiter.shouldProcessAt(start + 351ms));
+  EXPECT_TRUE(limiter.shouldProcessAt(start + 450ms));
+}
+
+TEST(ProcessingRateLimiter, NonPositiveRateDisablesLimiting)
+{
+  using Clock = depth_image_proc::ProcessingRateLimiter::Clock;
+
+  depth_image_proc::ProcessingRateLimiter limiter(0.0);
+  const Clock::time_point now{};
+  EXPECT_TRUE(limiter.shouldProcessAt(now));
+  EXPECT_TRUE(limiter.shouldProcessAt(now));
+
+  limiter.setRate(-1.0);
+  EXPECT_TRUE(limiter.shouldProcessAt(now));
+}
+
+TEST(ProcessingRateLimiter, AcceptsNominalFramesWithSmallClockJitter)
+{
+  using Clock = depth_image_proc::ProcessingRateLimiter::Clock;
+  using namespace std::chrono_literals;
+
+  depth_image_proc::ProcessingRateLimiter limiter(15.0);
+  const Clock::time_point start{};
+
+  EXPECT_TRUE(limiter.shouldProcessAt(start));
+  EXPECT_TRUE(limiter.shouldProcessAt(start + 65ms));
+  EXPECT_TRUE(limiter.shouldProcessAt(start + 130ms));
+}
+
 TEST(PointCloudXyz, ConvertsFlatDepthImage)
 {
   const uint32_t width = 3;