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

Alignment Closed Source

Browse Source
This commit is contained in:
jj
2024-09-23 20:22:23 +08:00
parent 8b6bdfcd5d
commit bbed84961a
42 changed files with 909 additions and 385 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
+13 -1
View File
@@ -110,6 +110,7 @@ set(COMMON_INCLUDE_DIRS
$<INSTALL_INTERFACE:include>
${ORBBEC_INCLUDE_DIR}
${OpenCV_INCLUDED_DIRS}
${CMAKE_CURRENT_SOURCE_DIR}/tools
)
set(COMMON_LIBRARIES
@@ -139,6 +140,7 @@ endif ()
set(SOURCE_FILES
src/d2c_viewer.cpp
src/dynamic_params.cpp
src/image_publisher.cpp
src/ob_camera_node_driver.cpp
src/ob_camera_node.cpp
src/ros_param_backend.cpp
@@ -215,8 +217,18 @@ add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cp
add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp)
add_library(frame_latency SHARED tools/frame_latency.cpp)
target_include_directories(frame_latency PUBLIC ${COMMON_INCLUDE_DIRS})
target_link_libraries(frame_latency ${COMMON_LIBRARIES})
ament_target_dependencies(frame_latency ${dependencies})
rclcpp_components_register_node(frame_latency
PLUGIN "orbbec_camera::FrameLatencyNode"
EXECUTABLE frame_latency_node
)
# Install rules
install(TARGETS ${PROJECT_NAME}
install(TARGETS ${PROJECT_NAME} frame_latency
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
orbbec_camera/include/orbbec_camera/constants.h
+4 -3
View File
@@ -21,9 +21,9 @@
#define THREAD_NUM 4
#define OB_ROS_MAJOR_VERSION 2
#define OB_ROS_MINOR_VERSION 0
#define OB_ROS_PATCH_VERSION 1
#define OB_ROS_MAJOR_VERSION 1
#define OB_ROS_MINOR_VERSION 5
#define OB_ROS_PATCH_VERSION 11
#ifndef STRINGIFY
#define STRINGIFY(arg) #arg
@@ -128,5 +128,6 @@ const int32_t GEMINI_335LG_PID = 0x080B; // Gemini 336Lg
const int32_t GEMINI_336LG_PID = 0x080D;
const int32_t GEMINI_335LE_PID = 0x080E; // Gemini 335Le
const int32_t GEMINI_336LE_PID = 0x0810; // Gemini 335Le
const int32_t DABAI_MAX_PID = 0x069a; // dabai max
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/image_publisher.h
+52
View File
@@ -0,0 +1,52 @@
// Copyright 2023 Intel Corporation. All Rights Reserved.
//
// 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 <sensor_msgs/msg/image.hpp>
#include <image_transport/image_transport.hpp>
namespace orbbec_camera {
class image_publisher {
public:
virtual void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) = 0;
virtual size_t get_subscription_count() const = 0;
virtual ~image_publisher() = default;
}; // namespace image_publisher
// Native RCL implementation of an image publisher (needed for intra-process communication)
class image_rcl_publisher : public image_publisher {
public:
image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
const rmw_qos_profile_t& qos);
void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) override;
size_t get_subscription_count() const override;
private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_impl;
};
// image_transport implementation of an image publisher (adds a compressed image topic)
class image_transport_publisher : public image_publisher {
public:
image_transport_publisher(rclcpp::Node& node, const std::string& topic_name,
const rmw_qos_profile_t& qos);
void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) override;
size_t get_subscription_count() const override;
private:
std::shared_ptr<image_transport::Publisher> image_publisher_impl;
};
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+13 -5
View File
@@ -60,6 +60,7 @@
#include "orbbec_camera/dynamic_params.h"
#include "orbbec_camera/d2c_viewer.h"
#include "magic_enum/magic_enum.hpp"
#include "orbbec_camera/image_publisher.h"
#include "jpeg_decoder.h"
#include <std_msgs/msg/string.hpp>
@@ -114,7 +115,7 @@ const stream_index_pair INFRA2{OB_STREAM_IR_RIGHT, 0};
const stream_index_pair GYRO{OB_STREAM_GYRO, 0};
const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0};
const std::vector<stream_index_pair> IMAGE_STREAMS = {DEPTH, INFRA0, COLOR, INFRA1, INFRA2};
const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR, DEPTH, INFRA0, INFRA1, INFRA2};
const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL};
@@ -131,7 +132,7 @@ const std::map<OBStreamType, OBFrameType> STREAM_TYPE_TO_FRAME_TYPE = {
class OBCameraNode {
public:
OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
std::shared_ptr<Parameters> parameters);
std::shared_ptr<Parameters> parameters, bool use_intra_process = false);
template <class T>
void setAndGetNodeParameter(
@@ -315,7 +316,7 @@ class OBCameraNode {
void onNewColorFrameCallback();
void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
const sensor_msgs::msg::Image::SharedPtr& image_msg);
const sensor_msgs::msg::Image& image_msg);
void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
const std::shared_ptr<ob::Frame>& gryoframe);
@@ -391,7 +392,7 @@ class OBCameraNode {
std::map<stream_index_pair, bool> enable_stream_;
std::map<stream_index_pair, bool> flip_stream_;
std::map<stream_index_pair, std::string> stream_name_;
std::map<stream_index_pair, image_transport::Publisher> image_publishers_;
std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
camera_info_publishers_;
@@ -462,8 +463,12 @@ class OBCameraNode {
int color_exposure_ = -1;
int color_gain_ = -1;
int color_white_balance_ = -1;
int color_ae_max_exposure_ = -1;
int color_brightness_ = -1;
int ir_exposure_ = -1;
int ir_gain_ = -1;
int ir_ae_max_exposure_ = -1;
int ir_brightness_ = -1;
int soft_filter_max_diff_ = -1;
int soft_filter_speckle_size_ = -1;
bool enable_frame_sync_ = false;
@@ -564,7 +569,10 @@ class OBCameraNode {
std::chrono::milliseconds software_trigger_period_{33};
bool enable_heartbeat_ = false;
bool enable_color_undistortion_ = false;
image_transport::Publisher color_undistortion_publisher_;
std::shared_ptr<image_publisher> color_undistortion_publisher_;
bool has_first_color_frame_ = false;
bool use_intra_process_ = false;
std::string cloud_frame_id_;
std::vector<std::shared_ptr<ob::Filter>> filter_list_;
};
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+2
View File
@@ -70,6 +70,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
std::shared_ptr<std_srvs::srv::Empty::Response> response);
private:
const rclcpp::NodeOptions node_options_;
std::string config_path_;
std::unique_ptr<ob::Context> ctx_ = nullptr;
rclcpp::Logger logger_;
@@ -105,5 +106,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
std::chrono::time_point<std::chrono::system_clock> start_time_;
std::string extension_path_;
static backward::SignalHandling sh; // for stack trace
};
} // namespace orbbec_camera
orbbec_camera/launch/astra.launch.py
+2
View File
@@ -21,6 +21,7 @@ def generate_launch_description():
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='640'),
@@ -62,6 +63,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/astra2.launch.py
+2
View File
@@ -21,6 +21,7 @@ def generate_launch_description():
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='1280'),
@@ -70,6 +71,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/astra_adv.launch.py
+62 -60
View File
@@ -12,66 +12,68 @@ import os
def generate_launch_description():
# Declare arguments
args = [
DeclareLaunchArgument('camera_name', default_value='camera'),
DeclareLaunchArgument('depth_registration', default_value='false'),
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='640'),
DeclareLaunchArgument('color_height', default_value='480'),
DeclareLaunchArgument('color_fps', default_value='30'),
DeclareLaunchArgument('color_format', default_value='RGB'),
DeclareLaunchArgument('enable_color', default_value='true'),
DeclareLaunchArgument('flip_color', default_value='false'),
DeclareLaunchArgument('color_qos', default_value='default'),
DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
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('depth_width', default_value='640'),
DeclareLaunchArgument('depth_height', default_value='480'),
DeclareLaunchArgument('depth_fps', default_value='30'),
DeclareLaunchArgument('depth_format', default_value='Y16'),
DeclareLaunchArgument('enable_depth', default_value='true'),
DeclareLaunchArgument('flip_depth', default_value='false'),
DeclareLaunchArgument('depth_qos', default_value='default'),
DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
DeclareLaunchArgument('ir_width', default_value='640'),
DeclareLaunchArgument('ir_height', default_value='480'),
DeclareLaunchArgument('ir_fps', default_value='30'),
DeclareLaunchArgument('ir_format', default_value='Y16'),
DeclareLaunchArgument('enable_ir', default_value='true'),
DeclareLaunchArgument('flip_ir', default_value='false'),
DeclareLaunchArgument('ir_qos', default_value='default'),
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
DeclareLaunchArgument('ir_exposure', default_value='-1'),
DeclareLaunchArgument('ir_gain', default_value='-1'),
DeclareLaunchArgument('publish_tf', default_value='true'),
DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='false'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('align_mode', default_value='HW'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument("camera_name", default_value="camera"),
DeclareLaunchArgument("depth_registration", default_value="false"),
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("vendor_id", default_value="0x2bc5"),
DeclareLaunchArgument("product_id", default_value=""),
DeclareLaunchArgument("enable_point_cloud", default_value="true"),
DeclareLaunchArgument("enable_colored_point_cloud", default_value="false"),
DeclareLaunchArgument("cloud_frame_id", default_value=""),
DeclareLaunchArgument("point_cloud_qos", default_value="default"),
DeclareLaunchArgument("connection_delay", default_value="100"),
DeclareLaunchArgument("color_width", default_value="640"),
DeclareLaunchArgument("color_height", default_value="480"),
DeclareLaunchArgument("color_fps", default_value="30"),
DeclareLaunchArgument("color_format", default_value="RGB"),
DeclareLaunchArgument("enable_color", default_value="true"),
DeclareLaunchArgument("flip_color", default_value="false"),
DeclareLaunchArgument("color_qos", default_value="default"),
DeclareLaunchArgument("color_camera_info_qos", default_value="default"),
DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"),
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("depth_width", default_value="640"),
DeclareLaunchArgument("depth_height", default_value="480"),
DeclareLaunchArgument("depth_fps", default_value="30"),
DeclareLaunchArgument("depth_format", default_value="Y16"),
DeclareLaunchArgument("enable_depth", default_value="true"),
DeclareLaunchArgument("flip_depth", default_value="false"),
DeclareLaunchArgument("depth_qos", default_value="default"),
DeclareLaunchArgument("depth_camera_info_qos", default_value="default"),
DeclareLaunchArgument("ir_width", default_value="640"),
DeclareLaunchArgument("ir_height", default_value="480"),
DeclareLaunchArgument("ir_fps", default_value="30"),
DeclareLaunchArgument("ir_format", default_value="Y16"),
DeclareLaunchArgument("enable_ir", default_value="true"),
DeclareLaunchArgument("flip_ir", default_value="false"),
DeclareLaunchArgument("ir_qos", default_value="default"),
DeclareLaunchArgument("ir_camera_info_qos", default_value="default"),
DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"),
DeclareLaunchArgument("ir_exposure", default_value="-1"),
DeclareLaunchArgument("ir_gain", default_value="-1"),
DeclareLaunchArgument("publish_tf", default_value="true"),
DeclareLaunchArgument("tf_publish_rate", default_value="0.0"),
DeclareLaunchArgument("ir_info_url", default_value=""),
DeclareLaunchArgument("color_info_url", default_value=""),
DeclareLaunchArgument("log_level", default_value="none"),
DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument("enable_ldp", default_value="true"),
DeclareLaunchArgument("enable_soft_filter", default_value="true"),
DeclareLaunchArgument("soft_filter_max_diff", default_value="-1"),
DeclareLaunchArgument("soft_filter_speckle_size", default_value="-1"),
DeclareLaunchArgument("ordered_pc", default_value="false"),
DeclareLaunchArgument("use_hardware_time", default_value="false"),
DeclareLaunchArgument("enable_depth_scale", default_value="true"),
DeclareLaunchArgument("align_mode", default_value="HW"),
DeclareLaunchArgument("laser_energy_level", default_value="-1"),
DeclareLaunchArgument("enable_heartbeat", default_value="false"),
]
# Node configuration
orbbec_camera/launch/astra_embedded_s.launch.py
+2
View File
@@ -21,6 +21,7 @@ def generate_launch_description():
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='640'),
@@ -62,6 +63,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/astra_pro2.launch.py
+2
View File
@@ -21,6 +21,7 @@ def generate_launch_description():
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='false'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='640'),
@@ -62,6 +63,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/astra_stereo_u3.launch.py
+2
View File
@@ -21,6 +21,7 @@ def generate_launch_description():
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='640'),
@@ -62,6 +63,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/dabai.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -62,6 +63,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/dabai_d1.launch.py
+1
View File
@@ -19,6 +19,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('depth_width', default_value='640'),
orbbec_camera/launch/dabai_dcl.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -71,6 +72,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
# Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json
DeclareLaunchArgument('depth_filter_config', default_value=''),
orbbec_camera/launch/dabai_dcw.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -62,6 +63,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/dabai_dcw2.launch.py
+1
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
orbbec_camera/launch/dabai_dw.launch.py
+1
View File
@@ -19,6 +19,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('depth_width', default_value='640'),
orbbec_camera/launch/dabai_dw2.launch.py
+1
View File
@@ -19,6 +19,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('depth_width', default_value='640'),
orbbec_camera/launch/dabai_max.launch.py
+1
View File
@@ -19,6 +19,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('depth_width', default_value='640'),
orbbec_camera/launch/dabai_max_pro.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -65,6 +66,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/dabai_pro.launch.py
+1
View File
@@ -19,6 +19,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('depth_width', default_value='640'),
orbbec_camera/launch/deeya.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -61,6 +62,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/femto.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -71,6 +72,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/femto_bolt.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='false'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -71,6 +72,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/femto_mega.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -77,6 +78,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/gemini2.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='false'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -71,6 +72,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
# Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json
DeclareLaunchArgument('depth_filter_config', default_value=''),
orbbec_camera/launch/gemini2L.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -71,6 +72,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/gemini2VL.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='1280'),
DeclareLaunchArgument('color_height', default_value='800'),
@@ -69,6 +70,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/gemini2XL.launch.py
+2
View File
@@ -19,6 +19,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -79,6 +80,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/gemini_330_series.launch.py
+151 -144
View File
@@ -8,7 +8,7 @@ from launch_ros.descriptions import ComposableNode
def load_yaml(file_path):
with open(file_path, 'r') as f:
with open(file_path, "r") as f:
return yaml.safe_load(f)
@@ -29,20 +29,22 @@ def convert_value(value):
return float(value)
except ValueError:
pass
if value.lower() == 'true':
if value.lower() == "true":
return True
elif value.lower() == 'false':
elif value.lower() == "false":
return False
return value
def load_parameters(context, args):
default_params = {arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args}
config_file_path = LaunchConfiguration('config_file_path').perform(context)
default_params = {
arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args
}
config_file_path = LaunchConfiguration("config_file_path").perform(context)
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {"config_file_path", "usb_port", "serial_number"}
return {
key: (value if key in skip_convert else convert_value(value))
for key, value in default_params.items()
@@ -51,122 +53,126 @@ def load_parameters(context, args):
def generate_launch_description():
args = [
DeclareLaunchArgument('camera_name', default_value='camera'),
DeclareLaunchArgument('depth_registration', default_value='true'),
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('connection_delay', default_value='10'),
DeclareLaunchArgument('color_width', default_value='0'),
DeclareLaunchArgument('color_height', default_value='0'),
DeclareLaunchArgument('color_fps', default_value='0'),
DeclareLaunchArgument('color_format', default_value='ANY'),
DeclareLaunchArgument('enable_color', default_value='true'),
DeclareLaunchArgument('color_qos', default_value='default'),
DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
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('depth_width', default_value='0'),
DeclareLaunchArgument('depth_height', default_value='0'),
DeclareLaunchArgument('depth_fps', default_value='0'),
DeclareLaunchArgument('depth_format', default_value='ANY'),
DeclareLaunchArgument('enable_depth', default_value='true'),
DeclareLaunchArgument('depth_qos', default_value='default'),
DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
DeclareLaunchArgument('left_ir_width', default_value='0'),
DeclareLaunchArgument('left_ir_height', default_value='0'),
DeclareLaunchArgument('left_ir_fps', default_value='0'),
DeclareLaunchArgument('left_ir_format', default_value='ANY'),
DeclareLaunchArgument('enable_left_ir', default_value='false'),
DeclareLaunchArgument('left_ir_qos', default_value='default'),
DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('right_ir_width', default_value='0'),
DeclareLaunchArgument('right_ir_height', default_value='0'),
DeclareLaunchArgument('right_ir_fps', default_value='0'),
DeclareLaunchArgument('right_ir_format', default_value='ANY'),
DeclareLaunchArgument('enable_right_ir', default_value='false'),
DeclareLaunchArgument('right_ir_qos', default_value='default'),
DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
DeclareLaunchArgument('ir_exposure', default_value='-1'),
DeclareLaunchArgument('ir_gain', default_value='-1'),
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'),
DeclareLaunchArgument('enable_accel', default_value='false'),
DeclareLaunchArgument('accel_rate', default_value='200hz'),
DeclareLaunchArgument('accel_range', default_value='4g'),
DeclareLaunchArgument('enable_gyro', default_value='false'),
DeclareLaunchArgument('gyro_rate', default_value='200hz'),
DeclareLaunchArgument('gyro_range', default_value='1000dps'),
DeclareLaunchArgument('liner_accel_cov', default_value='0.01'),
DeclareLaunchArgument('angular_vel_cov', default_value='0.01'),
DeclareLaunchArgument('publish_tf', default_value='true'),
DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
DeclareLaunchArgument('sync_mode', default_value='standalone'),
DeclareLaunchArgument('depth_delay_us', default_value='0'),
DeclareLaunchArgument('color_delay_us', default_value='0'),
DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_enabled', default_value='true'),
DeclareLaunchArgument('frames_per_trigger', default_value='2'),
DeclareLaunchArgument('software_trigger_period', default_value='33'), # ms
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='true'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'),
DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'),
DeclareLaunchArgument('decimation_filter_scale_', default_value='-1'),
DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'),
DeclareLaunchArgument('threshold_filter_max', default_value='-1'),
DeclareLaunchArgument('threshold_filter_min', default_value='-1'),
DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'),
DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),
DeclareLaunchArgument('spatial_filter_alpha', default_value='-1.0'),
DeclareLaunchArgument('spatial_filter_diff_threshold', default_value='-1'),
DeclareLaunchArgument('spatial_filter_magnitude', default_value='-1'),
DeclareLaunchArgument('spatial_filter_radius', default_value='-1'),
DeclareLaunchArgument('temporal_filter_diff_threshold', default_value='-1.0'),
DeclareLaunchArgument('temporal_filter_weight', default_value='-1.0'),
DeclareLaunchArgument('hole_filling_filter_mode', default_value=''),
DeclareLaunchArgument('hdr_merge_exposure_1', default_value='-1'),
DeclareLaunchArgument('hdr_merge_gain_1', default_value='-1'),
DeclareLaunchArgument('hdr_merge_exposure_2', default_value='-1'),
DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'),
DeclareLaunchArgument('align_mode', default_value='SW'),
DeclareLaunchArgument('diagnostic_period', default_value='1.0'),
DeclareLaunchArgument('enable_laser', default_value='true'),
DeclareLaunchArgument('depth_precision', default_value=''),
DeclareLaunchArgument('device_preset', default_value='Default'),
DeclareLaunchArgument('laser_on_off_mode', default_value='0'),
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_3d_reconstruction_mode', default_value='false'),
DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
DeclareLaunchArgument('time_domain', default_value='device'),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('config_file_path', default_value=''),
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument("camera_name", default_value="camera"),
DeclareLaunchArgument("depth_registration", default_value="true"),
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("point_cloud_qos", default_value="default"),
DeclareLaunchArgument("enable_point_cloud", default_value="true"),
DeclareLaunchArgument("cloud_frame_id", default_value=""),
DeclareLaunchArgument("enable_colored_point_cloud", default_value="false"),
DeclareLaunchArgument("connection_delay", default_value="10"),
DeclareLaunchArgument("color_width", default_value="0"),
DeclareLaunchArgument("color_height", default_value="0"),
DeclareLaunchArgument("color_fps", default_value="0"),
DeclareLaunchArgument("color_format", default_value="ANY"),
DeclareLaunchArgument("enable_color", default_value="true"),
DeclareLaunchArgument("color_qos", default_value="default"),
DeclareLaunchArgument("color_camera_info_qos", default_value="default"),
DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"),
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("depth_width", default_value="0"),
DeclareLaunchArgument("depth_height", default_value="0"),
DeclareLaunchArgument("depth_fps", default_value="0"),
DeclareLaunchArgument("depth_format", default_value="ANY"),
DeclareLaunchArgument("enable_depth", default_value="true"),
DeclareLaunchArgument("depth_qos", default_value="default"),
DeclareLaunchArgument("depth_camera_info_qos", default_value="default"),
DeclareLaunchArgument("left_ir_width", default_value="0"),
DeclareLaunchArgument("left_ir_height", default_value="0"),
DeclareLaunchArgument("left_ir_fps", default_value="0"),
DeclareLaunchArgument("left_ir_format", default_value="ANY"),
DeclareLaunchArgument("enable_left_ir", default_value="false"),
DeclareLaunchArgument("left_ir_qos", default_value="default"),
DeclareLaunchArgument("left_ir_camera_info_qos", default_value="default"),
DeclareLaunchArgument("right_ir_width", default_value="0"),
DeclareLaunchArgument("right_ir_height", default_value="0"),
DeclareLaunchArgument("right_ir_fps", default_value="0"),
DeclareLaunchArgument("right_ir_format", default_value="ANY"),
DeclareLaunchArgument("enable_right_ir", default_value="false"),
DeclareLaunchArgument("right_ir_qos", default_value="default"),
DeclareLaunchArgument("right_ir_camera_info_qos", default_value="default"),
DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"),
DeclareLaunchArgument("ir_exposure", default_value="-1"),
DeclareLaunchArgument("ir_gain", default_value="-1"),
DeclareLaunchArgument("enable_sync_output_accel_gyro", default_value="false"),
DeclareLaunchArgument("enable_accel", default_value="false"),
DeclareLaunchArgument("accel_rate", default_value="200hz"),
DeclareLaunchArgument("accel_range", default_value="4g"),
DeclareLaunchArgument("enable_gyro", default_value="false"),
DeclareLaunchArgument("gyro_rate", default_value="200hz"),
DeclareLaunchArgument("gyro_range", default_value="1000dps"),
DeclareLaunchArgument("liner_accel_cov", default_value="0.01"),
DeclareLaunchArgument("angular_vel_cov", default_value="0.01"),
DeclareLaunchArgument("publish_tf", default_value="true"),
DeclareLaunchArgument("tf_publish_rate", default_value="0.0"),
DeclareLaunchArgument("ir_info_url", default_value=""),
DeclareLaunchArgument("color_info_url", default_value=""),
DeclareLaunchArgument("log_level", default_value="none"),
DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument("enable_ldp", default_value="true"),
DeclareLaunchArgument("enable_soft_filter", default_value="true"),
DeclareLaunchArgument("soft_filter_max_diff", default_value="-1"),
DeclareLaunchArgument("soft_filter_speckle_size", default_value="-1"),
DeclareLaunchArgument("sync_mode", default_value="standalone"),
DeclareLaunchArgument("depth_delay_us", default_value="0"),
DeclareLaunchArgument("color_delay_us", default_value="0"),
DeclareLaunchArgument("trigger2image_delay_us", default_value="0"),
DeclareLaunchArgument("trigger_out_delay_us", default_value="0"),
DeclareLaunchArgument("trigger_out_enabled", default_value="true"),
DeclareLaunchArgument("frames_per_trigger", default_value="2"),
DeclareLaunchArgument("software_trigger_period", default_value="33"), # ms
DeclareLaunchArgument("enable_frame_sync", default_value="true"),
DeclareLaunchArgument("ordered_pc", default_value="false"),
DeclareLaunchArgument("use_hardware_time", default_value="true"),
DeclareLaunchArgument("enable_depth_scale", default_value="true"),
DeclareLaunchArgument("enable_decimation_filter", default_value="false"),
DeclareLaunchArgument("enable_hdr_merge", default_value="false"),
DeclareLaunchArgument("enable_sequence_id_filter", default_value="false"),
DeclareLaunchArgument("enable_threshold_filter", default_value="false"),
DeclareLaunchArgument("enable_noise_removal_filter", default_value="true"),
DeclareLaunchArgument("enable_spatial_filter", default_value="false"),
DeclareLaunchArgument("enable_temporal_filter", default_value="false"),
DeclareLaunchArgument("enable_hole_filling_filter", default_value="false"),
DeclareLaunchArgument("decimation_filter_scale_", default_value="-1"),
DeclareLaunchArgument("sequence_id_filter_id", default_value="-1"),
DeclareLaunchArgument("threshold_filter_max", default_value="-1"),
DeclareLaunchArgument("threshold_filter_min", default_value="-1"),
DeclareLaunchArgument("noise_removal_filter_min_diff", default_value="256"),
DeclareLaunchArgument("noise_removal_filter_max_size", default_value="80"),
DeclareLaunchArgument("spatial_filter_alpha", default_value="-1.0"),
DeclareLaunchArgument("spatial_filter_diff_threshold", default_value="-1"),
DeclareLaunchArgument("spatial_filter_magnitude", default_value="-1"),
DeclareLaunchArgument("spatial_filter_radius", default_value="-1"),
DeclareLaunchArgument("temporal_filter_diff_threshold", default_value="-1.0"),
DeclareLaunchArgument("temporal_filter_weight", default_value="-1.0"),
DeclareLaunchArgument("hole_filling_filter_mode", default_value=""),
DeclareLaunchArgument("hdr_merge_exposure_1", default_value="-1"),
DeclareLaunchArgument("hdr_merge_gain_1", default_value="-1"),
DeclareLaunchArgument("hdr_merge_exposure_2", default_value="-1"),
DeclareLaunchArgument("hdr_merge_gain_2", default_value="-1"),
DeclareLaunchArgument("align_mode", default_value="SW"),
DeclareLaunchArgument("diagnostic_period", default_value="1.0"),
DeclareLaunchArgument("enable_laser", default_value="true"),
DeclareLaunchArgument("depth_precision", default_value=""),
DeclareLaunchArgument("device_preset", default_value="Default"),
DeclareLaunchArgument("laser_on_off_mode", default_value="0"),
DeclareLaunchArgument("retry_on_usb3_detection_failure", default_value="false"),
DeclareLaunchArgument("laser_energy_level", default_value="-1"),
DeclareLaunchArgument("enable_3d_reconstruction_mode", default_value="false"),
DeclareLaunchArgument("enable_sync_host_time", default_value="true"),
DeclareLaunchArgument("time_domain", default_value="device"),
DeclareLaunchArgument("enable_color_undistortion", default_value="false"),
DeclareLaunchArgument("config_file_path", default_value=""),
DeclareLaunchArgument("enable_heartbeat", default_value="false"),
]
def get_params(context, args):
@@ -188,28 +194,29 @@ def generate_launch_description():
]
else:
return [
GroupAction([
PushRosNamespace(LaunchConfiguration("camera_name")),
ComposableNodeContainer(
name="camera_container",
namespace="",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
ComposableNode(
package="orbbec_camera",
plugin="orbbec_camera::OBCameraNodeDriver",
name=LaunchConfiguration("camera_name"),
parameters=params,
),
],
output="screen",
)
])
GroupAction(
[
PushRosNamespace(LaunchConfiguration("camera_name")),
ComposableNodeContainer(
name="camera_container",
namespace="",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
ComposableNode(
package="orbbec_camera",
plugin="orbbec_camera::OBCameraNodeDriver",
name=LaunchConfiguration("camera_name"),
parameters=params,
),
],
output="screen",
),
]
)
]
return LaunchDescription(
args + [
OpaqueFunction(function=lambda context: create_node_action(context, args))
]
)
args
+ [OpaqueFunction(function=lambda context: create_node_action(context, args))]
)
orbbec_camera/launch/gemini_e.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -62,6 +63,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/gemini_e_lite.launch.py
+2
View File
@@ -19,6 +19,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('depth_width', default_value='640'),
@@ -47,6 +48,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/gemini_ew.launch.py
+1
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
orbbec_camera/launch/gemini_ew_lite.launch.py
+1
View File
@@ -19,6 +19,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('depth_width', default_value='640'),
orbbec_camera/launch/gemini_uw.launch.py
+2
View File
@@ -20,6 +20,7 @@ def generate_launch_description():
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
@@ -65,6 +66,7 @@ def generate_launch_description():
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument("enable_hardware_d2d", default_value="true"),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
orbbec_camera/launch/multi_camera_synced.launch.py
+4 -33
View File
@@ -61,43 +61,14 @@ def generate_launch_description():
"config_file_path": config_file_path,
}.items(),
)
test_node = (
Node(
package="orbbec_camera",
executable="multi_save_rgbir_node",
name="multi_save_rgbir_node",
parameters=[
{
"ir_topics": [
"/front_camera/left_ir/image_raw",
"/right_camera/left_ir/image_raw",
"/rear_camera/left_ir/image_raw",
],
"color_topics": [
"/front_camera/color/image_raw",
"/right_camera/color/image_raw",
"/rear_camera/color/image_raw",
],
}
],
)
)
# If you need more cameras, just add more launch_include here, and change the usb_port and device_num
# Launch description
ld = LaunchDescription(
[
GroupAction([test_node]),
TimerAction(
period=3.0,
actions=[
GroupAction([rear_camera]),
# GroupAction([left_camera]),
GroupAction([right_camera]),
TimerAction(period=3.0, actions=[GroupAction([front_camera])]),
],
),
GroupAction([rear_camera]),
GroupAction([left_camera]),
GroupAction([right_camera]),
TimerAction(period=3.0, actions=[GroupAction([front_camera])]),
# The primary camera should be launched at last
]
)
orbbec_camera/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>orbbec_camera</name>
<version>2.0.1</version>
<version>2.0.2</version>
<description>Orbbec Camera package</description>
<maintainer email="mocun@orbbec.com">Joe Dong</maintainer>
<license>Apache-2.0</license>
orbbec_camera/src/image_publisher.cpp
+48
View File
@@ -0,0 +1,48 @@
// Copyright 2023 Intel Corporation. All Rights Reserved.
//
// 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.
#include "orbbec_camera/image_publisher.h"
namespace orbbec_camera {
// --- image_rcl_publisher implementation ---
image_rcl_publisher::image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
const rmw_qos_profile_t& qos) {
image_publisher_impl = node.create_publisher<sensor_msgs::msg::Image>(
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
}
void image_rcl_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
image_publisher_impl->publish(std::move(image_ptr));
}
size_t image_rcl_publisher::get_subscription_count() const {
return image_publisher_impl->get_subscription_count();
}
// --- image_transport_publisher implementation ---
image_transport_publisher::image_transport_publisher(rclcpp::Node& node,
const std::string& topic_name,
const rmw_qos_profile_t& qos) {
image_publisher_impl = std::make_shared<image_transport::Publisher>(
image_transport::create_publisher(&node, topic_name, qos));
}
void image_transport_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
image_publisher_impl->publish(*image_ptr);
}
size_t image_transport_publisher::get_subscription_count() const {
return image_publisher_impl->getNumSubscribers();
}
} // namespace orbbec_camera
orbbec_camera/src/ob_camera_node.cpp
+259 -107
View File
@@ -35,11 +35,14 @@ namespace orbbec_camera {
using namespace std::chrono_literals;
OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
std::shared_ptr<Parameters> parameters)
std::shared_ptr<Parameters> parameters, bool use_intra_process)
: node_(node),
device_(std::move(device)),
parameters_(std::move(parameters)),
logger_(node->get_logger()) {
logger_(node->get_logger()),
use_intra_process_(use_intra_process) {
RCLCPP_INFO_STREAM(logger_,
"OBCameraNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF"));
is_running_.store(true);
stream_name_[COLOR] = "color";
stream_name_[DEPTH] = "depth";
@@ -94,9 +97,11 @@ void OBCameraNode::setAndGetNodeParameter(
OBCameraNode::~OBCameraNode() noexcept { clean(); }
void OBCameraNode::rebootDevice() {
RCLCPP_WARN_STREAM(logger_, "Reboot device");
clean();
if (device_) {
device_->reboot();
RCLCPP_WARN_STREAM(logger_, "Reboot device DONE");
}
}
@@ -104,10 +109,11 @@ void OBCameraNode::clean() noexcept {
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
is_running_.store(false);
RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
if (tf_thread_ && tf_thread_->joinable()) {
tf_thread_->join();
}
RCLCPP_WARN_STREAM(logger_, "Stop color frame thread");
if (colorFrameThread_ && colorFrameThread_->joinable()) {
color_frame_queue_cv_.notify_all();
colorFrameThread_->join();
@@ -116,7 +122,6 @@ void OBCameraNode::clean() noexcept {
RCLCPP_WARN_STREAM(logger_, "stop streams");
stopStreams();
stopIMU();
RCLCPP_WARN_STREAM(logger_, "Destroy ~OBCameraNode DONE");
if (rgb_buffer_) {
delete[] rgb_buffer_;
rgb_buffer_ = nullptr;
@@ -129,9 +134,9 @@ void OBCameraNode::clean() noexcept {
delete[] xy_table_data_;
xy_table_data_ = nullptr;
}
RCLCPP_WARN_STREAM(logger_, "Destroy ~OBCameraNode DONE");
}
void OBCameraNode::setupDevices() {
auto sensor_list = device_->getSensorList();
for (size_t i = 0; i < sensor_list->getCount(); i++) {
@@ -162,20 +167,23 @@ void OBCameraNode::setupDevices() {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL,
retry_on_usb3_detection_failure_);
}
// if (device_->isPropertySupported(OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL,
// OB_PERMISSION_READ_WRITE)) {
// TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL,
// enable_noise_removal_filter_);
// }
// if (device_->isPropertySupported(OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL,
// OB_PERMISSION_READ_WRITE)) {
// TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL,
// enable_noise_removal_filter_);
// }
if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting heartbeat to " << (enable_heartbeat_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
}
if (max_depth_limit_ > 0 &&
device_->isPropertySupported(OB_PROP_MAX_DEPTH_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting max depth limit to " << max_depth_limit_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MAX_DEPTH_INT, max_depth_limit_);
}
if (min_depth_limit_ > 0 &&
device_->isPropertySupported(OB_PROP_MIN_DEPTH_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting min depth limit to " << min_depth_limit_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MIN_DEPTH_INT, min_depth_limit_);
}
if (laser_energy_level_ != -1 &&
@@ -193,6 +201,7 @@ void OBCameraNode::setupDevices() {
}
}
if (depth_registration_) {
RCLCPP_INFO_STREAM(logger_, "Create align filter");
align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
}
if (device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
@@ -202,12 +211,15 @@ void OBCameraNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Depth process is " << d2d_mode);
}
if (device_->isPropertySupported(OB_PROP_LDP_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting LDP to " << (enable_ldp_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_);
}
if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting laser control to " << enable_laser_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, enable_laser_);
}
if (device_->isPropertySupported(OB_PROP_LASER_ON_OFF_MODE_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting laser on off mode to " << laser_on_off_mode_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_ON_OFF_MODE_INT, laser_on_off_mode_);
}
if (!device_preset_.empty()) {
@@ -218,7 +230,7 @@ void OBCameraNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Preset " << i << ": " << preset_list->getName(i));
}
RCLCPP_INFO_STREAM(logger_, "Load device preset: " << device_preset_);
device_->loadPreset(device_preset_.c_str());
TRY_EXECUTE_BLOCK(device_->loadPreset(device_preset_.c_str()));
RCLCPP_INFO_STREAM(logger_, "Device preset " << device_->getCurrentPresetName() << " loaded");
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.getMessage());
@@ -230,7 +242,7 @@ void OBCameraNode::setupDevices() {
}
if (!depth_work_mode_.empty()) {
RCLCPP_INFO_STREAM(logger_, "Set depth work mode: " << depth_work_mode_);
device_->switchDepthWorkMode(depth_work_mode_.c_str());
TRY_EXECUTE_BLOCK(device_->switchDepthWorkMode(depth_work_mode_.c_str()));
}
if (!sync_mode_str_.empty() && device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
OB_PERMISSION_READ_WRITE)) {
@@ -246,15 +258,15 @@ void OBCameraNode::setupDevices() {
sync_config.triggerOutDelayUs = trigger_out_delay_us_;
sync_config.triggerOutEnable = trigger_out_enabled_;
sync_config.framesPerTrigger = frames_per_trigger_;
device_->setMultiDeviceSyncConfig(sync_config);
TRY_EXECUTE_BLOCK(device_->setMultiDeviceSyncConfig(sync_config));
sync_config = device_->getMultiDeviceSyncConfig();
RCLCPP_INFO_STREAM(logger_, "Set sync mode: " << magic_enum::enum_name(sync_config.syncMode));
if (sync_mode_ == OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING) {
RCLCPP_INFO_STREAM(logger_, "Frames per trigger: " << sync_config.framesPerTrigger);
RCLCPP_INFO_STREAM(logger_,
"Software trigger period " << software_trigger_period_.count() << " ms");
software_trigger_timer_ = node_->create_wall_timer(software_trigger_period_,
[this]() { device_->triggerCapture(); });
software_trigger_timer_ = node_->create_wall_timer(
software_trigger_period_, [this]() { TRY_EXECUTE_BLOCK(device_->triggerCapture()); });
}
}
@@ -277,8 +289,8 @@ void OBCameraNode::setupDevices() {
logger_, "depth unit flexible adjustment value is out of range, please check the value");
} else {
RCLCPP_INFO_STREAM(logger_, "set depth unit to " << depth_unit_flexible_adjustment << "mm");
device_->setFloatProperty(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
depth_unit_flexible_adjustment);
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
depth_unit_flexible_adjustment);
}
}
@@ -299,73 +311,107 @@ void OBCameraNode::setupDevices() {
}
if (device_->isPropertySupported(mirrorPropertyID, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(mirrorPropertyID, flip_stream_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Setting " << stream_name_[stream_index] << " mirror to "
<< (flip_stream_[stream_index] ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, mirrorPropertyID, flip_stream_[stream_index]);
}
}
}
if (!depth_filter_config_.empty() && enable_depth_filter_) {
RCLCPP_INFO_STREAM(logger_, "Load depth filter config: " << depth_filter_config_);
device_->loadDepthFilterConfig(depth_filter_config_.c_str());
TRY_EXECUTE_BLOCK(device_->loadDepthFilterConfig(depth_filter_config_.c_str()));
} else {
if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
RCLCPP_INFO_STREAM(logger_,
"Setting depth soft filter to " << (enable_soft_filter_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
}
}
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
RCLCPP_INFO_STREAM(
logger_, "Setting color auto exposure to " << (enable_color_auto_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL,
enable_color_auto_exposure_);
}
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL,
enable_color_auto_white_balance_);
RCLCPP_INFO_STREAM(logger_, "Setting color auto white balance to "
<< (enable_color_auto_white_balance_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL,
enable_color_auto_white_balance_);
}
if (color_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_EXPOSURE_INT);
if (color_exposure_ < range.min || color_exposure_ > range.max) {
RCLCPP_ERROR(logger_, "color exposure value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
device_->setIntProperty(OB_PROP_COLOR_EXPOSURE_INT, color_exposure_);
RCLCPP_INFO_STREAM(logger_, "Setting color exposure to " << color_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_EXPOSURE_INT, color_exposure_);
}
}
if (color_gain_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_GAIN_INT, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_GAIN_INT);
if (color_gain_ < range.min || color_gain_ > range.max) {
RCLCPP_ERROR(logger_, "color gain value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
device_->setIntProperty(OB_PROP_COLOR_GAIN_INT, color_gain_);
RCLCPP_INFO_STREAM(logger_, "Setting color gain to " << color_gain_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAIN_INT, color_gain_);
}
}
if (color_white_balance_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_WHITE_BALANCE_INT, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, false);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT);
if (color_white_balance_ < range.min || color_white_balance_ > range.max) {
RCLCPP_ERROR(logger_,
"color white balance value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, color_white_balance_);
RCLCPP_INFO_STREAM(logger_, "Setting color white balance to " << color_white_balance_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_WHITE_BALANCE_INT, color_white_balance_);
}
}
if (device_->isPropertySupported(OB_PROP_COLOR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting color AE max exposure to " << color_ae_max_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_EXPOSURE_INT, color_ae_max_exposure_);
}
if (device_->isPropertySupported(OB_PROP_COLOR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting color brightness to " << color_brightness_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BRIGHTNESS_INT, color_brightness_);
}
// ir ae max
if (device_->isPropertySupported(OB_PROP_IR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting IR AE max exposure to " << ir_ae_max_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure_);
}
// ir brightness
if (device_->isPropertySupported(OB_PROP_IR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting IR brightness to " << ir_brightness_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, ir_brightness_);
}
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
RCLCPP_INFO_STREAM(logger_,
"Setting IR auto exposure to " << (enable_ir_auto_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
}
if (ir_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_IR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_IR_EXPOSURE_INT);
if (ir_exposure_ < range.min || ir_exposure_ > range.max) {
RCLCPP_ERROR(logger_, "ir exposure value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
RCLCPP_INFO_STREAM(logger_, "Setting IR exposure to " << ir_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_EXPOSURE_INT, ir_exposure_);
}
}
@@ -376,11 +422,14 @@ void OBCameraNode::setupDevices() {
RCLCPP_ERROR(logger_, "ir gain value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
RCLCPP_INFO_STREAM(logger_, "Setting IR gain to " << ir_gain_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_GAIN_INT, ir_gain_);
}
}
if (device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_,
"Setting IR long exposure to " << (enable_ir_long_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_);
}
@@ -401,7 +450,8 @@ void OBCameraNode::setupDevices() {
"default_soft_filter_speckle_size: " << default_soft_filter_speckle_size);
if (soft_filter_speckle_size_ != -1 &&
default_soft_filter_speckle_size != soft_filter_speckle_size_) {
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT,
soft_filter_speckle_size_);
auto new_soft_filter_speckle_size =
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
RCLCPP_INFO_STREAM(logger_,
@@ -551,19 +601,19 @@ void OBCameraNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor
auto origin_profile = profiles->getProfile(i);
if (sensor->getType() == OB_SENSOR_COLOR) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight()
<< " " << profile->getFps() << "fps "
<< profile->getFormat());
RCLCPP_INFO_STREAM(
logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
<< profile->getFps() << "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_DEPTH) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight()
<< " " << profile->getFps() << "fps "
<< profile->getFormat());
RCLCPP_INFO_STREAM(
logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
<< profile->getFps() << "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_IR) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x" << profile->getHeight()
<< " " << profile->getFps() << "fps "
<< profile->getFormat());
RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x"
<< profile->getHeight() << " " << profile->getFps()
<< "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_ACCEL) {
auto profile = origin_profile->as<ob::AccelStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->getSampleRate()
@@ -590,12 +640,19 @@ void OBCameraNode::setupProfiles() {
CHECK_NOTNULL(profiles.get());
CHECK(profiles->getCount() > 0);
for (size_t i = 0; i < profiles->getCount(); i++) {
auto profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
auto base_profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
if (base_profile == nullptr) {
throw std::runtime_error("Failed to get profile " + std::to_string(i));
}
auto profile = base_profile->as<ob::VideoStreamProfile>();
if (profile == nullptr) {
throw std::runtime_error("Failed cast profile to VideoStreamProfile");
}
RCLCPP_DEBUG_STREAM(
logger_,
"Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->getType())
<< "Format: " << profile->getFormat() << ", Width: " << profile->getWidth()
<< ", Height: " << profile->getHeight() << ", FPS: " << profile->getFps());
logger_, "Sensor profile: "
<< "stream_type: " << magic_enum::enum_name(profile->getType())
<< "Format: " << profile->getFormat() << ", Width: " << profile->getWidth()
<< ", Height: " << profile->getHeight() << ", FPS: " << profile->getFps());
supported_profiles_[elem].emplace_back(profile);
}
std::shared_ptr<ob::VideoStreamProfile> selected_profile;
@@ -622,6 +679,7 @@ void OBCameraNode::setupProfiles() {
"configuration and try again. The current process will now exit.");
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor);
RCLCPP_ERROR(logger_, "Because can not set this stream, so exit.");
exit(-1);
}
@@ -653,12 +711,13 @@ void OBCameraNode::setupProfiles() {
updateImageConfig(elem);
images_[elem] =
cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
RCLCPP_INFO_STREAM(
logger_, " stream " << stream_name_[elem]
<< " is enabled - width: " << selected_profile->getWidth()
<< ", height: " << selected_profile->getHeight()
<< ", fps: " << selected_profile->getFps() << ", "
<< "Format: " << magic_enum::enum_name(selected_profile->getFormat()));
RCLCPP_INFO_STREAM(logger_,
" stream "
<< stream_name_[elem]
<< " is enabled - width: " << selected_profile->getWidth()
<< ", height: " << selected_profile->getHeight()
<< ", fps: " << selected_profile->getFps() << ", "
<< "Format: " << magic_enum::enum_name(selected_profile->getFormat()));
}
}
// IMU
@@ -740,10 +799,10 @@ void OBCameraNode::startStreams() {
}
if (enable_frame_sync_) {
RCLCPP_INFO_STREAM(logger_, "Enable frame sync");
pipeline_->enableFrameSync();
TRY_EXECUTE_BLOCK(pipeline_->enableFrameSync());
} else {
RCLCPP_INFO_STREAM(logger_, "Disable frame sync");
pipeline_->disableFrameSync();
TRY_EXECUTE_BLOCK(pipeline_->disableFrameSync());
}
pipeline_started_.store(true);
}
@@ -771,7 +830,7 @@ void OBCameraNode::startIMUSyncStream() {
std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
imuConfig->enableStream(accelProfile);
imuConfig->enableStream(gyroProfile);
imuPipeline_->enableFrameSync();
TRY_EXECUTE_BLOCK(imuPipeline_->enableFrameSync());
imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame) {
auto frameSet = frame->as<ob::FrameSet>();
auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
@@ -802,8 +861,9 @@ void OBCameraNode::startIMU() {
for (const auto &stream_index : HID_STREAMS) {
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
auto imu_profile = stream_profile_[stream_index];
CHECK(imu_profile);
CHECK_NOTNULL(imu_profile);
RCLCPP_INFO_STREAM(logger_, "start " << stream_name_[stream_index] << " stream");
CHECK_NOTNULL(sensors_[stream_index]);
sensors_[stream_index]->start(
imu_profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
onNewIMUFrameCallback(frame, stream_index);
@@ -815,31 +875,42 @@ void OBCameraNode::startIMU() {
void OBCameraNode::stopStreams() {
if (!pipeline_started_ || !pipeline_) {
RCLCPP_INFO_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline");
return;
}
try {
pipeline_->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
}
}
void OBCameraNode::stopIMU() {
if (enable_sync_output_accel_gyro_) {
if (!imu_sync_output_start_ || !imuPipeline_) {
RCLCPP_INFO_STREAM(logger_, "imu pipeline not started or not exist, skip stop imu pipeline");
return;
}
try {
imuPipeline_->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline: " << e.getMessage());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline");
}
} else {
for (const auto &stream_index : HID_STREAMS) {
if (imu_started_[stream_index]) {
CHECK(sensors_.count(stream_index));
RCLCPP_INFO_STREAM(logger_, "stop " << stream_name_[stream_index] << " stream");
sensors_[stream_index]->stop();
try {
sensors_[stream_index]->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop " << stream_name_[stream_index]
<< " stream: " << e.getMessage());
}
imu_started_[stream_index] = false;
}
}
@@ -886,7 +957,11 @@ void OBCameraNode::getParameters() {
param_name = stream_name_[stream_index] + "_fps";
setAndGetNodeParameter(fps_[stream_index], param_name, 0);
param_name = "enable_" + stream_name_[stream_index];
setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
if (stream_index == DEPTH) {
setAndGetNodeParameter(enable_stream_[stream_index], param_name, true);
} else {
setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
}
param_name = "flip_" + stream_name_[stream_index];
setAndGetNodeParameter(flip_stream_[stream_index], param_name, false);
param_name = camera_name_ + "_" + stream_name_[stream_index] + "_frame_id";
@@ -956,9 +1031,13 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(color_exposure_, "color_exposure", -1);
setAndGetNodeParameter<int>(color_gain_, "color_gain", -1);
setAndGetNodeParameter<int>(color_white_balance_, "color_white_balance", -1);
setAndGetNodeParameter<int>(color_ae_max_exposure_, "color_ae_max_exposure", -1);
setAndGetNodeParameter<int>(color_brightness_, "color_brightness", -1);
setAndGetNodeParameter(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);
setAndGetNodeParameter<int>(ir_brightness_, "ir_brightness", -1);
setAndGetNodeParameter(enable_ir_long_exposure_, "enable_ir_long_exposure", true);
setAndGetNodeParameter<std::string>(depth_work_mode_, "depth_work_mode", "");
setAndGetNodeParameter<std::string>(sync_mode_str_, "sync_mode", "");
@@ -968,10 +1047,11 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter(trigger_out_delay_us_, "trigger_out_delay_us", 0);
setAndGetNodeParameter(trigger_out_enabled_, "trigger_out_enabled", false);
setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "");
setAndGetNodeParameter<std::string>(cloud_frame_id_, "cloud_frame_id", "");
if (!depth_precision_str_.empty()) {
depth_precision_ = depthPrecisionLevelFromString(depth_precision_str_);
}
if (enable_colored_point_cloud_) {
if (enable_colored_point_cloud_ || enable_d2c_viewer_) {
depth_registration_ = true;
}
if (!enable_stream_[COLOR]) {
@@ -1058,10 +1138,13 @@ void OBCameraNode::setupTopics() {
setupDiagnosticUpdater();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.getMessage());
throw std::runtime_error(e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
throw std::runtime_error(e.what());
} catch (...) {
RCLCPP_ERROR(logger_, "Failed to setup topics");
throw std::runtime_error("Failed to setup topics");
}
}
@@ -1125,20 +1208,22 @@ void OBCameraNode::setupPipelineConfig() {
if (enable_stream_[stream_index]) {
RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
auto profile = stream_profile_[stream_index]->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_,
"Stream " << stream_name_[stream_index] << " width: " << profile->getWidth()
<< " height: " << profile->getHeight() << " fps: " << profile->getFps()
<< " format: " << profile->getFormat());
RCLCPP_INFO_STREAM(
logger_, "Stream " << stream_name_[stream_index] << " width: " << profile->getWidth()
<< " height: " << profile->getHeight() << " fps: " << profile->getFps()
<< " format: " << profile->getFormat());
pipeline_config_->enableStream(stream_profile_[stream_index]);
}
}
}
}
void OBCameraNode::setupPublishers() {
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using CameraInfo = sensor_msgs::msg::CameraInfo;
auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
if (use_intra_process_) {
point_cloud_qos_profile = rmw_qos_profile_default;
}
if (enable_colored_point_cloud_) {
depth_registration_cloud_pub_ = node_->create_publisher<PointCloud2>(
"depth_registered/points",
@@ -1159,13 +1244,24 @@ void OBCameraNode::setupPublishers() {
}
std::string name = stream_name_[stream_index];
std::string topic = name + "/image_raw";
auto image_qos = image_qos_[stream_index];
auto image_qos_profile = getRMWQosProfileFromString(image_qos);
image_publishers_[stream_index] =
image_transport::create_publisher(node_, topic, image_qos_profile);
auto image_qos_profile = image_qos_[stream_index];
// if (use_intra_process_) {
// image_qos_profile = rmw_qos_profile_default;
// }
// if (use_intra_process_) {
// image_publishers_[stream_index] =
// std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
// } else {
// image_publishers_[stream_index] =
// std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
// }
topic = name + "/camera_info";
auto camera_info_qos = camera_info_qos_[stream_index];
auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos);
if (use_intra_process_) {
camera_info_qos_profile = rmw_qos_profile_default;
}
camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
@@ -1176,15 +1272,23 @@ void OBCameraNode::setupPublishers() {
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
}
if (stream_index == COLOR && enable_color_undistortion_) {
color_undistortion_publisher_ =
image_transport::create_publisher(node_, "color/image_undistorted", image_qos_profile);
}
// if (stream_index == COLOR && enable_color_undistortion_) {
// if (use_intra_process_) {
// color_undistortion_publisher_ = std::make_shared<image_rcl_publisher>(
// *node_, "color/image_undistorted", image_qos_profile);
// } else {
// color_undistortion_publisher_ = std::make_shared<image_transport_publisher>(
// *node_, "color/image_undistorted", image_qos_profile);
// }
// }
}
if (enable_sync_output_accel_gyro_) {
std::string topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]);
if (use_intra_process_) {
data_qos = rmw_qos_profile_default;
}
imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
topic_name = stream_name_[GYRO] + "/imu_info";
@@ -1200,6 +1304,9 @@ void OBCameraNode::setupPublishers() {
}
std::string data_topic_name = stream_name_[stream_index] + "/sample";
auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
if (use_intra_process_) {
data_qos = rmw_qos_profile_default;
}
imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
data_topic_name = stream_name_[stream_index] + "/imu_info";
@@ -1209,38 +1316,42 @@ void OBCameraNode::setupPublishers() {
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
}
}
auto extrinsics_qos = rclcpp::QoS(1).transient_local();
if (use_intra_process_) {
extrinsics_qos = rclcpp::QoS(1);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA0]) {
depth_to_other_extrinsics_publishers_[INFRA0] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_ir", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_ir", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[COLOR]) {
depth_to_other_extrinsics_publishers_[COLOR] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_color", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_color", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) {
depth_to_other_extrinsics_publishers_[INFRA1] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_left_ir", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_left_ir", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) {
depth_to_other_extrinsics_publishers_[INFRA2] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_right_ir", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_right_ir", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[ACCEL]) {
depth_to_other_extrinsics_publishers_[ACCEL] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_accel", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_accel", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[GYRO]) {
depth_to_other_extrinsics_publishers_[GYRO] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_gyro", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_gyro", extrinsics_qos);
}
filter_status_pub_ = node_->create_publisher<std_msgs::msg::String>(
"depth_filter_status", rclcpp::QoS(1).transient_local());
filter_status_pub_ =
node_->create_publisher<std_msgs::msg::String>("depth_filter_status", extrinsics_qos);
std_msgs::msg::String msg;
msg.data = filter_status_.dump(2);
filter_status_pub_->publish(msg);
@@ -1249,15 +1360,12 @@ void OBCameraNode::setupPublishers() {
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
try {
if (depth_registration_ || enable_colored_point_cloud_) {
if (frame_set->depthFrame()!= nullptr && frame_set->colorFrame()!= nullptr) {
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
publishColoredPointCloud(frame_set);
}
}
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set);
}
@@ -1277,6 +1385,10 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
return;
}
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
if (!depth_frame_) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
}
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
if (!depth_frame) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
@@ -1284,8 +1396,11 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
}
CHECK_NOTNULL(pipeline_);
auto camera_params = pipeline_->getCameraParam();
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info.get());
auto pid = device_info->getPid();
if (depth_registration_) {
if (depth_registration_ || pid == DABAI_MAX_PID) {
camera_params.depthIntrinsic = camera_params.rgbIntrinsic;
}
depth_point_cloud_filter_.setCameraParam(camera_params);
@@ -1343,6 +1458,9 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
auto frame_timestamp = getFrameTimestampUs(depth_frame);
auto timestamp = fromUsToROSTime(frame_timestamp);
std::string frame_id = depth_registration_ ? optical_frame_id_[COLOR] : optical_frame_id_[DEPTH];
if (!cloud_frame_id_.empty()) {
frame_id = cloud_frame_id_;
}
point_cloud_msg->header.stamp = timestamp;
point_cloud_msg->header.frame_id = frame_id;
if (save_point_cloud_) {
@@ -1373,6 +1491,10 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
}
CHECK_NOTNULL(depth_frame_.get());
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
if (!depth_frame_) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
}
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
auto color_frame = frame_set->colorFrame();
@@ -1391,7 +1513,6 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
}
if (!xy_tables_.has_value()) {
calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
uint32_t table_size =
@@ -1471,9 +1592,13 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
modifier.resize(valid_count);
}
auto frame_timestamp = getFrameTimestampUs(depth_frame);
std::string frame_id = optical_frame_id_[COLOR];
if (!cloud_frame_id_.empty()) {
frame_id = cloud_frame_id_;
}
auto timestamp = fromUsToROSTime(frame_timestamp);
point_cloud_msg->header.stamp = timestamp;
point_cloud_msg->header.frame_id = optical_frame_id_[COLOR];
point_cloud_msg->header.frame_id = frame_id;
if (save_colored_point_cloud_) {
save_colored_point_cloud_ = false;
auto now = std::time(nullptr);
@@ -1501,6 +1626,9 @@ std::shared_ptr<ob::Frame> OBCameraNode::processDepthFrameFilter(
if (frame == nullptr || frame->getType() != OB_FRAME_DEPTH) {
return nullptr;
}
auto sensor = device_->getSensor(OB_SENSOR_DEPTH);
CHECK_NOTNULL(sensor.get());
auto filter_list = sensor->getRecommendedFilters();
for (size_t i = 0; i < filter_list_.size(); i++) {
auto filter = filter_list_[i];
CHECK_NOTNULL(filter.get());
@@ -1553,10 +1681,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
CHECK_NOTNULL(device_info.get());
auto pid = device_info->getPid();
auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
has_first_color_frame_ = has_first_color_frame_ || color_frame;
if (isGemini335PID(pid)) {
depth_frame_ = processDepthFrameFilter(depth_frame_);
if (depth_registration_ && align_filter_ && depth_frame_ && color_frame) {
if (depth_registration_ && align_filter_ && depth_frame_ && has_first_color_frame_) {
auto new_frame = align_filter_->process(frame_set);
if (new_frame) {
auto new_frame_set = new_frame->as<ob::FrameSet>();
@@ -1569,6 +1697,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
RCLCPP_DEBUG(logger_,
"Depth registration is disabled or align filter is null or depth frame is "
"null or color frame is null");
return;
}
if (depth_registration_ && align_filter_ && depth_frame_ && !has_first_color_frame_) {
return;
}
}
@@ -1675,7 +1807,8 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
if (!rgb_buffer_) {
return false;
}
bool has_subscriber = image_publishers_[COLOR].getNumSubscribers() > 0;
CHECK_NOTNULL(image_publishers_[COLOR]);
bool has_subscriber = image_publishers_[COLOR]->get_subscription_count() > 0;
if (enable_colored_point_cloud_ && depth_registration_cloud_pub_->get_subscription_count() > 0) {
has_subscriber = true;
}
@@ -1738,8 +1871,9 @@ std::shared_ptr<ob::Frame> OBCameraNode::decodeIRMJPGFrame(
cv::Mat mjpgMat(1, video_frame->getDataSize(), CV_8UC1, video_frame->getData());
cv::Mat irRawMat = cv::imdecode(mjpgMat, cv::IMREAD_GRAYSCALE);
std::shared_ptr<ob::Frame> irFrame = ob::FrameFactory::createVideoFrame(
video_frame->getType(), video_frame->getFormat(), video_frame->getWidth(), video_frame->getHeight(), 0);
std::shared_ptr<ob::Frame> irFrame =
ob::FrameFactory::createVideoFrame(video_frame->getType(), video_frame->getFormat(),
video_frame->getWidth(), video_frame->getHeight(), 0);
uint32_t buffer_size = irRawMat.rows * irRawMat.cols * irRawMat.channels();
@@ -1764,7 +1898,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (frame == nullptr) {
return;
}
bool has_subscriber = image_publishers_[stream_index].getNumSubscribers() > 0;
CHECK_NOTNULL(image_publishers_[stream_index]);
bool has_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0;
has_subscriber =
has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0;
has_subscriber =
@@ -1811,11 +1946,19 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
: camera_params.depthIntrinsic;
distortion = stream_index.first == OB_STREAM_COLOR ? camera_params.rgbDistortion
: camera_params.depthDistortion;
if (pid == DABAI_MAX_PID) {
// use color param
intrinsic = camera_params.rgbIntrinsic;
distortion = camera_params.rgbDistortion;
}
}
std::string frame_id = optical_frame_id_[stream_index];
if (depth_registration_ && stream_index == DEPTH) {
frame_id = depth_aligned_frame_id_[stream_index];
}
if (stream_index == COLOR && enable_color_undistortion_) {
memset(&distortion, 0, sizeof(distortion));
}
auto camera_info = convertToCameraInfo(intrinsic, distortion, width);
camera_info.header.stamp = timestamp;
camera_info.header.frame_id = frame_id;
@@ -1839,7 +1982,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (isGemini335PID(pid)) {
publishMetadata(frame, stream_index, camera_info.header);
}
if (image_publishers_[stream_index].getNumSubscribers() == 0) {
CHECK_NOTNULL(image_publishers_[stream_index]);
if (image_publishers_[stream_index]->get_subscription_count() == 0) {
return;
}
auto &image = images_[stream_index];
@@ -1859,28 +2003,30 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
image = image * depth_scale;
}
auto image_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image).toImageMsg();
sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
.toImageMsg(*image_msg);
CHECK_NOTNULL(image_msg.get());
image_msg->header.stamp = timestamp;
image_msg->is_bigendian = false;
image_msg->step = width * unit_step_size_[stream_index];
image_msg->header.frame_id = frame_id;
CHECK(image_publishers_.count(stream_index) > 0);
saveImageToFile(stream_index, image, image_msg);
image_publishers_[stream_index].publish(std::move(image_msg));
saveImageToFile(stream_index, image, *image_msg);
image_publishers_[stream_index]->publish(std::move(image_msg));
if (stream_index == COLOR && enable_color_undistortion_ &&
color_undistortion_publisher_.getNumSubscribers() > 0) {
color_undistortion_publisher_->get_subscription_count() > 0) {
auto undistorted_image = undistortImage(image, intrinsic, distortion);
auto undistorted_image_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
.toImageMsg();
sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
.toImageMsg(*undistorted_image_msg);
CHECK_NOTNULL(undistorted_image_msg.get());
undistorted_image_msg->header.stamp = timestamp;
undistorted_image_msg->is_bigendian = false;
undistorted_image_msg->step = width * unit_step_size_[stream_index];
undistorted_image_msg->header.frame_id = frame_id;
color_undistortion_publisher_.publish(std::move(undistorted_image_msg));
color_undistortion_publisher_->publish(std::move(undistorted_image_msg));
}
}
@@ -1912,7 +2058,7 @@ void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
}
void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image,
const sensor_msgs::msg::Image::SharedPtr &image_msg) {
const sensor_msgs::msg::Image &image_msg) {
if (save_images_[stream_index]) {
auto now = time(nullptr);
std::stringstream ss;
@@ -1922,8 +2068,8 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
int index = save_images_count_[stream_index];
std::string file_suffix = stream_index == COLOR ? ".png" : ".raw";
std::string filename = current_path + "/image/" + stream_name_[stream_index] + "_" +
std::to_string(image_msg->width) + "x" +
std::to_string(image_msg->height) + "_" + std::to_string(fps) + "hz_" +
std::to_string(image_msg.width) + "x" +
std::to_string(image_msg.height) + "_" + std::to_string(fps) + "hz_" +
ss.str() + "_" + std::to_string(index) + file_suffix;
if (!std::filesystem::exists(current_path + "/image")) {
std::filesystem::create_directory(current_path + "/image");
@@ -2231,6 +2377,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[COLOR] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[COLOR]);
depth_to_other_extrinsics_publishers_[COLOR]->publish(ex_msg);
}
@@ -2246,6 +2393,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[INFRA0] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA0]);
depth_to_other_extrinsics_publishers_[INFRA0]->publish(ex_msg);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) {
@@ -2260,6 +2408,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[INFRA1] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA1]);
depth_to_other_extrinsics_publishers_[INFRA1]->publish(ex_msg);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) {
@@ -2275,6 +2424,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex.trans[0] = -std::abs(ex.trans[0]);
depth_to_other_extrinsics_[INFRA2] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA2]);
depth_to_other_extrinsics_publishers_[INFRA2]->publish(ex_msg);
}
if (enable_stream_[DEPTH] && enable_stream_[ACCEL]) {
@@ -2289,6 +2439,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[ACCEL] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[ACCEL]);
depth_to_other_extrinsics_publishers_[ACCEL]->publish(ex_msg);
}
if (enable_stream_[DEPTH] && enable_stream_[GYRO]) {
@@ -2303,6 +2454,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[GYRO] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[GYRO]);
depth_to_other_extrinsics_publishers_[GYRO]->publish(ex_msg);
}
}
orbbec_camera/src/ob_camera_node_driver.cpp
+74 -31
View File
@@ -15,6 +15,7 @@
*******************************************************************************/
#include "orbbec_camera/ob_camera_node_driver.h"
#include "orbbec_camera/utils.h"
#include <fcntl.h>
#include <semaphore.h>
#include <sys/shm.h>
@@ -38,7 +39,7 @@ void signalHandler(int sig) {
// get current time
std::time_t now = std::time(nullptr);
std::tm* local_time = std::localtime(&now);
std::tm *local_time = std::localtime(&now);
// format date and time to string, format as "2024_05_20_12_34_56"
std::ostringstream time_stream;
@@ -69,24 +70,27 @@ void signalHandler(int sig) {
}
namespace orbbec_camera {
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions& node_options)
backward::SignalHandling OBCameraNodeDriver::sh;
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options),
node_options_(node_options),
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
"/config/OrbbecSDKConfig_v1.0.xml"),
ctx_(std::make_unique<ob::Context>(config_path_.c_str())),
logger_(this->get_logger()),
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") +
"/lib/extensions") {
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
init();
}
OBCameraNodeDriver::OBCameraNodeDriver(const std::string& node_name, const std::string& ns,
const rclcpp::NodeOptions& node_options)
OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::string &ns,
const rclcpp::NodeOptions &node_options)
: Node(node_name, ns, node_options),
node_options_(node_options),
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
"/config/OrbbecSDKConfig_v1.0.xml"),
ctx_(std::make_unique<ob::Context>()),
logger_(this->get_logger()),
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") +
"/lib/extensions") {
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
init();
}
@@ -128,8 +132,8 @@ void OBCameraNodeDriver::init() {
return;
}
orb_device_lock_shm_addr_ =
static_cast<uint8_t*>(mmap(NULL, sizeof(pthread_mutex_t), PROT_READ | PROT_WRITE, MAP_SHARED,
orb_device_lock_shm_fd_, 0));
static_cast<uint8_t *>(mmap(NULL, sizeof(pthread_mutex_t), PROT_READ | PROT_WRITE, MAP_SHARED,
orb_device_lock_shm_fd_, 0));
if (orb_device_lock_shm_addr_ == MAP_FAILED) {
RCLCPP_ERROR_STREAM(logger_, "Failed to map shared memory " << ORB_DEFAULT_LOCK_NAME);
return;
@@ -139,7 +143,7 @@ void OBCameraNodeDriver::init() {
std::placeholders::_1, std::placeholders::_2));
pthread_mutexattr_init(&orb_device_lock_attr_);
pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
orb_device_lock_ = (pthread_mutex_t*)orb_device_lock_shm_addr_;
orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
pthread_mutex_init(orb_device_lock_, &orb_device_lock_attr_);
is_alive_.store(true);
parameters_ = std::make_shared<Parameters>(this);
@@ -150,8 +154,8 @@ void OBCameraNodeDriver::init() {
net_device_port_ = static_cast<int>(declare_parameter<int>("net_device_port", 0));
enumerate_net_device_ = declare_parameter<bool>("enumerate_net_device", false);
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
ctx_->setDeviceChangedCallback([this](const std::shared_ptr<ob::DeviceList>& removed_list,
const std::shared_ptr<ob::DeviceList>& added_list) {
ctx_->setDeviceChangedCallback([this](const std::shared_ptr<ob::DeviceList> &removed_list,
const std::shared_ptr<ob::DeviceList> &added_list) {
onDeviceConnected(added_list);
onDeviceDisconnected(removed_list);
});
@@ -162,7 +166,7 @@ void OBCameraNodeDriver::init() {
reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
}
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>& device_list) {
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(device_list);
if (device_list->getCount() == 0) {
return;
@@ -172,7 +176,7 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>
}
}
void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceList>& device_list) {
void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(device_list);
if (device_list->getCount() == 0) {
return;
@@ -194,7 +198,7 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
}
}
OBLogSeverity OBCameraNodeDriver::obLogSeverityFromString(const std::string_view& log_level) {
OBLogSeverity OBCameraNodeDriver::obLogSeverityFromString(const std::string_view &log_level) {
if (log_level == "debug") {
return OBLogSeverity::OB_LOG_SEVERITY_DEBUG;
} else if (log_level == "info") {
@@ -276,7 +280,7 @@ void OBCameraNodeDriver::rebootDeviceCallback(
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
const std::shared_ptr<ob::DeviceList>& list) {
const std::shared_ptr<ob::DeviceList> &list) {
if (device_num_ == 1) {
RCLCPP_INFO_STREAM(logger_, "Connecting to the default device");
return list->getDevice(0);
@@ -300,7 +304,7 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
const std::shared_ptr<ob::DeviceList>& list, const std::string& serial_number) {
const std::shared_ptr<ob::DeviceList> &list, const std::string &serial_number) {
std::string lower_sn;
std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
[](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
@@ -327,10 +331,10 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
return list->getDevice(i);
}
}
} catch (ob::Error& e) {
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000,
"Failed to get device info " << e.getMessage());
} catch (std::exception& e) {
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info");
@@ -340,17 +344,24 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
const std::shared_ptr<ob::DeviceList>& list, const std::string& usb_port) {
const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
try {
RCLCPP_INFO_STREAM(logger_, "Before lock: Select device usb port: " << usb_port);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM(logger_, "After lock: Select device usb port: " << usb_port);
auto device = list->getDeviceByUid(usb_port.c_str());
RCLCPP_INFO_STREAM(logger_, "Device usb port " << usb_port << " done");
if (device) {
RCLCPP_INFO_STREAM(logger_, "getDeviceByUid device usb port " << usb_port << " done");
} else {
RCLCPP_ERROR_STREAM(logger_, "getDeviceByUid device usb port " << usb_port << " failed");
RCLCPP_ERROR_STREAM(logger_,
"Please use script to get usb port: "
"ros2 run orbbec_camera list_devices_node");
}
return device;
} catch (ob::Error& e) {
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.getMessage());
} catch (std::exception& e) {
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info");
@@ -359,14 +370,46 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
return nullptr;
}
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device>& device) {
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
device_ = device;
CHECK_NOTNULL(device_);
CHECK_NOTNULL(device_.get());
if (ob_camera_node_) {
ob_camera_node_.reset();
}
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_);
int retry_count = 0;
constexpr int max_retries = 3;
bool initialized = false;
device_info_ = device_->getDeviceInfo();
RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
while (retry_count < max_retries && !initialized) {
try {
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
node_options_.use_intra_process_comms());
initialized = true;
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
<< retry_count + 1 << " of " << max_retries
<< "): " << e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt " << retry_count + 1
<< " of " << max_retries
<< "): " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
<< retry_count + 1 << " of " << max_retries << ")");
}
retry_count++;
}
if (!initialized) {
RCLCPP_ERROR_STREAM(logger_,
"Device initialization failed after " << max_retries << " attempts.");
throw std::runtime_error("Device initialization failed after " + std::to_string(max_retries) +
" attempts.");
}
ob_camera_node_->startIMU();
ob_camera_node_->startStreams();
device_connected_ = true;
@@ -397,7 +440,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device>& dev
RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
} // namespace orbbec_camera
void OBCameraNodeDriver::connectNetDevice(const std::string& net_device_ip, int net_device_port) {
void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int net_device_port) {
if (net_device_ip.empty() || net_device_port == 0) {
RCLCPP_ERROR_STREAM(logger_, "Invalid net device ip or port");
return;
@@ -411,7 +454,7 @@ void OBCameraNodeDriver::connectNetDevice(const std::string& net_device_ip, int
initializeDevice(device);
}
void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList>& list) {
void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
if (device_connected_) {
return;
}
@@ -426,7 +469,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList>& list
std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
pthread_mutex_lock(orb_device_lock_);
std::shared_ptr<int> lock_holder(nullptr,
[this](int*) { pthread_mutex_unlock(orb_device_lock_); });
[this](int *) { pthread_mutex_unlock(orb_device_lock_); });
bool start_device_failed = false;
try {
auto start_time = std::chrono::high_resolution_clock::now();
@@ -445,10 +488,10 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList>& list
end_time = std::chrono::high_resolution_clock::now();
time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
} catch (ob::Error& e) {
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.getMessage());
start_device_failed = true;
} catch (std::exception& e) {
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.what());
start_device_failed = true;
} catch (...) {
orbbec_camera/tools/frame_latency.cpp
+123
View File
@@ -0,0 +1,123 @@
// Copyright 2023 Intel Corporation. All Rights Reserved.
//
// 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.
// DESCRIPTION: #
// ------------ #
// This tool created a node which can be used to calulate the specified topic's latency.
// Input parameters:
// - topic_name : <String>
// - topic to which latency need to be calculated
// - topic_type : <String>
// - Message type of the topic.
// - Valid inputs: {'image','points','imu','metadata','camera_info','rgbd','imu_info','tf'}
// Note:
// - This tool doesn't support calulating latency for extrinsic topics.
// Because, those topics doesn't have timestamp in it and this tool uses
// that timestamp as an input to calculate the latency.
//
#include <sstream>
#include <string>
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <chrono>
using namespace std::chrono_literals;
#include "frame_latency.hpp"
namespace orbbec_camera {
FrameLatencyNode::FrameLatencyNode(const std::string& node_name, const std::string& ns,
const rclcpp::NodeOptions& node_options)
: Node(node_name, ns, node_options), logger_(this->get_logger()) {}
std::string topic_name = "/camera/color/image_raw";
std::string topic_type = "image";
template <typename MsgType>
void FrameLatencyNode::createListener(const std::string& topic_name,
const rmw_qos_profile_t qos_profile) {
RCLCPP_INFO_STREAM(logger_, "createListener");
using namespace std::chrono_literals;
timer_ = this->create_wall_timer(1s, [this, topic_name = topic_name]() {
// print fps
RCLCPP_INFO_STREAM(logger_, "topic: " << topic_name << " fps: " << frame_count_ / 1.0);
frame_count_ = 0;
});
sub_ = this->create_subscription<MsgType>(
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile),
[&, this](const std::shared_ptr<MsgType> msg) {
rclcpp::Time curr_time = this->get_clock()->now();
auto latency = (curr_time - msg->header.stamp).seconds();
frame_count_++;
RCLCPP_INFO_STREAM_THROTTLE(logger_, *this->get_clock(), 1000.0,
"Got msg with "
<< msg->header.frame_id << " frame id at address 0x"
<< std::hex << reinterpret_cast<std::uintptr_t>(msg.get())
<< std::dec << " with latency of " << latency << " [sec]");
});
}
void FrameLatencyNode::createTFListener(const std::string& topic_name,
const rmw_qos_profile_t qos_profile) {
sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>(
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile),
[&, this](const std::shared_ptr<tf2_msgs::msg::TFMessage> msg) {
rclcpp::Time curr_time = this->get_clock()->now();
auto latency = (curr_time - msg->transforms.back().header.stamp).seconds();
RCLCPP_INFO_STREAM_THROTTLE(
logger_, *this->get_clock(), 1000.0,
"Got msg with " << msg->transforms.back().header.frame_id << " frame id at address 0x"
<< std::hex << reinterpret_cast<std::uintptr_t>(msg.get()) << std::dec
<< " with latency of " << latency << " [sec]");
});
}
FrameLatencyNode::FrameLatencyNode(const rclcpp::NodeOptions& node_options)
: Node("frame_latency", "/", node_options), logger_(this->get_logger()) {
RCLCPP_INFO_STREAM(logger_, "frame_latency node is UP!");
RCLCPP_INFO_STREAM(
logger_,
"Intra-Process is " << (this->get_node_options().use_intra_process_comms() ? "ON" : "OFF"));
topic_name = this->declare_parameter("topic_name", topic_name);
topic_type = this->declare_parameter("topic_type", topic_type);
RCLCPP_INFO_STREAM(logger_, "Subscribing to Topic: " << topic_name);
if (topic_type == "image") {
createListener<sensor_msgs::msg::Image>(topic_name, rmw_qos_profile_default);
} else if (topic_type == "points") {
createListener<sensor_msgs::msg::PointCloud2>(topic_name, rmw_qos_profile_default);
} else if (topic_type == "imu") {
createListener<sensor_msgs::msg::Imu>(topic_name, rmw_qos_profile_sensor_data);
} else if (topic_type == "metadata") {
createListener<orbbec_camera_msgs::msg::Metadata>(topic_name, rmw_qos_profile_default);
} else if (topic_type == "camera_info") {
createListener<sensor_msgs::msg::CameraInfo>(topic_name, rmw_qos_profile_default);
} else if (topic_type == "rgbd") {
createListener<orbbec_camera_msgs::msg::RGBD>(topic_name, rmw_qos_profile_default);
} else if (topic_type == "imu_info") {
createListener<orbbec_camera_msgs::msg::IMUInfo>(topic_name, rmw_qos_profile_default);
} else if (topic_type == "tf") {
createTFListener(topic_name, rmw_qos_profile_default);
} else {
RCLCPP_ERROR_STREAM(logger_, "Specified message type '" << topic_type << "' is not supported");
}
}
} // namespace orbbec_camera
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::FrameLatencyNode)
orbbec_camera/tools/frame_latency.hpp
+55
View File
@@ -0,0 +1,55 @@
// Copyright 2023 Intel Corporation. All Rights Reserved.
//
// 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 "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>
#include "orbbec_camera_msgs/msg/imu_info.hpp"
#include "orbbec_camera_msgs/msg/extrinsics.hpp"
#include "orbbec_camera_msgs/msg/metadata.hpp"
#include "orbbec_camera_msgs/msg/rgbd.hpp"
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
namespace orbbec_camera {
class FrameLatencyNode : public rclcpp::Node {
public:
explicit FrameLatencyNode(const rclcpp::NodeOptions& node_options =
rclcpp::NodeOptions().use_intra_process_comms(true));
FrameLatencyNode(const std::string& node_name, const std::string& ns,
const rclcpp::NodeOptions& node_options =
rclcpp::NodeOptions().use_intra_process_comms(true));
template <typename MsgType>
void createListener(const std::string& topic_name, rmw_qos_profile_t qos_profile);
void createTFListener(const std::string& topic_name, rmw_qos_profile_t qos_profile);
private:
std::shared_ptr<void> sub_ = nullptr;
rclcpp::Logger logger_;
rclcpp::TimerBase::SharedPtr timer_;
size_t frame_count_ = 0;
};
} // namespace orbbec_camera