otimal

src/depth_local_costmap_noetic_test_node.cpp

@@ -9,6 +9,7 @@
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <robot_depth_image_proc/depth_frame_filter.h>
+#include <robot_depth_image_proc/processing_rate_limiter.h>
 #include <robot_depth_image_proc/ros_message_conversions.h>
 #include <robot/robot.h>
 #include <robot_geometry_msgs/PoseStamped.h>
@@ -32,7 +33,10 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
+#include <algorithm>
 #include <atomic>
+#include <chrono>
+#include <cstdint>
 #include <cmath>
 #include <exception>
 #include <memory>
@@ -272,7 +276,7 @@ public:
   {
     private_nh.param("depth_cloud_topic", depth_cloud_topic_, std::string("/camera/depth/points_proc"));
     private_nh.param("enable_depth_cloud_voxel_filter", enable_voxel_filter_, true);
-    private_nh.param("depth_cloud_voxel_leaf_size", voxel_leaf_size_, 0.10);
+    private_nh.param("depth_cloud_voxel_leaf_size", voxel_leaf_size_, 0.05);
     voxel_leaf_size_ = std::max(0.0, voxel_leaf_size_);
 
     depth_cloud_sub_ =
@@ -327,7 +331,7 @@ private:
   robot_costmap_2d::Costmap2DROBOT& local_costmap_;
   std::string depth_cloud_topic_;
   bool enable_voxel_filter_{true};
-  double voxel_leaf_size_{0.10};
+  double voxel_leaf_size_{0.05};
   ros::Subscriber depth_cloud_sub_;
 };
 
@@ -345,6 +349,19 @@ public:
                      depth_camera_data_topic_,
                      std::string("/camera/depth/data"));
     private_nh.param("enable_depth_clearing_filter", enable_clearing_filter_, enable_clearing_filter_);
+    private_nh.param(
+        "depth_clearing_processing_rate", processing_rate_hz_, processing_rate_hz_);
+    private_nh.param(
+        "feeder_performance_metrics_enabled",
+        performance_metrics_enabled_,
+        performance_metrics_enabled_);
+    private_nh.param(
+        "feeder_performance_metrics_period",
+        performance_metrics_period_,
+        performance_metrics_period_);
+    performance_metrics_period_ = std::max(1.0, performance_metrics_period_);
+    processing_rate_limiter_.setRate(processing_rate_hz_);
+    metrics_window_start_ = ros::WallTime::now();
 
     camera_info_sub_ =
         private_nh.subscribe(camera_info_topic_, 1, &DepthCameraDataFeeder::cameraInfoCallback, this);
@@ -443,6 +460,15 @@ 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::DepthCameraData::Ptr depth_camera_data(new robot_sensor_msgs::DepthCameraData());
     depth_camera_data->depth = depth_image_proc::toRobotImage(*msg);
 
@@ -464,6 +490,7 @@ private:
 
     robot_sensor_msgs::DepthCameraData::ConstPtr const_depth_camera_data = depth_camera_data;
     feedVoxelLayers(local_costmap_, const_depth_camera_data, depth_camera_data_topic_);
+    recordProcessedFrame(processing_start);
 
     ROS_INFO_THROTTLE(5.0, "Fed DepthCameraData to robot_costmap_2d: %ux%u encoding=%s frame=%s topic=%s",
                       msg->width,
@@ -471,6 +498,48 @@ private:
                       msg->encoding.c_str(),
                       depth_camera_data->header.frame_id.c_str(),
                       depth_camera_data_topic_.c_str());
+    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(
+        "Depth clearing performance: input=%.1f Hz processed=%.1f Hz "
+        "skipped=%llu avg=%.2f ms max=%.2f ms",
+        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;
   }
 
   robot_costmap_2d::Costmap2DROBOT& local_costmap_;
@@ -478,6 +547,16 @@ private:
   std::string camera_info_topic_;
   std::string depth_camera_data_topic_;
   bool enable_clearing_filter_{true};
+  double processing_rate_hz_{15.0};
+  depth_image_proc::ProcessingRateLimiter processing_rate_limiter_;
+  bool performance_metrics_enabled_{true};
+  double performance_metrics_period_{5.0};
+  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};
   depth_image_proc::DepthFilterConfig clearing_filter_config_;
   depth_image_proc::DepthFrameFilter clearing_frame_filter_;
   robot_sensor_msgs::CameraInfo camera_info_;