i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-09 22:29:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: implement stream status tracking and update device status message structure

Browse Source
This commit is contained in:
ob-yalian
2026-09-21 15:45:46 +08:00
parent 80f9a24778
commit c1eda5e187
10 changed files with 246 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/fps_delay_status.hpp
-139
View File
@@ -1,139 +0,0 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#pragma once
#include <rclcpp/rclcpp.hpp>
#include "orbbec_camera_msgs/msg/device_status.hpp"
#include <chrono>
#include <functional>
#include <limits>
#include <mutex>
namespace orbbec_camera {
class FpsDelayStatus {
public:
explicit FpsDelayStatus(rclcpp::Logger logger) : log_level_(LogLevel::INFO), logger_(logger) {}
void tick(u_int64_t stream_timestamp) {
std::lock_guard<std::mutex> lock(mutex_);
double dt = (stream_timestamp - last_stream_timestamp_) / 1000000.0;
double fps = (dt > 0) ? (1.0 / dt) : 0.0;
// Convert now to milliseconds since steady_clock epoch
auto now2 = std::chrono::system_clock::now();
uint64_t ms_since_epoch =
std::chrono::duration_cast<std::chrono::milliseconds>(now2.time_since_epoch()).count();
double delay_ms =
static_cast<double>(ms_since_epoch) - static_cast<double>(stream_timestamp / 1000.0);
frame_count_++;
fps_sum_ += fps;
delay_sum_ += delay_ms;
last_fps_ = fps;
last_delay_ms_ = delay_ms;
last_stream_timestamp_ = stream_timestamp;
if (fps_max_ <= 0) fps_max_ = fps;
if (fps_min_ <= 0) fps_min_ = fps;
fps_max_ = std::max(fps_max_, fps);
fps_min_ = std::min(fps_min_, fps);
if (delay_max_ <= 0) delay_max_ = delay_ms;
if (delay_min_ <= 0) delay_min_ = delay_ms;
delay_max_ = std::max(delay_max_, delay_ms);
delay_min_ = std::min(delay_min_, delay_ms);
}
void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.color_frame_rate_cur, msg.color_frame_rate_avg, msg.color_frame_rate_min,
msg.color_frame_rate_max, msg.color_delay_ms_cur, msg.color_delay_ms_avg,
msg.color_delay_ms_min, msg.color_delay_ms_max);
}
void fillLeftColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.left_color_frame_rate_cur, msg.left_color_frame_rate_avg,
msg.left_color_frame_rate_min, msg.left_color_frame_rate_max,
msg.left_color_delay_ms_cur, msg.left_color_delay_ms_avg,
msg.left_color_delay_ms_min, msg.left_color_delay_ms_max);
}
void fillRightColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.right_color_frame_rate_cur, msg.right_color_frame_rate_avg,
msg.right_color_frame_rate_min, msg.right_color_frame_rate_max,
msg.right_color_delay_ms_cur, msg.right_color_delay_ms_avg,
msg.right_color_delay_ms_min, msg.right_color_delay_ms_max);
}
void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.depth_frame_rate_cur, msg.depth_frame_rate_avg, msg.depth_frame_rate_min,
msg.depth_frame_rate_max, msg.depth_delay_ms_cur, msg.depth_delay_ms_avg,
msg.depth_delay_ms_min, msg.depth_delay_ms_max);
}
void fillLeftIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.left_ir_frame_rate_cur, msg.left_ir_frame_rate_avg, msg.left_ir_frame_rate_min,
msg.left_ir_frame_rate_max, msg.left_ir_delay_ms_cur, msg.left_ir_delay_ms_avg,
msg.left_ir_delay_ms_min, msg.left_ir_delay_ms_max);
}
void fillRightIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.right_ir_frame_rate_cur, msg.right_ir_frame_rate_avg,
msg.right_ir_frame_rate_min, msg.right_ir_frame_rate_max, msg.right_ir_delay_ms_cur,
msg.right_ir_delay_ms_avg, msg.right_ir_delay_ms_min, msg.right_ir_delay_ms_max);
}
private:
void fillStatus(double &frame_rate_cur, double &frame_rate_avg, double &frame_rate_min,
double &frame_rate_max, double &delay_ms_cur, double &delay_ms_avg,
double &delay_ms_min, double &delay_ms_max) {
std::lock_guard<std::mutex> lock(mutex_);
frame_rate_cur = last_fps_;
frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
frame_rate_min = frame_count_ > 0 ? fps_min_ : 0;
frame_rate_max = frame_count_ > 0 ? fps_max_ : 0;
delay_ms_cur = last_delay_ms_;
delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
delay_ms_min = frame_count_ > 0 ? delay_min_ : 0;
delay_ms_max = frame_count_ > 0 ? delay_max_ : 0;
last_delay_ms_ = 0.0;
last_fps_ = 0.0;
frame_count_ = 0;
fps_sum_ = delay_sum_ = 0.0;
fps_max_ = delay_max_ = 0.0;
fps_min_ = delay_min_ = 0.0;
}
mutable std::mutex mutex_;
u_int64_t last_stream_timestamp_{0};
double last_delay_ms_{0.0};
double last_fps_{0.0};
int frame_count_{0};
double fps_sum_{0.0};
double delay_sum_{0.0};
double fps_max_{std::numeric_limits<double>::lowest()};
double fps_min_{std::numeric_limits<double>::max()};
double delay_max_{std::numeric_limits<double>::lowest()};
double delay_min_{std::numeric_limits<double>::max()};
LogLevel log_level_;
rclcpp::Logger logger_;
};
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+13 -18
View File
@@ -50,6 +50,7 @@
#include "libobsensor/ObSensor.hpp"
#include "orbbec_camera_msgs/msg/device_info.hpp"
#include "orbbec_camera_msgs/msg/device_status.hpp"
#include "orbbec_camera_msgs/msg/depth_filter_state.hpp"
#include "orbbec_camera_msgs/msg/depth_filters_status.hpp"
#include "orbbec_camera_msgs/srv/get_device_config.hpp"
@@ -76,7 +77,7 @@
#include "orbbec_camera/d2c_viewer.h"
#include "orbbec_camera/image_publisher.h"
#include "orbbec_camera/fps_counter.hpp"
#include "orbbec_camera/fps_delay_status.hpp"
#include "orbbec_camera/stream_status.hpp"
#include "orbbec_camera/timestamp_csv_logger.h"
#include "jpeg_decoder.h"
#include <std_msgs/msg/header.hpp>
@@ -235,17 +236,7 @@ class OBCameraNode {
return (color_info_manager_ && color_info_manager_->isCalibrated() && ir_info_manager_ &&
ir_info_manager_->isCalibrated());
}
void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
fps_delay_status_color_->fillColorStatus(status_msg);
fps_delay_status_left_color_->fillLeftColorStatus(status_msg);
fps_delay_status_right_color_->fillRightColorStatus(status_msg);
}
void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
fps_delay_status_depth_->fillDepthStatus(status_msg);
fps_delay_status_left_ir_->fillLeftIrStatus(status_msg);
fps_delay_status_right_ir_->fillRightIrStatus(status_msg);
}
void fillStreamStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg);
bool checkUserCalibrationReady() {
static bool first_check = true;
@@ -363,6 +354,14 @@ class OBCameraNode {
void setupPublishers();
void registerStreamStatus(const std::string& topic_name,
StreamStatusTracker::SubscriberCountFn subscriber_count);
void removeStreamStatus(const std::string& topic_name);
void recordStreamStatus(const std::string& topic_name,
const builtin_interfaces::msg::Time& stamp);
std::string resolveStreamStatusTopic(const std::string& topic_name) const;
std::string compressedStreamStatusTopic(const stream_index_pair& stream_index) const;
void syncSoftwareAlignment();
void publishDepthFiltersStatus();
@@ -1199,12 +1198,8 @@ class OBCameraNode {
std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_right_ir_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_left_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_right_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_depth_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_left_ir_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_right_ir_{nullptr};
std::map<std::string, std::shared_ptr<StreamStatusTracker>> stream_status_trackers_;
mutable std::mutex stream_status_mutex_;
std::string intra_camera_sync_reference_ = "";
std::string ae_reference_stream_;
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
-1
View File
@@ -157,7 +157,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
std::atomic<bool> is_reupdating_{false}; // Flag to track if we're in reupdate process
std::atomic<bool> delay_stream_start_after_reconnect_{false};
rclcpp::TimerBase::SharedPtr device_status_timer_ = nullptr;
int device_status_interval_hz = 2; // 2Hz
rclcpp::Publisher<orbbec_camera_msgs::msg::DeviceStatus>::SharedPtr device_status_pub_ = nullptr;
std::string node_name_;
bool force_ip_enable_{false};
orbbec_camera/include/orbbec_camera/stream_status.hpp
+80
View File
@@ -0,0 +1,80 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#pragma once
#include <builtin_interfaces/msg/time.hpp>
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <functional>
#include <mutex>
#include <string>
#include <utility>
#include "orbbec_camera_msgs/msg/stream_status.hpp"
namespace orbbec_camera {
class StreamStatusTracker {
public:
using SubscriberCountFn = std::function<size_t()>;
StreamStatusTracker(std::string topic_name, SubscriberCountFn subscriber_count)
: topic_name_(std::move(topic_name)),
subscriber_count_(std::move(subscriber_count)),
window_start_(std::chrono::steady_clock::now()) {}
void record(const builtin_interfaces::msg::Time& stamp) {
const auto now = std::chrono::system_clock::now();
const double now_ms = std::chrono::duration<double, std::milli>(now.time_since_epoch()).count();
const double stamp_ms =
static_cast<double>(stamp.sec) * 1000.0 + static_cast<double>(stamp.nanosec) / 1000000.0;
std::lock_guard<std::mutex> lock(mutex_);
published_count_++;
delay_sum_ms_ += now_ms - stamp_ms;
}
void fill(orbbec_camera_msgs::msg::StreamStatus& status) {
const bool has_subscribers = subscriber_count_ && subscriber_count_() > 0;
const auto now = std::chrono::steady_clock::now();
std::lock_guard<std::mutex> lock(mutex_);
const double window_seconds = std::chrono::duration<double>(now - window_start_).count();
status.topic_name = topic_name_;
status.has_subscribers = has_subscribers;
status.publish_rate_hz =
window_seconds > 0.0 ? static_cast<double>(published_count_) / window_seconds : 0.0;
status.delay_ms_avg =
published_count_ > 0 ? delay_sum_ms_ / static_cast<double>(published_count_) : 0.0;
published_count_ = 0;
delay_sum_ms_ = 0.0;
window_start_ = now;
}
private:
std::string topic_name_;
SubscriberCountFn subscriber_count_;
std::chrono::steady_clock::time_point window_start_;
std::mutex mutex_;
uint32_t published_count_{0};
double delay_sum_ms_{0.0};
};
} // namespace orbbec_camera