diff --git a/orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake b/orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake index c3bd1654..1e7eecfb 100644 --- a/orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake +++ b/orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake @@ -8,12 +8,12 @@ set(CMAKE_IMPORT_FILE_VERSION 1) # Import target "ob::OrbbecSDK" for configuration "Release" set_property(TARGET ob::OrbbecSDK APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE) set_target_properties(ob::OrbbecSDK PROPERTIES - IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.2" + IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.3" IMPORTED_SONAME_RELEASE "libOrbbecSDK.so.2" ) list(APPEND _cmake_import_check_targets ob::OrbbecSDK ) -list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.2" ) +list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.3" ) # Commands beyond this point should not need to know the version. set(CMAKE_IMPORT_FILE_VERSION) diff --git a/orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake b/orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake index 515957a9..b8c23359 100644 --- a/orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake +++ b/orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake @@ -9,19 +9,19 @@ # The variable CVF_VERSION must be set before calling configure_file(). -set(PACKAGE_VERSION "2.10.2") +set(PACKAGE_VERSION "2.10.3") if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION) set(PACKAGE_VERSION_COMPATIBLE FALSE) else() - if("2.10.2" MATCHES "^([0-9]+)\\.") + if("2.10.3" MATCHES "^([0-9]+)\\.") set(CVF_VERSION_MAJOR "${CMAKE_MATCH_1}") if(NOT CVF_VERSION_MAJOR VERSION_EQUAL 0) string(REGEX REPLACE "^0+" "" CVF_VERSION_MAJOR "${CVF_VERSION_MAJOR}") endif() else() - set(CVF_VERSION_MAJOR "2.10.2") + set(CVF_VERSION_MAJOR "2.10.3") endif() if(PACKAGE_FIND_VERSION_RANGE) diff --git a/orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so b/orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so index 072d0d21..a8b64642 100644 Binary files a/orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so and b/orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so differ diff --git a/orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so b/orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so index 434e72f4..7c141f41 100644 Binary files a/orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so and b/orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so differ diff --git a/orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so b/orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so index c155c3c9..0d41df77 100644 Binary files a/orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so and b/orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so differ diff --git a/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2 b/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2 index 6687316d..9e58fe5c 120000 --- a/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2 +++ b/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2 @@ -1 +1 @@ -libOrbbecSDK.so.2.10.2 \ No newline at end of file +libOrbbecSDK.so.2.10.3 \ No newline at end of file diff --git a/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.2 b/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.3 similarity index 68% rename from orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.2 rename to orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.3 index 1eb61970..ee235429 100644 Binary files a/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.2 and b/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.3 differ diff --git a/orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake b/orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake index c3bd1654..1e7eecfb 100644 --- a/orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake +++ b/orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake @@ -8,12 +8,12 @@ set(CMAKE_IMPORT_FILE_VERSION 1) # Import target "ob::OrbbecSDK" for configuration "Release" set_property(TARGET ob::OrbbecSDK APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE) set_target_properties(ob::OrbbecSDK PROPERTIES - IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.2" + IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.3" IMPORTED_SONAME_RELEASE "libOrbbecSDK.so.2" ) list(APPEND _cmake_import_check_targets ob::OrbbecSDK ) -list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.2" ) +list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.3" ) # Commands beyond this point should not need to know the version. set(CMAKE_IMPORT_FILE_VERSION) diff --git a/orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake b/orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake index 515957a9..b8c23359 100644 --- a/orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake +++ b/orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake @@ -9,19 +9,19 @@ # The variable CVF_VERSION must be set before calling configure_file(). -set(PACKAGE_VERSION "2.10.2") +set(PACKAGE_VERSION "2.10.3") if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION) set(PACKAGE_VERSION_COMPATIBLE FALSE) else() - if("2.10.2" MATCHES "^([0-9]+)\\.") + if("2.10.3" MATCHES "^([0-9]+)\\.") set(CVF_VERSION_MAJOR "${CMAKE_MATCH_1}") if(NOT CVF_VERSION_MAJOR VERSION_EQUAL 0) string(REGEX REPLACE "^0+" "" CVF_VERSION_MAJOR "${CVF_VERSION_MAJOR}") endif() else() - set(CVF_VERSION_MAJOR "2.10.2") + set(CVF_VERSION_MAJOR "2.10.3") endif() if(PACKAGE_FIND_VERSION_RANGE) diff --git a/orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so b/orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so index cf1d5f96..89abe45d 100644 Binary files a/orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so and b/orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so differ diff --git a/orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so b/orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so index 14f73eb6..2ac3f108 100644 Binary files a/orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so and b/orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so differ diff --git a/orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so b/orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so index b3e5608e..5e095fc6 100644 Binary files a/orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so and b/orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so differ diff --git a/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2 b/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2 index 6687316d..9e58fe5c 120000 --- a/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2 +++ b/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2 @@ -1 +1 @@ -libOrbbecSDK.so.2.10.2 \ No newline at end of file +libOrbbecSDK.so.2.10.3 \ No newline at end of file diff --git a/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.2 b/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.3 similarity index 68% rename from orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.2 rename to orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.3 index ba8936d3..b34883cb 100644 Binary files a/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.2 and b/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.3 differ diff --git a/orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp b/orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp index 0986ae40..874252d7 100644 --- a/orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp +++ b/orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp @@ -193,14 +193,12 @@ class CameraExampleNode : public rclcpp::Node { void deviceStatusCallback(const orbbec_camera_msgs::msg::DeviceStatus::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "-------orbbec camera real-time status-------"); RCLCPP_INFO(this->get_logger(), "--------------------------------------------"); - RCLCPP_INFO(this->get_logger(), "Color Frame Rate: Cur: %.2f, Avg: %.2f", - msg->color_frame_rate_cur, msg->color_frame_rate_avg); - RCLCPP_INFO(this->get_logger(), "Depth Frame Rate: Cur: %.2f, Avg: %.2f", - msg->depth_frame_rate_cur, msg->depth_frame_rate_avg); - RCLCPP_INFO(this->get_logger(), "Color Delay (ms): Cur: %.2f, Avg: %.2f", - msg->color_delay_ms_cur, msg->color_delay_ms_avg); - RCLCPP_INFO(this->get_logger(), "Depth Delay (ms): Cur: %.2f, Avg: %.2f", - msg->depth_delay_ms_cur, msg->depth_delay_ms_avg); + for (const auto &stream : msg->streams) { + RCLCPP_INFO(this->get_logger(), + "Topic: %s, Subscribers: %s, Rate: %.2f Hz, Avg Delay: %.2f ms", + stream.topic_name.c_str(), stream.has_subscribers ? "true" : "false", + stream.publish_rate_hz, stream.delay_ms_avg); + } RCLCPP_INFO(this->get_logger(), "Device Online: %s", msg->device_online ? "True" : "False"); RCLCPP_INFO(this->get_logger(), "Connection Type: %s", msg->connection_type.c_str()); diff --git a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp b/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp deleted file mode 100644 index ecb86a92..00000000 --- a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp +++ /dev/null @@ -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 -#include "orbbec_camera_msgs/msg/device_status.hpp" -#include -#include -#include -#include -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 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(now2.time_since_epoch()).count(); - double delay_ms = - static_cast(ms_since_epoch) - static_cast(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 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::lowest()}; - double fps_min_{std::numeric_limits::max()}; - double delay_max_{std::numeric_limits::lowest()}; - double delay_min_{std::numeric_limits::max()}; - - LogLevel log_level_; - rclcpp::Logger logger_; -}; - -} // namespace orbbec_camera diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index abe66fde..2f1be9f3 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,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 +78,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 @@ -136,6 +138,11 @@ #define DEVICE_PATH "/dev/camsync" namespace orbbec_camera { +class StreamConfigurationError : public std::runtime_error { + public: + explicit StreamConfigurationError(const std::string& message) : std::runtime_error(message) {} +}; + using GetDeviceConfig = orbbec_camera_msgs::srv::GetDeviceConfig; using GetActionConfig = orbbec_camera_msgs::srv::GetActionConfig; using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo; @@ -235,17 +242,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 +360,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(); @@ -676,6 +681,7 @@ class OBCameraNode { static bool isGemini435LePID(uint32_t pid); static bool isPublishMetaData(uint32_t pid); static bool isDabaiASeriesForHwD2C(uint32_t pid); + static bool isLingBotSupportedPID(uint32_t pid); static bool isDepthWorkModeDevices(uint32_t pid); static bool isnotLaserDevices(uint32_t pid); @@ -1199,12 +1205,8 @@ class OBCameraNode { std::unique_ptr fps_counter_left_ir_{nullptr}; std::unique_ptr fps_counter_right_ir_{nullptr}; - std::unique_ptr fps_delay_status_color_{nullptr}; - std::unique_ptr fps_delay_status_left_color_{nullptr}; - std::unique_ptr fps_delay_status_right_color_{nullptr}; - std::unique_ptr fps_delay_status_depth_{nullptr}; - std::unique_ptr fps_delay_status_left_ir_{nullptr}; - std::unique_ptr fps_delay_status_right_ir_{nullptr}; + std::map> stream_status_trackers_; + mutable std::mutex stream_status_mutex_; std::string intra_camera_sync_reference_ = ""; std::string ae_reference_stream_; diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h index a132d177..6b594f13 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -114,6 +114,7 @@ class OBCameraNodeDriver : public rclcpp::Node { std::atomic_bool is_alive_{false}; std::atomic_bool device_connected_{false}; std::atomic_bool device_connecting_{false}; + std::atomic_bool stream_configuration_error_{false}; std::string serial_number_; std::string device_unique_id_; std::string usb_port_; @@ -157,7 +158,6 @@ class OBCameraNodeDriver : public rclcpp::Node { std::atomic is_reupdating_{false}; // Flag to track if we're in reupdate process std::atomic delay_stream_start_after_reconnect_{false}; rclcpp::TimerBase::SharedPtr device_status_timer_ = nullptr; - int device_status_interval_hz = 2; // 2Hz rclcpp::Publisher::SharedPtr device_status_pub_ = nullptr; std::string node_name_; bool force_ip_enable_{false}; diff --git a/orbbec_camera/include/orbbec_camera/stream_status.hpp b/orbbec_camera/include/orbbec_camera/stream_status.hpp new file mode 100644 index 00000000..7580c501 --- /dev/null +++ b/orbbec_camera/include/orbbec_camera/stream_status.hpp @@ -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 + +#include +#include +#include +#include +#include +#include +#include + +#include "orbbec_camera_msgs/msg/stream_status.hpp" + +namespace orbbec_camera { + +class StreamStatusTracker { + public: + using SubscriberCountFn = std::function; + + 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(now.time_since_epoch()).count(); + const double stamp_ms = + static_cast(stamp.sec) * 1000.0 + static_cast(stamp.nanosec) / 1000000.0; + + std::lock_guard 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 lock(mutex_); + const double window_seconds = std::chrono::duration(now - window_start_).count(); + status.topic_name = topic_name_; + status.has_subscribers = has_subscribers; + status.publish_rate_hz = + window_seconds > 0.0 ? static_cast(published_count_) / window_seconds : 0.0; + status.delay_ms_avg = + published_count_ > 0 ? delay_sum_ms_ / static_cast(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 diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index c105ddd5..b56d603e 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/orbbec_camera/launch/dabai_a.launch.py @@ -43,7 +43,7 @@ def load_parameters(context, args): yaml_params = load_yaml(config_file_path) default_params = merge_params(default_params, yaml_params) skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', - 'depth_colorizer_mode'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -148,7 +148,6 @@ def generate_launch_description(): DeclareLaunchArgument('ir_exposure', default_value='-1'), DeclareLaunchArgument('ir_gain', default_value='-1'), DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), - DeclareLaunchArgument('ir_brightness', default_value='-1'), DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('enable_accel_data_correction', default_value='true'), @@ -194,6 +193,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), DeclareLaunchArgument('enable_false_positive_filter', default_value='false'), + DeclareLaunchArgument('enable_enhanced_depth', default_value='false'), + DeclareLaunchArgument('enhanced_depth_model_path', default_value=''), + DeclareLaunchArgument('enhanced_depth_confidence_threshold', default_value='51'), DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'), DeclareLaunchArgument('threshold_filter_max', default_value='-1'), @@ -214,6 +216,7 @@ def generate_launch_description(): DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH + DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame, color_frame, ANY or disable DeclareLaunchArgument('diagnostic_period', default_value='1.0'), DeclareLaunchArgument('depth_precision', default_value=''), DeclareLaunchArgument('device_preset', default_value='Standard'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index 6dc42693..5eab9010 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/orbbec_camera/launch/dabai_al.launch.py @@ -43,7 +43,7 @@ def load_parameters(context, args): yaml_params = load_yaml(config_file_path) default_params = merge_params(default_params, yaml_params) skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', - 'depth_colorizer_mode'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -148,7 +148,6 @@ def generate_launch_description(): DeclareLaunchArgument('ir_exposure', default_value='-1'), DeclareLaunchArgument('ir_gain', default_value='-1'), DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), - DeclareLaunchArgument('ir_brightness', default_value='-1'), DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('enable_accel_data_correction', default_value='true'), @@ -194,6 +193,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), DeclareLaunchArgument('enable_false_positive_filter', default_value='false'), + DeclareLaunchArgument('enable_enhanced_depth', default_value='false'), + DeclareLaunchArgument('enhanced_depth_model_path', default_value=''), + DeclareLaunchArgument('enhanced_depth_confidence_threshold', default_value='51'), DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'), DeclareLaunchArgument('threshold_filter_max', default_value='-1'), @@ -214,6 +216,7 @@ def generate_launch_description(): DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH + DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame, color_frame, ANY or disable DeclareLaunchArgument('diagnostic_period', default_value='1.0'), DeclareLaunchArgument('enable_laser', default_value='true'), DeclareLaunchArgument('depth_precision', default_value=''), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index 197fd52b..c0149aec 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/orbbec_camera/launch/gemini345.launch.py @@ -43,7 +43,7 @@ def load_parameters(context, args): yaml_params = load_yaml(config_file_path) default_params = merge_params(default_params, yaml_params) skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', - 'depth_colorizer_mode'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -147,7 +147,6 @@ def generate_launch_description(): DeclareLaunchArgument('ir_exposure', default_value='-1'), DeclareLaunchArgument('ir_gain', default_value='-1'), DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), - DeclareLaunchArgument('ir_brightness', default_value='-1'), DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('enable_accel_data_correction', default_value='true'), @@ -193,6 +192,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), DeclareLaunchArgument('enable_false_positive_filter', default_value='true'), + DeclareLaunchArgument('enable_enhanced_depth', default_value='false'), + DeclareLaunchArgument('enhanced_depth_model_path', default_value=''), + DeclareLaunchArgument('enhanced_depth_confidence_threshold', default_value='51'), DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'), DeclareLaunchArgument('threshold_filter_max', default_value='-1'), @@ -213,6 +215,7 @@ def generate_launch_description(): DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH + DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame, color_frame, ANY or disable DeclareLaunchArgument('diagnostic_period', default_value='1.0'), DeclareLaunchArgument('depth_precision', default_value=''), DeclareLaunchArgument('device_preset', default_value=''), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index cf22d547..1a84e50d 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/orbbec_camera/launch/gemini345_lg.launch.py @@ -43,7 +43,7 @@ def load_parameters(context, args): yaml_params = load_yaml(config_file_path) default_params = merge_params(default_params, yaml_params) skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', - 'depth_colorizer_mode'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -148,7 +148,6 @@ def generate_launch_description(): DeclareLaunchArgument('ir_exposure', default_value='-1'), DeclareLaunchArgument('ir_gain', default_value='-1'), DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), - DeclareLaunchArgument('ir_brightness', default_value='-1'), DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('enable_accel_data_correction', default_value='true'), @@ -194,6 +193,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), DeclareLaunchArgument('enable_false_positive_filter', default_value='true'), + DeclareLaunchArgument('enable_enhanced_depth', default_value='false'), + DeclareLaunchArgument('enhanced_depth_model_path', default_value=''), + DeclareLaunchArgument('enhanced_depth_confidence_threshold', default_value='51'), DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'), DeclareLaunchArgument('threshold_filter_max', default_value='-1'), @@ -214,6 +216,7 @@ def generate_launch_description(): DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH + DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame, color_frame, ANY or disable DeclareLaunchArgument('diagnostic_period', default_value='1.0'), DeclareLaunchArgument('enable_laser', default_value='true'), DeclareLaunchArgument('depth_precision', default_value=''), diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py index 42b757c5..af71f294 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/orbbec_camera/launch/gemini435_le.launch.py @@ -171,7 +171,6 @@ def generate_launch_description(): DeclareLaunchArgument('ir_exposure', default_value='-1'), DeclareLaunchArgument('ir_gain', default_value='-1'), DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), - DeclareLaunchArgument('ir_brightness', default_value='-1'), DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('enable_accel_data_correction', default_value='true'), diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 93d77a94..46e045b0 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -130,7 +130,6 @@ def generate_launch_description(): DeclareLaunchArgument('right_color_qos_history', default_value='default'), DeclareLaunchArgument('right_color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), - DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('color_rotation', default_value='-1'),#color rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('color_flip', default_value='false'), DeclareLaunchArgument('color_mirror', default_value='false'), @@ -138,11 +137,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_ae_roi_right', default_value='-1'), DeclareLaunchArgument('color_ae_roi_top', default_value='-1'), DeclareLaunchArgument('color_ae_roi_bottom', default_value='-1'), - DeclareLaunchArgument('color_exposure', default_value='-1'), - DeclareLaunchArgument('color_gain', default_value='-1'), DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'), DeclareLaunchArgument('color_white_balance', default_value='-1'), - DeclareLaunchArgument('color_ae_max_exposure', default_value='-1'), DeclareLaunchArgument('color_brightness', default_value='-1'), DeclareLaunchArgument('color_sharpness', default_value='-1'), DeclareLaunchArgument('color_gamma', default_value='-1'), @@ -207,12 +203,13 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_mirror', default_value='false'), DeclareLaunchArgument('enable_right_ir_sequence_id_filter', default_value='false'), DeclareLaunchArgument('right_ir_sequence_id_filter_id', default_value='-1'), - # Gemini 301 color, depth, and IR streams share one auto-exposure switch. - DeclareLaunchArgument('enable_auto_exposure', default_value='true'), + # Gemini 301 color/depth/IR share these AE, exposure and gain controls. + # Use only these entries in YAML too; color exposure/max exposure values + # must be multiplied by 100 when migrating. Existing IR units are unchanged. + DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), DeclareLaunchArgument('ir_gain', default_value='-1'), DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), - DeclareLaunchArgument('ir_brightness', default_value='-1'), DeclareLaunchArgument('publish_tf', default_value='true'), DeclareLaunchArgument('tf_publish_rate', default_value='0.0'), DeclareLaunchArgument('ir_info_url', default_value=''), diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index 5b80cd0e..f98091af 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -192,7 +192,6 @@ def generate_launch_description(): DeclareLaunchArgument('ir_exposure', default_value='-1'), DeclareLaunchArgument('ir_gain', default_value='-1'), DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), - DeclareLaunchArgument('ir_brightness', default_value='-1'), DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('enable_accel_data_correction', default_value='true'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index 4624d300..09dcb83c 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py @@ -190,7 +190,6 @@ def generate_launch_description(): DeclareLaunchArgument('ir_exposure', default_value='-1'), DeclareLaunchArgument('ir_gain', default_value='-1'), DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), - DeclareLaunchArgument('ir_brightness', default_value='-1'), DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('enable_accel_data_correction', default_value='true'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 14d588ce..b7faff7f 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -650,7 +650,7 @@ void OBCameraNode::publishDepthFiltersStatus() { if (disp_outliers_filter_supported) { append_unique_filter_name("DispOutliersFilter"); } - if (isGemini330SeriesPID(pid_)) { + if (isLingBotSupportedPID(pid_)) { append_unique_filter_name("EnhancedDepthFilter"); } @@ -783,13 +783,6 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic fps_counter_depth_->setLogLevel(log_level); fps_counter_left_ir_->setLogLevel(log_level); fps_counter_right_ir_->setLogLevel(log_level); - - fps_delay_status_color_ = std::make_unique(logger_); - fps_delay_status_left_color_ = std::make_unique(logger_); - fps_delay_status_right_color_ = std::make_unique(logger_); - fps_delay_status_depth_ = std::make_unique(logger_); - fps_delay_status_left_ir_ = std::make_unique(logger_); - fps_delay_status_right_ir_ = std::make_unique(logger_); } template @@ -1011,21 +1004,24 @@ void OBCameraNode::setupDevices() { std::string token; std::vector values; values.reserve(4); - while (std::getline(iss, token, ',')) { - values.push_back(std::stoi(token)); + try { + while (std::getline(iss, token, ',')) { + values.push_back(std::stoi(token)); + } + } catch (const std::exception &e) { + throw StreamConfigurationError("Invalid preset_resolution_config '" + + preset_resolution_config_ + "': " + e.what()); } - if (values.size() >= 4) { - presetResolutionConfig.width = values[0]; - presetResolutionConfig.height = values[1]; - presetResolutionConfig.irDecimationFactor = values[2]; - presetResolutionConfig.depthDecimationFactor = values[3]; - } else { - RCLCPP_WARN_STREAM( - logger_, - "Invalid preset_resolution_config parameter. " - "Expected format: width,height,ir_decimation_factor,depth_decimation_factor"); + if (values.size() < 4) { + throw StreamConfigurationError( + "Invalid preset_resolution_config '" + preset_resolution_config_ + + "'. Expected format: width,height,ir_decimation_factor,depth_decimation_factor"); } + presetResolutionConfig.width = values[0]; + presetResolutionConfig.height = values[1]; + presetResolutionConfig.irDecimationFactor = values[2]; + presetResolutionConfig.depthDecimationFactor = values[3]; RCLCPP_INFO_STREAM( logger_, "Set preset resolution config: " @@ -1473,7 +1469,9 @@ void OBCameraNode::setupDevices() { logger_, "Current color gain: " << device_->getIntProperty(OB_PROP_COLOR_GAIN_INT))); } } - if (color_mjpeg_quality_ != -1) { + if (color_mjpeg_quality_ != -1 && + (format_[COLOR] == OB_FORMAT_UNKNOWN || format_[COLOR] == OB_FORMAT_MJPG || + format_[COLOR] == OB_FORMAT_MJPEG)) { if (!device_->isPropertySupported(OB_PROP_MJPEG_QUALITY_INT, OB_PERMISSION_WRITE)) { RCLCPP_WARN_STREAM(logger_, "color_mjpeg_quality is not supported by this device"); } else { @@ -1489,6 +1487,9 @@ void OBCameraNode::setupDevices() { "Current color MJPEG quality: " << device_->getIntProperty(OB_PROP_MJPEG_QUALITY_INT))); } } + } else if (color_mjpeg_quality_ != -1) { + RCLCPP_WARN_STREAM(logger_, "color_mjpeg_quality is ignored because color format is " + << format_str_[COLOR] << "; MJPG/MJPEG is required"); } if (should_apply_launch_config("enable_color_auto_exposure_priority") && device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT, OB_PERMISSION_WRITE)) { @@ -1735,8 +1736,7 @@ void OBCameraNode::setupDevices() { "Current depth auto exposure priority: " << (device_->getIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF"))); } - if ((should_apply_launch_config("enable_auto_exposure") || - should_apply_launch_config("enable_ir_auto_exposure")) && + if (should_apply_launch_config("enable_ir_auto_exposure") && device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) { TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_); TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM( @@ -1988,9 +1988,10 @@ void OBCameraNode::setupDevices() { TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEVICE_AE_STRATEGY_INT, (ae_strategy_ == "motion" ? 1 : 0)); TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM( - logger_, "Current AE Strategy: " - << (device_->getIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT) == 1 ? "Motion" - : "Default"))); + logger_, + "Current AE Strategy: " << (device_->getIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT) == 1 + ? "Motion" + : "Default"))); } if (should_apply_launch_config("ae_reference_stream") && @@ -2002,8 +2003,7 @@ void OBCameraNode::setupDevices() { TRY_EXECUTE_BLOCK({ auto current_ae_reference = device_->getIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT); RCLCPP_INFO_STREAM( - logger_, - "Current AE Reference: " << (current_ae_reference == 0 ? "Depth" : "Color")); + logger_, "Current AE Reference: " << (current_ae_reference == 0 ? "Depth" : "Color")); }); } } @@ -3130,6 +3130,12 @@ void OBCameraNode::setupIrPostProcessFilter() { } void OBCameraNode::setupUndistortionFilters() { + // LingBot requires color undistortion before D2C alignment on Dabai A series devices. + if (enable_enhanced_depth_.load() && isDabaiASeriesForHwD2C(pid_)) { + enable_undistortion_[COLOR] = true; + RCLCPP_INFO(logger_, "Enable color undistortion for LingBot enhanced depth filter"); + } + auto remove_undistortion_filter = [](std::vector> &filters) { filters.erase(std::remove_if(filters.begin(), filters.end(), [](const std::shared_ptr &filter) { @@ -3612,7 +3618,6 @@ void OBCameraNode::setupProfiles() { supported_profiles_[elem].emplace_back(profile); } std::shared_ptr selected_profile; - std::shared_ptr default_profile; try { if (is_playback_device_) { selected_profile = profiles->getProfile(0)->as(); @@ -3654,31 +3659,25 @@ void OBCameraNode::setupProfiles() { << ", Height: " << height_[elem] << ", FPS: " << fps_[elem] << ", Format: " << magic_enum::enum_name(format_[elem])); RCLCPP_ERROR(logger_, - "Error: The device might be connected via USB 2.0. Please verify your " - "configuration and try again. The current process will now exit."); + "The requested stream profile is invalid. Please correct the stream " + "configuration and restart the node."); RCLCPP_INFO_STREAM(logger_, "Available profiles:"); printSensorProfiles(sensor); - RCLCPP_ERROR(logger_, "Failed to configure the requested stream profile, exiting."); - exit(-1); + throw StreamConfigurationError( + "Failed to configure the requested " + stream_name_[elem] + + " stream profile: " + orbbec_camera::formatObErrorWithStatus(ex)); } if (!selected_profile) { - RCLCPP_WARN_STREAM(logger_, - "Requested stream configuration is not supported by the device: " - << "stream=" << magic_enum::enum_name(elem.first) - << ", stream_index=" << elem.second << ", width=" << width_[elem] - << ", height=" << height_[elem] << ", fps=" << fps_[elem] - << ", format=" << magic_enum::enum_name(format_[elem])); - if (default_profile) { - RCLCPP_WARN_STREAM(logger_, "Using the default profile instead"); - RCLCPP_WARN_STREAM(logger_, "Default profile FPS: " << default_profile->getFps()); - selected_profile = default_profile; - } else { - RCLCPP_ERROR_STREAM(logger_, "No default profile found, disabling stream " - << magic_enum::enum_name(elem.first)); - enable_stream_[elem] = false; - continue; - } + const auto message = "Requested " + stream_name_[elem] + + " stream profile is not supported by the device: " + "width=" + + std::to_string(width_[elem]) + + ", height=" + std::to_string(height_[elem]) + + ", fps=" + std::to_string(fps_[elem]) + + ", format=" + std::string(magic_enum::enum_name(format_[elem])); + RCLCPP_ERROR_STREAM(logger_, message); + throw StreamConfigurationError(message); } CHECK_NOTNULL(selected_profile); stream_profile_[elem] = selected_profile; @@ -3712,7 +3711,7 @@ void OBCameraNode::setupProfiles() { std::string stream_fps_message; if (!validate301SeriesStreamFrameRates(fps_, stream_fps_message)) { RCLCPP_ERROR_STREAM(logger_, stream_fps_message); - throw std::runtime_error(stream_fps_message); + throw StreamConfigurationError(stream_fps_message); } // IMU @@ -4340,6 +4339,7 @@ void OBCameraNode::startStreams() { "Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e)); RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again"); enable_stream_[INFRA0] = false; + setupImagePublisher(INFRA0); setupPipelineConfig(); pipeline_->start(pipeline_config_, [this](const std::shared_ptr &frame_set) { onNewFrameSetCallback(frame_set); @@ -4700,7 +4700,7 @@ void OBCameraNode::getParameters() { "right_color_frame_queue_max_frames", 10); const auto validate_queue_capacity = [](const char *name, int capacity) { if (capacity < 1) { - throw std::invalid_argument(std::string(name) + " must be greater than zero"); + throw StreamConfigurationError(std::string(name) + " must be greater than zero"); } }; validate_queue_capacity("color_frame_queue_max_frames", color_frame_queue_max_frames_); @@ -4747,12 +4747,12 @@ void OBCameraNode::getParameters() { if (image_qos_history_[stream_index] != "DEFAULT" && image_qos_history_[stream_index] != "KEEP_LAST" && image_qos_history_[stream_index] != "KEEP_ALL") { - throw std::invalid_argument(param_name + " must be DEFAULT, KEEP_LAST, or KEEP_ALL"); + throw StreamConfigurationError(param_name + " must be DEFAULT, KEEP_LAST, or KEEP_ALL"); } param_name = stream_name_[stream_index] + "_qos_depth"; setAndGetNodeParameter(image_qos_depth_[stream_index], param_name, -1); if (image_qos_depth_[stream_index] == 0 || image_qos_depth_[stream_index] < -1) { - throw std::invalid_argument(param_name + " must be -1 or greater than zero"); + throw StreamConfigurationError(param_name + " must be -1 or greater than zero"); } param_name = stream_name_[stream_index] + "_camera_info_qos"; setAndGetNodeParameter(camera_info_qos_[stream_index], param_name, "default"); @@ -4879,11 +4879,7 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(mean_intensity_set_point_, "mean_intensity_set_point", depth_brightness_); setAndGetNodeParameter(depth_precision_str_, "depth_precision", ""); - setAndGetNodeParameter(enable_ir_auto_exposure_, - isLaunchParamProvided("enable_auto_exposure") - ? "enable_auto_exposure" - : "enable_ir_auto_exposure", - true); + setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true); setAndGetNodeParameter(ir_exposure_, "ir_exposure", -1); setAndGetNodeParameter(ir_gain_, "ir_gain", -1); setAndGetNodeParameter(ir_ae_max_exposure_, "ir_ae_max_exposure", -1); @@ -5192,7 +5188,7 @@ void OBCameraNode::setupTopics() { if (enable_enhanced_depth_.load()) { std::string message; if (!ensureEnhancedDepthFilter(message)) { - throw std::runtime_error(message); + throw StreamConfigurationError(message); } } setupCameraInfo(); @@ -5201,6 +5197,8 @@ void OBCameraNode::setupTopics() { setupPublishers(); setupDiagnosticUpdater(); exportConfigJsonIfRequested(); + } catch (const StreamConfigurationError &) { + throw; } catch (const ob::Error &e) { RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e)); @@ -5399,8 +5397,8 @@ bool OBCameraNode::validateEnhancedDepthFilterConfig(std::string &message) const constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800"; constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16"; - if (!isGemini330SeriesPID(pid_)) { - message = "Enhanced depth filter is only supported by Gemini 330 series devices"; + if (!isLingBotSupportedPID(pid_)) { + message = "Enhanced depth filter is only supported by Gemini 330 and Dabai A series devices"; return false; } @@ -5722,12 +5720,71 @@ void OBCameraNode::setupCameraInfo() { } } +std::string OBCameraNode::resolveStreamStatusTopic(const std::string &topic_name) const { + return node_->get_node_topics_interface()->resolve_topic_name(topic_name); +} + +std::string OBCameraNode::compressedStreamStatusTopic(const stream_index_pair &stream_index) const { + const std::string topic = stream_name_.at(stream_index) + "/image_raw"; + return topic + (stream_index == DEPTH ? "/compressedDepth" : "/compressed"); +} + +void OBCameraNode::registerStreamStatus(const std::string &topic_name, + StreamStatusTracker::SubscriberCountFn subscriber_count) { + const auto resolved_topic_name = resolveStreamStatusTopic(topic_name); + auto tracker = + std::make_shared(resolved_topic_name, std::move(subscriber_count)); + std::lock_guard lock(stream_status_mutex_); + stream_status_trackers_[topic_name] = std::move(tracker); +} + +void OBCameraNode::removeStreamStatus(const std::string &topic_name) { + std::lock_guard lock(stream_status_mutex_); + stream_status_trackers_.erase(topic_name); +} + +void OBCameraNode::recordStreamStatus(const std::string &topic_name, + const builtin_interfaces::msg::Time &stamp) { + std::shared_ptr tracker; + { + std::lock_guard lock(stream_status_mutex_); + const auto iter = stream_status_trackers_.find(topic_name); + if (iter == stream_status_trackers_.end()) { + return; + } + tracker = iter->second; + } + tracker->record(stamp); +} + +void OBCameraNode::fillStreamStatus(orbbec_camera_msgs::msg::DeviceStatus &status_msg) { + std::vector> trackers; + { + std::lock_guard lock(stream_status_mutex_); + trackers.reserve(stream_status_trackers_.size()); + for (const auto &[topic_name, tracker] : stream_status_trackers_) { + (void)topic_name; + trackers.push_back(tracker); + } + } + + status_msg.streams.clear(); + status_msg.streams.reserve(trackers.size()); + for (const auto &tracker : trackers) { + status_msg.streams.emplace_back(); + tracker->fill(status_msg.streams.back()); + } +} + void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { const std::string topic = stream_name_[stream_index] + "/image_raw"; if (!enable_stream_[stream_index]) { releaseGlobalImageTransportPublisher(*node_, topic); image_publishers_.erase(stream_index); compressed_image_publishers_.erase(stream_index); + removeStreamStatus(topic); + removeStreamStatus(topic + "/compressed"); + removeStreamStatus(compressedStreamStatusTopic(stream_index)); return; } @@ -5735,6 +5792,7 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { const bool is_mjpg_color_stream = (stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) && format_[stream_index] == OB_FORMAT_MJPG; + const bool uses_image_transport = !use_intra_process_ && !is_mjpg_color_stream; if (use_intra_process_ || is_mjpg_color_stream) { releaseGlobalImageTransportPublisher(*node_, topic); image_publishers_[stream_index] = @@ -5745,13 +5803,33 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { } RCLCPP_INFO_STREAM(logger_, topic << " QoS: " << getRMWQosProfileDescription(image_qos_profile)); + const auto image_publisher = image_publishers_.at(stream_index); + if (uses_image_transport) { + registerStreamStatus(topic, [this, topic]() { return node_->count_subscribers(topic); }); + } else { + registerStreamStatus(topic, [image_publisher]() { + return image_publisher ? image_publisher->get_subscription_count() : 0; + }); + } + if (is_mjpg_color_stream) { compressed_image_publishers_[stream_index] = node_->create_publisher( topic + "/compressed", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile)); + const auto compressed_publisher = compressed_image_publishers_.at(stream_index); + registerStreamStatus(topic + "/compressed", [compressed_publisher]() { + return compressed_publisher ? compressed_publisher->get_subscription_count() : 0; + }); } else { compressed_image_publishers_.erase(stream_index); + removeStreamStatus(compressedStreamStatusTopic(stream_index)); + if (uses_image_transport) { + const auto compressed_topic = compressedStreamStatusTopic(stream_index); + registerStreamStatus(compressed_topic, [this, compressed_topic]() { + return node_->count_subscribers(compressed_topic); + }); + } } } @@ -5785,11 +5863,23 @@ void OBCameraNode::setupPublishers() { "depth_registered/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile), point_cloud_qos_profile)); + const auto publisher = depth_registration_cloud_pub_; + registerStreamStatus("depth_registered/points", [publisher]() { + return publisher ? publisher->get_subscription_count() : 0; + }); + } else { + removeStreamStatus("depth_registered/points"); } if (enable_point_cloud_) { depth_cloud_pub_ = node_->create_publisher( "depth/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile), point_cloud_qos_profile)); + const auto publisher = depth_cloud_pub_; + registerStreamStatus("depth/points", [publisher]() { + return publisher ? publisher->get_subscription_count() : 0; + }); + } else { + removeStreamStatus("depth/points"); } auto device_info = device_->getDeviceInfo(); CHECK_NOTNULL(device_info.get()); @@ -5834,6 +5924,9 @@ void OBCameraNode::setupPublishers() { } imu_gyro_accel_publisher_ = node_->create_publisher( topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos)); + const auto publisher = imu_gyro_accel_publisher_; + registerStreamStatus( + topic_name, [publisher]() { return publisher ? publisher->get_subscription_count() : 0; }); topic_name = stream_name_[GYRO] + "/imu_info"; imu_info_publishers_[GYRO] = node_->create_publisher( topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos)); @@ -5852,6 +5945,10 @@ void OBCameraNode::setupPublishers() { } imu_publishers_[stream_index] = node_->create_publisher( data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos)); + const auto publisher = imu_publishers_.at(stream_index); + registerStreamStatus(data_topic_name, [publisher]() { + return publisher ? publisher->get_subscription_count() : 0; + }); data_topic_name = stream_name_[stream_index] + "/imu_info"; imu_info_publishers_[stream_index] = node_->create_publisher( @@ -6196,6 +6293,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr &f RCLCPP_ERROR_STREAM(logger_, "Failed to save point cloud: " << e.what()); } } + recordStreamStatus("depth/points", point_cloud_msg->header.stamp); depth_cloud_pub_->publish(std::move(point_cloud_msg)); } @@ -6332,6 +6430,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr } } + recordStreamStatus("depth_registered/points", point_cloud_msg->header.stamp); depth_registration_cloud_pub_->publish(std::move(point_cloud_msg)); } std::shared_ptr OBCameraNode::processIrFrameFilter(std::shared_ptr &frame) { @@ -7159,9 +7258,17 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, } }; CHECK_NOTNULL(image_publishers_[stream_index]); - const bool has_raw_image_subscriber = - image_publishers_[stream_index]->get_subscription_count() > 0; + const bool has_explicit_compressed_publisher = + compressed_image_publishers_.count(stream_index) > 0 && + compressed_image_publishers_.at(stream_index); + bool has_raw_image_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0; + if (!use_intra_process_ && !has_explicit_compressed_publisher) { + has_raw_image_subscriber = + node_->count_subscribers(stream_name_[stream_index] + "/image_raw") > 0; + } const bool has_compressed_image_subscriber = hasCompressedImageSubscriber(stream_index); + const bool has_image_transport_compressed_subscriber = + has_compressed_image_subscriber && !has_explicit_compressed_publisher; bool has_subscriber = has_raw_image_subscriber || has_compressed_image_subscriber || save_images_[stream_index]; has_subscriber = @@ -7278,18 +7385,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, getSteadyNowUs()); } publishCompressedColorImage(frame, stream_index, timestamp, frame_id); - if (!has_raw_image_subscriber) { - if (stream_index == COLOR) { - fps_delay_status_color_->tick(frame_timestamp); - } else if (stream_index == COLOR_LEFT) { - fps_delay_status_left_color_->tick(frame_timestamp); - } else { - fps_delay_status_right_color_->tick(frame_timestamp); - } - } } - if (!has_raw_image_subscriber && !save_images_[stream_index]) { + if (!has_raw_image_subscriber && !save_images_[stream_index] && + !has_image_transport_compressed_subscriber) { CHECK(camera_info_publishers_.count(stream_index) > 0); camera_info_publishers_[stream_index]->publish(camera_info); publishMetadata(frame, stream_index, camera_info.header); @@ -7351,11 +7450,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, } } } - if (!has_raw_image_subscriber && !save_images_[stream_index]) { + if (!has_raw_image_subscriber && !save_images_[stream_index] && + !has_image_transport_compressed_subscriber) { return; } CHECK(image_publishers_.count(stream_index) > 0); - if (has_raw_image_subscriber || save_images_[stream_index]) { + if (has_raw_image_subscriber || has_image_transport_compressed_subscriber || + save_images_[stream_index]) { sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image()); cv_bridge::CvImage(std_msgs::msg::Header(), image_encoding, image_to_publish) .toImageMsg(*image_msg); @@ -7365,7 +7466,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, image_msg->step = image_step; image_msg->header.frame_id = frame_id; saveImageToFile(stream_index, raw_image, image_to_publish, *image_msg, frame); - if (!has_raw_image_subscriber) { + if (!has_raw_image_subscriber && !has_image_transport_compressed_subscriber) { record_image_publish_skipped(); return; } @@ -7373,18 +7474,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(), getSteadyNowUs()); } - if (stream_index == COLOR) { - fps_delay_status_color_->tick(frame_timestamp); - } else if (stream_index == COLOR_LEFT) { - fps_delay_status_left_color_->tick(frame_timestamp); - } else if (stream_index == COLOR_RIGHT) { - fps_delay_status_right_color_->tick(frame_timestamp); - } else if (stream_index == DEPTH) { - fps_delay_status_depth_->tick(frame_timestamp); - } else if (stream_index == INFRA1) { - fps_delay_status_left_ir_->tick(frame_timestamp); - } else if (stream_index == INFRA2) { - fps_delay_status_right_ir_->tick(frame_timestamp); + if (has_raw_image_subscriber) { + recordStreamStatus(stream_name_[stream_index] + "/image_raw", image_msg->header.stamp); + } + if (has_image_transport_compressed_subscriber) { + recordStreamStatus(compressedStreamStatusTopic(stream_index), image_msg->header.stamp); } image_publishers_[stream_index]->publish(std::move(image_msg)); } @@ -7392,8 +7486,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, bool OBCameraNode::hasCompressedImageSubscriber(const stream_index_pair &stream_index) const { auto it = compressed_image_publishers_.find(stream_index); - return it != compressed_image_publishers_.end() && it->second && - it->second->get_subscription_count() > 0; + if (it != compressed_image_publishers_.end() && it->second) { + return it->second->get_subscription_count() > 0; + } + if (use_intra_process_) { + return false; + } + return node_->count_subscribers(compressedStreamStatusTopic(stream_index)) > 0; } void OBCameraNode::publishCompressedColorImage(const std::shared_ptr &frame, @@ -7410,6 +7509,7 @@ void OBCameraNode::publishCompressedColorImage(const std::shared_ptr msg.format = "jpeg"; const auto *data = static_cast(frame->getData()); msg.data.assign(data, data + frame->getDataSize()); + recordStreamStatus(stream_name_[stream_index] + "/image_raw/compressed", msg.header.stamp); it->second->publish(std::move(msg)); } @@ -7622,6 +7722,8 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptrpublish(imu_msg); record_timestamps(publish_system_us); } @@ -7682,6 +7784,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr &frame return; } const auto publish_system_us = getSystemNowUs(); + recordStreamStatus(stream_name_[stream_index] + "/sample", imu_msg.header.stamp); imu_publishers_[stream_index]->publish(imu_msg); record_timestamps(publish_system_us); } @@ -8099,6 +8202,10 @@ bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) { pid == GEMINI_345LG_PID; } +bool OBCameraNode::isLingBotSupportedPID(uint32_t pid) { + return isGemini330SeriesPID(pid) || isDabaiASeriesForHwD2C(pid); +} + bool OBCameraNode::isDepthWorkModeDevices(uint32_t pid) { return pid == GEMINI_435Le_PID; } bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini301SeriesPID(pid); } @@ -8428,8 +8535,8 @@ bool OBCameraNode::applyEnhancedDepthFilterConfig( bool enabled, const std::vector &positional_params, const std::vector &named_params, std::string &message) { - if (!isGemini330SeriesPID(pid_)) { - message = "Enhanced depth filter is only supported by Gemini 330 series devices"; + if (!isLingBotSupportedPID(pid_)) { + message = "Enhanced depth filter is only supported by Gemini 330 and Dabai A series devices"; return false; } diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index db77106e..a44302dd 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -442,8 +442,7 @@ void OBCameraNodeDriver::init() { CHECK_NOTNULL(check_connect_timer_); if (device_type_ == "camera") { device_status_timer_ = - this->create_wall_timer(std::chrono::milliseconds(1000 / device_status_interval_hz), - [this]() { deviceStatusTimer(); }); + this->create_wall_timer(std::chrono::seconds(1), [this]() { deviceStatusTimer(); }); auto qos = rclcpp::QoS(1).transient_local(); if (node_options_.use_intra_process_comms()) { qos = rclcpp::QoS(1); @@ -480,6 +479,10 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr return; } + if (stream_configuration_error_.load()) { + return; + } + if (!device_) { startDevice(device_list); } @@ -552,6 +555,10 @@ void OBCameraNodeDriver::checkConnectTimer() { void OBCameraNodeDriver::queryDevice() { while (is_alive_ && rclcpp::ok()) { + if (stream_configuration_error_.load()) { + return; + } + // Check if device reset is in progress before attempting to connect { std::unique_lock reset_lock(reset_device_mutex_); @@ -714,6 +721,10 @@ void OBCameraNodeDriver::deviceStatusTimer() { status_msg.calibration_from_launch_param = false; status_msg.customer_calibration_ready = false; + if (ob_camera_node_) { + ob_camera_node_->fillStreamStatus(status_msg); + } + // Flag to track if device communication error occurs bool device_communication_error = false; @@ -725,30 +736,6 @@ void OBCameraNodeDriver::deviceStatusTimer() { if (reset_lock.owns_lock() && !reset_device_flag_) { // Only get device-specific info if we have a valid camera node and device if (ob_camera_node_) { - // Safely get color and depth status - these may access device - try { - ob_camera_node_->getColorStatus(status_msg); - ob_camera_node_->getDepthStatus(status_msg); - } catch (const ob::Error &e) { - std::string error_msg = orbbec_camera::formatObErrorWithStatus(e); - if (error_msg.find("Device is deactivated") != std::string::npos || - error_msg.find("disconnected") != std::string::npos || - error_msg.find("Send control transfer failed") != std::string::npos) { - RCLCPP_WARN( - logger_, - "Device communication error in %s at line %d: %s - Device may be disconnected", - __FUNCTION__, __LINE__, error_msg.c_str()); - device_communication_error = true; - } else { - RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__, - error_msg.c_str()); - } - } catch (const std::exception &e) { - RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what()); - } catch (...) { - RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__); - } - // These should be safe as they don't directly access hardware status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated(); } @@ -1204,6 +1191,12 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev } initialized = true; + } catch (const StreamConfigurationError &e) { + if (!stream_configuration_error_.exchange(true)) { + RCLCPP_ERROR_STREAM(logger_, "Invalid stream configuration; shutting down: " << e.what()); + rclcpp::shutdown(); + } + throw; } catch (const ob::Error &e) { RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt " << retry_count + 1 << " of " << max_retries @@ -1507,6 +1500,8 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int if (!device_connected_) { RCLCPP_ERROR_STREAM(logger_, "Failed to initialize net device " << net_device_ip); } + } catch (const StreamConfigurationError &) { + device_connected_ = false; } catch (const std::exception &e) { RCLCPP_ERROR_STREAM(logger_, "Exception during net device initialization: " << e.what()); device_connected_ = false; @@ -1517,7 +1512,7 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int } void OBCameraNodeDriver::startDevice(const std::shared_ptr &list) { - if (device_connected_.load()) { + if (device_connected_.load() || stream_configuration_error_.load()) { return; } @@ -1608,6 +1603,8 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr &list // // Fixing 301 series hot-swap not outputting power // ob_camera_node_->startStreams(); // } + } catch (const StreamConfigurationError &) { + device_connected_ = false; } catch (ob::Error &e) { RCLCPP_ERROR_STREAM( logger_, "Failed to initialize device " << orbbec_camera::formatObErrorWithStatus(e)); diff --git a/orbbec_camera/tools/firmware_update_tool.cpp b/orbbec_camera/tools/firmware_update_tool.cpp index be693071..20d4c847 100644 --- a/orbbec_camera/tools/firmware_update_tool.cpp +++ b/orbbec_camera/tools/firmware_update_tool.cpp @@ -420,8 +420,10 @@ DeviceIdentity getDeviceIdentity(const std::shared_ptr &device_i DeviceIdentity identity; identity.serial_number = safeString(device_info->getSerialNumber()); identity.uid = safeString(device_info->getUid()); - identity.ip_address = safeString(device_info->getIpAddress()); identity.is_network = safeString(device_info->getConnectionType()) == "Ethernet"; + if (identity.is_network) { + identity.ip_address = safeString(device_info->getIpAddress()); + } return identity; } diff --git a/orbbec_camera/tools/list_camera_profile.cpp b/orbbec_camera/tools/list_camera_profile.cpp index 07e352e5..b3807280 100644 --- a/orbbec_camera/tools/list_camera_profile.cpp +++ b/orbbec_camera/tools/list_camera_profile.cpp @@ -22,15 +22,18 @@ constexpr char kDocumentationUrl[] = struct CliArgs { bool help = false; std::string serial_number; + std::string device_preset; std::string sdk_log_level = "off"; }; void printUsage() { std::cout << "Usage:\n" << "ros2 run orbbec_camera list_camera_profile_mode_node --\\\n" - << " [--serial_number SN]\n\n" + << " [--serial_number SN] [--device_preset PRESET]\n\n" << "Parameters:\n" << " --serial_number SN Select a specific camera by serial number.\n" + << " --device_preset PRESET\n" + << " Load a device preset before listing profiles.\n" << " --sdk_log_level LEVEL\n" << " SDK file log level: debug/info/warn/error/fatal/off " "(default: off).\n" @@ -67,6 +70,28 @@ bool parseArgs(int argc, char** argv, CliArgs& args, std::string& error) { continue; } + if (current.rfind("--device_preset=", 0) == 0) { + args.device_preset = current.substr(std::strlen("--device_preset=")); + if (args.device_preset.empty()) { + error = "--device_preset requires a value"; + return false; + } + continue; + } + + if (current == "--device_preset") { + if (++i >= argc) { + error = "--device_preset requires a value"; + return false; + } + args.device_preset = argv[i]; + if (args.device_preset.empty()) { + error = "--device_preset requires a value"; + return false; + } + continue; + } + if (current.rfind("--sdk_log_level=", 0) == 0) { args.sdk_log_level = current.substr(std::strlen("--sdk_log_level=")); continue; @@ -220,6 +245,23 @@ void printPreset(const std::shared_ptr& device) { } } +bool loadDevicePreset(const std::shared_ptr& device, const std::string& device_preset) { + if (device_preset.empty()) { + return true; + } + + try { + device->loadPreset(device_preset.c_str()); + std::cout << "Loaded device preset: " << device_preset << std::endl; + return true; + } catch (const ob::Error& e) { + std::cerr << "Failed to load device preset: " << formatObErrorWithStatus(e) << std::endl; + } catch (const std::exception& e) { + std::cerr << "Failed to load device preset: " << e.what() << std::endl; + } + return false; +} + int main(int argc, char** argv) { CliArgs args; std::string parse_error; @@ -247,6 +289,9 @@ int main(int argc, char** argv) { if (isSdkLogEnabled(args.sdk_log_level)) { firmware_log_enabled = enableFirmwareLog(device); } + if (!loadDevicePreset(device, args.device_preset)) { + return -1; + } listSensorProfiles(device); printDeviceProperties(device); printPreset(device); diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index c004d1d6..14acfc3a 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/orbbec_camera_msgs/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces( ${PROJECT_NAME} "msg/DeviceInfo.msg" + "msg/StreamStatus.msg" "msg/DeviceStatus.msg" "msg/DepthFilterParam.msg" "msg/DepthFilterState.msg" diff --git a/orbbec_camera_msgs/msg/DeviceStatus.msg b/orbbec_camera_msgs/msg/DeviceStatus.msg index 32b46777..5e07cda1 100644 --- a/orbbec_camera_msgs/msg/DeviceStatus.msg +++ b/orbbec_camera_msgs/msg/DeviceStatus.msg @@ -1,76 +1,7 @@ std_msgs/Header header - -# --- Color stream --- -float64 color_frame_rate_cur -float64 color_frame_rate_avg -float64 color_frame_rate_min -float64 color_frame_rate_max - -float64 color_delay_ms_cur -float64 color_delay_ms_avg -float64 color_delay_ms_min -float64 color_delay_ms_max - -# --- Left color stream --- -float64 left_color_frame_rate_cur -float64 left_color_frame_rate_avg -float64 left_color_frame_rate_min -float64 left_color_frame_rate_max - -float64 left_color_delay_ms_cur -float64 left_color_delay_ms_avg -float64 left_color_delay_ms_min -float64 left_color_delay_ms_max - -# --- Right color stream --- -float64 right_color_frame_rate_cur -float64 right_color_frame_rate_avg -float64 right_color_frame_rate_min -float64 right_color_frame_rate_max - -float64 right_color_delay_ms_cur -float64 right_color_delay_ms_avg -float64 right_color_delay_ms_min -float64 right_color_delay_ms_max - -# --- Depth stream --- -float64 depth_frame_rate_cur -float64 depth_frame_rate_avg -float64 depth_frame_rate_min -float64 depth_frame_rate_max - -float64 depth_delay_ms_cur -float64 depth_delay_ms_avg -float64 depth_delay_ms_min -float64 depth_delay_ms_max - -# --- Left IR stream --- -float64 left_ir_frame_rate_cur -float64 left_ir_frame_rate_avg -float64 left_ir_frame_rate_min -float64 left_ir_frame_rate_max - -float64 left_ir_delay_ms_cur -float64 left_ir_delay_ms_avg -float64 left_ir_delay_ms_min -float64 left_ir_delay_ms_max - -# --- Right IR stream --- -float64 right_ir_frame_rate_cur -float64 right_ir_frame_rate_avg -float64 right_ir_frame_rate_min -float64 right_ir_frame_rate_max - -float64 right_ir_delay_ms_cur -float64 right_ir_delay_ms_avg -float64 right_ir_delay_ms_min -float64 right_ir_delay_ms_max - -# --- Device info --- bool device_online -string connection_type # e.g. "USB2.0", "USB3.0", "GigE" - -# --- Calibration status --- +string connection_type bool customer_calibration_ready bool calibration_from_factory bool calibration_from_launch_param +orbbec_camera_msgs/StreamStatus[] streams diff --git a/orbbec_camera_msgs/msg/StreamStatus.msg b/orbbec_camera_msgs/msg/StreamStatus.msg new file mode 100644 index 00000000..11dc7956 --- /dev/null +++ b/orbbec_camera_msgs/msg/StreamStatus.msg @@ -0,0 +1,4 @@ +string topic_name +bool has_subscribers +float64 publish_rate_hz +float64 delay_ms_avg