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

fix: restore changes omitted by v2 develop merge

Browse Source
This commit is contained in:
ob-yalian
2026-09-21 18:54:31 +08:00
parent a625397460
commit e10afc11a4
34 changed files with 422 additions and 388 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake
+2 -2
View File
@@ -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)
orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake
+3 -3
View File
@@ -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)
orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so
BIN
View File
Binary file not shown.
orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so
BIN
View File
Binary file not shown.
orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so
BIN
View File
Binary file not shown.
orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2
+1 -1
View File
@@ -1 +1 @@
libOrbbecSDK.so.2.10.2
libOrbbecSDK.so.2.10.3
orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.2 → orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.3
BIN
View File
Binary file not shown.
orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake
+2 -2
View File
@@ -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)
orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake
+3 -3
View File
@@ -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)
orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so
BIN
View File
Binary file not shown.
orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so
BIN
View File
Binary file not shown.
orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so
BIN
View File
Binary file not shown.
orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2
+1 -1
View File
@@ -1 +1 @@
libOrbbecSDK.so.2.10.2
libOrbbecSDK.so.2.10.3
orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.2 → orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.3
BIN
View File
Binary file not shown.
orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp
+6 -8
View File
@@ -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());
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
+20 -18
View File
@@ -26,6 +26,7 @@
#include <queue>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <stdexcept>
#include <unordered_map>
#include <unordered_set>
#include <utility>
@@ -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 <std_msgs/msg/header.hpp>
@@ -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<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 -1
View File
@@ -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<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
orbbec_camera/launch/dabai_a.launch.py
+5 -2
View File
@@ -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'),
orbbec_camera/launch/dabai_al.launch.py
+5 -2
View File
@@ -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=''),
orbbec_camera/launch/gemini345.launch.py
+5 -2
View File
@@ -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=''),
orbbec_camera/launch/gemini345_lg.launch.py
+5 -2
View File
@@ -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=''),
orbbec_camera/launch/gemini435_le.launch.py
-1
View File
@@ -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'),
orbbec_camera/launch/gemini_301_series.launch.py
+4 -7
View File
@@ -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=''),
orbbec_camera/launch/gemini_330_series.launch.py
-1
View File
@@ -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'),
orbbec_camera/launch/gemini_330_series_low_cpu.launch.py
-1
View File
@@ -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'),
orbbec_camera/src/ob_camera_node.cpp
+199 -92
View File
@@ -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<ob::Device> 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<FpsDelayStatus>(logger_);
fps_delay_status_left_color_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_right_color_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_left_ir_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_right_ir_ = std::make_unique<FpsDelayStatus>(logger_);
}
template <class T>
@@ -1011,21 +1004,24 @@ void OBCameraNode::setupDevices() {
std::string token;
std::vector<int> 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<std::shared_ptr<ob::Filter>> &filters) {
filters.erase(std::remove_if(filters.begin(), filters.end(),
[](const std::shared_ptr<ob::Filter> &filter) {
@@ -3612,7 +3618,6 @@ void OBCameraNode::setupProfiles() {
supported_profiles_[elem].emplace_back(profile);
}
std::shared_ptr<ob::VideoStreamProfile> selected_profile;
std::shared_ptr<ob::VideoStreamProfile> default_profile;
try {
if (is_playback_device_) {
selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
@@ -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<ob::FrameSet> &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<int>(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<std::string>(camera_info_qos_[stream_index], param_name, "default");
@@ -4879,11 +4879,7 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(mean_intensity_set_point_, "mean_intensity_set_point",
depth_brightness_);
setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "");
setAndGetNodeParameter<bool>(enable_ir_auto_exposure_,
isLaunchParamProvided("enable_auto_exposure")
? "enable_auto_exposure"
: "enable_ir_auto_exposure",
true);
setAndGetNodeParameter<bool>(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
setAndGetNodeParameter<int>(ir_exposure_, "ir_exposure", -1);
setAndGetNodeParameter<int>(ir_gain_, "ir_gain", -1);
setAndGetNodeParameter<int>(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<StreamStatusTracker>(resolved_topic_name, std::move(subscriber_count));
std::lock_guard<std::mutex> lock(stream_status_mutex_);
stream_status_trackers_[topic_name] = std::move(tracker);
}
void OBCameraNode::removeStreamStatus(const std::string &topic_name) {
std::lock_guard<std::mutex> 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<StreamStatusTracker> tracker;
{
std::lock_guard<std::mutex> 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<std::shared_ptr<StreamStatusTracker>> trackers;
{
std::lock_guard<std::mutex> 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<sensor_msgs::msg::CompressedImage>(
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<PointCloud2>(
"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<sensor_msgs::msg::Imu>(
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<orbbec_camera_msgs::msg::IMUInfo>(
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<sensor_msgs::msg::Imu>(
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<orbbec_camera_msgs::msg::IMUInfo>(
@@ -6196,6 +6293,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &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<ob::FrameSet>
}
}
recordStreamStatus("depth_registered/points", point_cloud_msg->header.stamp);
depth_registration_cloud_pub_->publish(std::move(point_cloud_msg));
}
std::shared_ptr<ob::Frame> OBCameraNode::processIrFrameFilter(std::shared_ptr<ob::Frame> &frame) {
@@ -7159,9 +7258,17 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &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<ob::Frame> &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<ob::Frame> &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<ob::Frame> &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<ob::Frame> &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<ob::Frame> &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<ob::Frame> &frame,
@@ -7410,6 +7509,7 @@ void OBCameraNode::publishCompressedColorImage(const std::shared_ptr<ob::Frame>
msg.format = "jpeg";
const auto *data = static_cast<const uint8_t *>(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_ptr<ob::Fra
imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];
const auto publish_system_us = getSystemNowUs();
recordStreamStatus(stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample",
imu_msg.header.stamp);
imu_gyro_accel_publisher_->publish(imu_msg);
record_timestamps(publish_system_us);
}
@@ -7682,6 +7784,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &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<float> &positional_params,
const std::vector<orbbec_camera_msgs::msg::DepthFilterParam> &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;
}
orbbec_camera/src/ob_camera_node_driver.cpp
+24 -27
View File
@@ -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<ob::DeviceList>
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<decltype(reset_device_mutex_)> 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<ob::Device> &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<ob::DeviceList> &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<ob::DeviceList> &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));
orbbec_camera/tools/firmware_update_tool.cpp
+3 -1
View File
@@ -420,8 +420,10 @@ DeviceIdentity getDeviceIdentity(const std::shared_ptr<ob::DeviceInfo> &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;
}
orbbec_camera/tools/list_camera_profile.cpp
+46 -1
View File
@@ -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<ob::Device>& device) {
}
}
bool loadDevicePreset(const std::shared_ptr<ob::Device>& 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);
orbbec_camera_msgs/CMakeLists.txt
+1
View File
@@ -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"
orbbec_camera_msgs/msg/DeviceStatus.msg
+2 -71
View File
@@ -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
orbbec_camera_msgs/msg/StreamStatus.msg
+4
View File
@@ -0,0 +1,4 @@
string topic_name
bool has_subscribers
float64 publish_rate_hz
float64 delay_ms_avg