DuongTD/depth_image_proc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

otimal

Browse Source
This commit is contained in:
duongtd
2026-07-24 10:28:20 +07:00
parent a341cd215b
commit cfea88a834
4 changed files with 230 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
include/robot_depth_image_proc/processing_rate_limiter.h Normal file
View File

@@ -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

11
launch/depth_image_proc_gazebo.launch
View File

@@ -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)"/>

102
src/depth_image_proc_node.cpp
View File

@@ -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_;
};

45
test/test_point_cloud_xyz.cpp
View File

@@ -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;