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

Fix device_status topic

Browse Source
This commit is contained in:
xiexun
2025-09-02 17:08:21 +08:00
parent c21ef8e926
commit 211edd146b
6 changed files with 136 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/fps_counter.hpp
+16 -2
View File
@@ -1,3 +1,19 @@
/*******************************************************************************
* 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>
@@ -5,11 +21,9 @@
#include <chrono>
#include <functional>
#include <string>
namespace orbbec_camera {
enum class LogLevel { DEBUG, INFO };
class FpsCounter {
public:
explicit FpsCounter(const std::string &name, rclcpp::Logger logger, int print_interval_sec = 1)
orbbec_camera/include/orbbec_camera/fps_delay_status.hpp
+59 -27
View File
@@ -1,3 +1,19 @@
/*******************************************************************************
* 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>
@@ -5,73 +21,87 @@
#include <chrono>
#include <functional>
#include <mutex>
namespace orbbec_camera {
class FpsDelayStatus {
public:
explicit FpsDelayStatus():last_frame_stamp_(std::chrono::steady_clock::now()){}
void tick() {
std::lock_guard<std::mutex> lock(mutex_);
auto now = std::chrono::steady_clock::now();
double dt = std::chrono::duration_cast<std::chrono::duration<double>>(now - last_frame_stamp_).count();
double fps = (dt > 0) ? (1.0 / dt) : 0.0;
double delay_ms = dt * 1000.0;
public:
explicit FpsDelayStatus(rclcpp::Logger logger) : log_level_(LogLevel::INFO), logger_(logger) {}
last_fps_ = fps;
last_delay_ms_ = delay_ms;
last_frame_stamp_ = now;
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){
void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
std::lock_guard<std::mutex> lock(mutex_);
msg.color_frame_rate_cur = last_fps_;
msg.color_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0.0;
msg.color_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
msg.color_frame_rate_min = fps_min_;
msg.color_frame_rate_max = fps_max_;
msg.color_delay_ms_cur = last_delay_ms_;
msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0.0;
msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
msg.color_delay_ms_min = delay_min_;
msg.color_delay_ms_max = delay_max_;
last_delay_ms_ = 0.0;
last_fps_ = 0.0;
frame_count_ = 0;
fps_sum_ = delay_sum_ = 0.0;
fps_max_ = delay_max_ = std::numeric_limits<double>::lowest();
fps_min_ = delay_min_ = std::numeric_limits<double>::max();
fps_max_ = delay_max_ = 0.0;
fps_min_ = delay_min_ = 0.0;
}
void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg){
void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
std::lock_guard<std::mutex> lock(mutex_);
msg.depth_frame_rate_cur = last_fps_;
msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0.0;
msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
msg.depth_frame_rate_min = fps_min_;
msg.depth_frame_rate_max = fps_max_;
msg.depth_delay_ms_cur = last_delay_ms_;
msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0.0;
msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
msg.depth_delay_ms_min = delay_min_;
msg.depth_delay_ms_max = delay_max_;
// RCLCPP_ERROR_STREAM(logger_, "Depth status: " << fps_sum_ << "," << frame_count_);
last_delay_ms_ = 0.0;
last_fps_ = 0.0;
frame_count_ = 0;
fps_sum_ = delay_sum_ = 0.0;
fps_max_ = delay_max_ = std::numeric_limits<double>::lowest();
fps_min_ = delay_min_ = std::numeric_limits<double>::max();
fps_max_ = delay_max_ = 0.0;
fps_min_ = delay_min_ = 0.0;
}
private:
private:
mutable std::mutex mutex_;
std::chrono::steady_clock::time_point last_frame_stamp_;
u_int64_t last_stream_timestamp_{0};
double last_delay_ms_{0.0};
double last_fps_{0.0};
@@ -83,6 +113,8 @@ private:
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
+17 -15
View File
@@ -165,7 +165,7 @@ class OBCameraNode {
// Safely expose the lock
template <typename Func>
auto withDeviceLock(Func &&func) -> decltype(func()) {
auto withDeviceLock(Func&& func) -> decltype(func()) {
std::lock_guard<std::recursive_mutex> lock(device_lock_);
return func();
}
@@ -183,19 +183,20 @@ class OBCameraNode {
void startGmslTrigger();
void stopGmslTrigger();
bool isParamCalibrated() const{
return (color_info_manager_ && color_info_manager_->isCalibrated() &&
ir_info_manager_ && ir_info_manager_->isCalibrated());
bool isParamCalibrated() const {
return (color_info_manager_ && color_info_manager_->isCalibrated() && ir_info_manager_ &&
ir_info_manager_->isCalibrated());
}
void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus &status_msg){
void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
fps_delay_status_color_->fillColorStatus(status_msg);
}
void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &status_msg){
void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
fps_delay_status_depth_->fillDepthStatus(status_msg);
status_msg.header.frame_id = camera_link_frame_id_;
}
void publishDeviceStatus(const orbbec_camera_msgs::msg::DeviceStatus &msg) {
void publishDeviceStatus(const orbbec_camera_msgs::msg::DeviceStatus& msg) {
if (device_status_pub_) {
device_status_pub_->publish(msg);
}
@@ -264,8 +265,9 @@ class OBCameraNode {
void setStreamsEnableCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
std::shared_ptr<std_srvs::srv::SetBool::Response> response);
void getStreamsEnableCallback(const std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Response> response);
void getStreamsEnableCallback(
const std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Response> response);
void getExposureCallback(const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response,
@@ -373,9 +375,9 @@ class OBCameraNode {
void switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request,
std::shared_ptr<SetString::Response>& response);
bool writeCustomerData(const std::string &data);
bool writeCustomerData(const std::string& data);
bool readCustomerData(std::string &out_data);
bool readCustomerData(std::string& out_data);
void writeCustomerDataCallback(const std::shared_ptr<SetString::Request>& request,
std::shared_ptr<SetString::Response>& response);
@@ -383,11 +385,11 @@ class OBCameraNode {
void readCustomerDataCallback(const std::shared_ptr<GetString::Request>& request,
std::shared_ptr<GetString::Response>& response);
void getCameraParamsCallback(const std::shared_ptr<GetCameraParams::Request>& request,
std::shared_ptr<GetCameraParams::Response>& response);
void getCameraParamsCallback(const std::shared_ptr<GetCameraParams::Request>& request,
std::shared_ptr<GetCameraParams::Response>& response);
void setCameraParamsCallback(const std::shared_ptr<SetCameraParams::Request>& request,
std::shared_ptr<SetCameraParams::Response>& response);
void setCameraParamsCallback(const std::shared_ptr<SetCameraParams::Request>& request,
std::shared_ptr<SetCameraParams::Response>& response);
void setIRLongExposureCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);