From cfea88a834fea95a52bb30254a158aea4728e59f Mon Sep 17 00:00:00 2001 From: duongtd Date: Fri, 24 Jul 2026 10:28:20 +0700 Subject: [PATCH] otimal --- .../processing_rate_limiter.h | 81 ++++++++++++++ launch/depth_image_proc_gazebo.launch | 11 +- src/depth_image_proc_node.cpp | 102 ++++++++++++++++-- test/test_point_cloud_xyz.cpp | 45 ++++++++ 4 files changed, 230 insertions(+), 9 deletions(-) create mode 100644 include/robot_depth_image_proc/processing_rate_limiter.h diff --git a/include/robot_depth_image_proc/processing_rate_limiter.h b/include/robot_depth_image_proc/processing_rate_limiter.h new file mode 100644 index 0000000..4d18271 --- /dev/null +++ b/include/robot_depth_image_proc/processing_rate_limiter.h @@ -0,0 +1,81 @@ +#pragma once + +#include +#include + +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( + std::chrono::duration(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 diff --git a/launch/depth_image_proc_gazebo.launch b/launch/depth_image_proc_gazebo.launch index bb1e64f..84b9e24 100644 --- a/launch/depth_image_proc_gazebo.launch +++ b/launch/depth_image_proc_gazebo.launch @@ -1,11 +1,15 @@ - + + + + + @@ -19,7 +23,7 @@ - + + + + diff --git a/src/depth_image_proc_node.cpp b/src/depth_image_proc_node.cpp index ce1ef9f..ee3e33d 100644 --- a/src/depth_image_proc_node.cpp +++ b/src/depth_image_proc_node.cpp @@ -1,3 +1,6 @@ +#include +#include +#include #include #include #include @@ -12,6 +15,7 @@ #include #include +#include #include 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(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( + 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(received_frames_) / window_seconds; + const double processed_hz = static_cast(processed_frames_) / window_seconds; + const double average_ms = + processed_frames_ > 0 ? processing_time_ms_ / static_cast(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(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( - 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> pipelines_; }; diff --git a/test/test_point_cloud_xyz.cpp b/test/test_point_cloud_xyz.cpp index 2cc68ef..e01094f 100644 --- a/test/test_point_cloud_xyz.cpp +++ b/test/test_point_cloud_xyz.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include @@ -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;