diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index f2e0ceb2..6df524d8 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -143,6 +143,7 @@ set(SOURCE_FILES src/image_publisher.cpp src/ob_camera_node_driver.cpp src/ob_camera_node.cpp + src/ob_lidar_node.cpp src/ros_param_backend.cpp src/ros_service.cpp src/synced_imu_publisher.cpp diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 1e74976d..b93a0f5e 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -119,11 +119,12 @@ const stream_index_pair DEPTH{OB_STREAM_DEPTH, 0}; const stream_index_pair INFRA0{OB_STREAM_IR, 0}; const stream_index_pair INFRA1{OB_STREAM_IR_LEFT, 0}; const stream_index_pair INFRA2{OB_STREAM_IR_RIGHT, 0}; +const stream_index_pair LIDAR{OB_STREAM_LIDAR, 0}; const stream_index_pair GYRO{OB_STREAM_GYRO, 0}; const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0}; -const std::vector IMAGE_STREAMS = {COLOR, DEPTH, INFRA0, INFRA1, INFRA2}; +const std::vector IMAGE_STREAMS = {COLOR, DEPTH, INFRA0, INFRA1, INFRA2, LIDAR}; const std::vector HID_STREAMS = {GYRO, ACCEL}; diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h index 623bba76..4e7f03b8 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -20,6 +20,7 @@ #include #include #include "ob_camera_node.h" +#include "ob_lidar_node.h" #include "utils.h" #include "dynamic_params.h" @@ -83,6 +84,7 @@ class OBCameraNodeDriver : public rclcpp::Node { std::unique_ptr ctx_ = nullptr; rclcpp::Logger logger_; std::unique_ptr ob_camera_node_ = nullptr; + std::unique_ptr ob_lidar_node_ = nullptr; std::shared_ptr device_ = nullptr; std::shared_ptr device_info_ = nullptr; std::atomic_bool is_alive_{false}; @@ -118,5 +120,6 @@ class OBCameraNodeDriver : public rclcpp::Node { std::string extension_path_; static backward::SignalHandling sh; // for stack trace std::string upgrade_firmware_; + std::string device_type_; }; } // namespace orbbec_camera diff --git a/orbbec_camera/include/orbbec_camera/ob_lidar_node.h b/orbbec_camera/include/orbbec_camera/ob_lidar_node.h new file mode 100644 index 00000000..09429eb0 --- /dev/null +++ b/orbbec_camera/include/orbbec_camera/ob_lidar_node.h @@ -0,0 +1,472 @@ +/******************************************************************************* + * Copyright (c) 2023 Orbbec 3D Technology, Inc + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *******************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "ob_camera_node.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "libobsensor/ObSensor.hpp" + +#include "orbbec_camera_msgs/msg/device_info.hpp" +#include "orbbec_camera_msgs/srv/get_device_info.hpp" +#include "orbbec_camera_msgs/msg/extrinsics.hpp" +#include "orbbec_camera_msgs/msg/metadata.hpp" +#include "orbbec_camera_msgs/msg/imu_info.hpp" +#include "orbbec_camera_msgs/srv/get_int32.hpp" +#include "orbbec_camera_msgs/srv/get_string.hpp" +#include "orbbec_camera_msgs/srv/set_int32.hpp" +#include "orbbec_camera_msgs/srv/get_bool.hpp" +#include "orbbec_camera_msgs/srv/set_string.hpp" +#include "orbbec_camera_msgs/srv/set_filter.hpp" +#include "orbbec_camera_msgs/srv/set_arrays.hpp" +#include "orbbec_camera/constants.h" +#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 +#include +#include + +#if defined(ROS_JAZZY) || defined(ROS_IRON) +#include +#else +#include +#endif + +#define STREAM_NAME(sip) \ + (static_cast(std::ostringstream() \ + << _stream_name[sip.first] \ + << ((sip.second > 0) ? std::to_string(sip.second) : ""))) \ + .str() +#define FRAME_ID(sip) \ + (static_cast(std::ostringstream() \ + << getNamespaceStr() << "_" << STREAM_NAME(sip) << "_frame")) \ + .str() +#define OPTICAL_FRAME_ID(sip) \ + (static_cast( \ + std::ostringstream() << getNamespaceStr() << "_" << STREAM_NAME(sip) << "_optical_frame")) \ + .str() +#define ALIGNED_DEPTH_TO_FRAME_ID(sip) \ + (static_cast(std::ostringstream() \ + << getNamespaceStr() << "_aligned_depth_to_" \ + << STREAM_NAME(sip) << "_frame")) \ + .str() +#define BASE_FRAME_ID() \ + (static_cast(std::ostringstream() << getNamespaceStr() << "_link")).str() +#define ODOM_FRAME_ID() \ + (static_cast(std::ostringstream() << getNamespaceStr() << "_odom_frame")) \ + .str() + +#define DEVICE_PATH "/dev/camsync" + +namespace orbbec_camera { +using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo; +using Extrinsics = orbbec_camera_msgs::msg::Extrinsics; +using SetInt32 = orbbec_camera_msgs::srv::SetInt32; +using GetInt32 = orbbec_camera_msgs::srv::GetInt32; +using GetString = orbbec_camera_msgs::srv::GetString; +using SetString = orbbec_camera_msgs::srv::SetString; +using SetBool = std_srvs::srv::SetBool; +using GetBool = orbbec_camera_msgs::srv::GetBool; +using SetFilter = orbbec_camera_msgs::srv::SetFilter; +using SetArrays = orbbec_camera_msgs::srv::SetArrays; + +class OBLidarNode { + public: + OBLidarNode(rclcpp::Node* node, std::shared_ptr device, + std::shared_ptr parameters, bool use_intra_process = false); + + template + void setAndGetNodeParameter( + T& param, const std::string& param_name, const T& default_value, + const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor()); // set and get parameter + + ~OBLidarNode() noexcept; + + void clean() noexcept; + + void rebootDevice(); + + private: + + void setupDevices(); + +// void selectBaseStream(); + +// void setupProfiles(); + + void getParameters(); + + void setupTopics(); + + void setupPublishers(); + + private: + rclcpp::Node* node_ = nullptr; + std::shared_ptr device_ = nullptr; + std::shared_ptr parameters_ = nullptr; + rclcpp::Logger logger_; + std::atomic_bool is_running_{false}; + std::unique_ptr pipeline_ = nullptr; + std::unique_ptr imuPipeline_ = nullptr; + std::atomic_bool pipeline_started_{false}; + std::string camera_name_ = "camera"; + std::string accel_gyro_frame_id_ = "camera_accel_gyro_optical_frame"; + const std::string imu_frame_id_ = "camera_gyro_frame"; + std::shared_ptr pipeline_config_ = nullptr; + std::map> sensors_; + std::map stream_intrinsics_; + std::map camera_infos_; + std::map ob_camera_param_; + std::map depth_to_other_extrinsics_; + std::map::SharedPtr> + depth_to_other_extrinsics_publishers_; + std::map::SharedPtr> + metadata_publishers_; + std::map::SharedPtr> + imu_info_publishers_; + std::map width_; + std::map height_; + std::map fps_; + std::map frame_id_; + std::map optical_frame_id_; + std::map depth_aligned_frame_id_; + std::string camera_link_frame_id_; + bool depth_registration_ = false; + std::map image_qos_; + std::map camera_info_qos_; + std::map format_; + std::map format_str_; + std::map image_format_; + std::map>> + supported_profiles_; + std::map> stream_profile_; + stream_index_pair base_stream_ = DEPTH; + std::map seq_; + std::map images_; + std::map encoding_; + std::map unit_step_size_; + std::vector compression_params_; + ob::FormatConvertFilter format_convert_filter_; + + std::map enable_stream_; + std::map flip_stream_; + std::map mirror_stream_; + std::map rotation_stream_; + std::map stream_name_; + std::map> image_publishers_; + std::map::SharedPtr> + camera_info_publishers_; + + std::map::SharedPtr> get_exposure_srv_; + std::map::SharedPtr> set_exposure_srv_; + std::map::SharedPtr> get_gain_srv_; + std::map::SharedPtr> set_gain_srv_; + std::map::SharedPtr> toggle_sensor_srv_; + std::map::SharedPtr> set_mirror_srv_; + std::map::SharedPtr> set_flip_srv_; + std::map::SharedPtr> set_rotation_srv_; + rclcpp::Service::SharedPtr get_white_balance_srv_; + rclcpp::Service::SharedPtr set_white_balance_srv_; + rclcpp::Service::SharedPtr get_auto_white_balance_srv_; + rclcpp::Service::SharedPtr set_auto_white_balance_srv_; + rclcpp::Service::SharedPtr get_sdk_version_srv_; + rclcpp::Service::SharedPtr switch_ir_camera_srv_; + rclcpp::Service::SharedPtr set_write_customerdata_srv_; + rclcpp::Service::SharedPtr set_read_customerdata_srv_; + rclcpp::Service::SharedPtr set_ir_long_exposure_srv_; + std::map::SharedPtr> + set_auto_exposure_srv_; + std::map::SharedPtr> set_ae_roi_srv_; + rclcpp::Service::SharedPtr get_device_srv_; + rclcpp::Service::SharedPtr set_laser_enable_srv_; + rclcpp::Service::SharedPtr set_ldp_enable_srv_; + rclcpp::Service::SharedPtr get_ldp_status_srv_; + rclcpp::Service::SharedPtr set_floor_enable_srv_; + rclcpp::Service::SharedPtr set_fan_work_mode_srv_; + rclcpp::Service::SharedPtr toggle_sensors_srv_; + rclcpp::Service::SharedPtr get_lrm_measure_distance_srv_; + rclcpp::Service::SharedPtr set_reset_timestamp_srv_; + rclcpp::Service::SharedPtr set_interleaver_laser_sync_srv_; + rclcpp::Service::SharedPtr set_sync_host_time_srv_; + rclcpp::Service::SharedPtr set_filter_srv_; + + bool enable_sync_output_accel_gyro_ = false; + bool publish_tf_ = false; + bool tf_published_ = false; + std::shared_ptr static_tf_broadcaster_ = nullptr; + std::shared_ptr dynamic_tf_broadcaster_ = nullptr; + std::vector tf_msgs; + rclcpp::Publisher::SharedPtr depth_registration_cloud_pub_; + rclcpp::Publisher::SharedPtr cloud_pub_; + bool enable_point_cloud_ = true; + bool enable_colored_point_cloud_ = false; + std::recursive_mutex point_cloud_mutex_; + + orbbec_camera_msgs::msg::DeviceInfo device_info_; + std::string point_cloud_qos_; + std::vector static_tf_msgs_; + std::shared_ptr tf_thread_ = nullptr; + std::condition_variable tf_cv_; + double tf_publish_rate_ = 10.0; + std::unique_ptr ir_info_manager_ = nullptr; + std::unique_ptr color_info_manager_ = nullptr; + std::string color_info_url_; + std::string ir_info_url_; + std::optional camera_param_; + bool enable_d2c_viewer_ = false; + std::unique_ptr d2c_viewer_ = nullptr; + std::map save_images_; + std::map save_images_count_; + int max_save_images_count_ = 10; + std::atomic_bool save_point_cloud_{false}; + std::atomic_bool save_colored_point_cloud_{false}; + rclcpp::Service::SharedPtr save_images_srv_; + rclcpp::Service::SharedPtr save_point_cloud_srv_; + std::string depth_filter_config_; + bool enable_depth_filter_ = false; + bool enable_color_auto_exposure_priority_ = false; + bool enable_color_auto_exposure_ = true; + bool enable_color_auto_white_balance_ = true; + bool enable_depth_auto_exposure_priority_ = false; + bool enable_ir_auto_exposure_ = true; + bool enable_ir_long_exposure_ = false; + bool enable_ldp_ = true; + int ldp_power_level_ = -1; + int color_rotation_ = -1; + // color ae roi + int color_ae_roi_left_ = -1; + int color_ae_roi_top_ = -1; + int color_ae_roi_right_ = -1; + int color_ae_roi_bottom_ = -1; + int color_exposure_ = -1; + int color_gain_ = -1; + int color_white_balance_ = -1; + int color_ae_max_exposure_ = -1; + int color_brightness_ = -1; + int color_sharpness_ = -1; + int color_gamma_ = -1; + int color_saturation_ = -1; + int color_constrast_ = -1; + int color_hue_ = -1; + bool enable_color_backlight_compenstation_ = false; + std::string color_powerline_freq_; + bool enable_color_decimation_filter_ = false; + int color_decimation_filter_scale_ = -1; + // depth ae roi + int depth_ae_roi_left_ = -1; + int depth_ae_roi_top_ = -1; + int depth_ae_roi_right_ = -1; + int depth_ae_roi_bottom_ = -1; + int depth_brightness_ = -1; + int ir_exposure_ = -1; + int ir_gain_ = -1; + int ir_ae_max_exposure_ = -1; + int ir_brightness_ = -1; + bool enable_right_ir_sequence_id_filter_ = false; + int right_ir_sequence_id_filter_id_ = -1; + bool enable_left_ir_sequence_id_filter_ = false; + int left_ir_sequence_id_filter_id_ = -1; + bool enable_frame_sync_ = false; + // Only for Gemini2 device + std::string disaparity_to_depth_mode_ = "HW"; + std::string depth_work_mode_; + OBMultiDeviceSyncMode sync_mode_ = OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN; + std::string sync_mode_str_; + int depth_delay_us_ = 0; + int color_delay_us_ = 0; + int trigger2image_delay_us_ = 0; + int trigger_out_delay_us_ = 0; + bool trigger_out_enabled_ = false; + int frames_per_trigger_ = 2; + bool enable_ptp_config_ = false; + std::string depth_precision_str_; + OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8; + double depth_precision_float_ = 0.10; + // IMU + std::map::SharedPtr> imu_publishers_; + rclcpp::Publisher::SharedPtr imu_gyro_accel_publisher_; + bool imu_sync_output_start_ = false; + std::map imu_rate_; + std::map imu_range_; + std::map imu_qos_; + std::map imu_started_; + double liner_accel_cov_ = 0.0001; + double angular_vel_cov_ = 0.0001; + bool enable_accel_data_correction_ = true; + bool enable_gyro_data_correction_ = true; + // mjpeg decoder + std::shared_ptr jpeg_decoder_ = nullptr; + uint8_t* rgb_buffer_ = nullptr; + bool is_color_frame_decoded_ = false; + std::mutex device_lock_; + // For color + std::queue> color_frame_queue_; + std::shared_ptr colorFrameThread_ = nullptr; + std::mutex color_frame_queue_lock_; + std::condition_variable color_frame_queue_cv_; + + bool ordered_pc_ = false; + bool enable_depth_scale_ = true; + std::string device_preset_ = "Default"; + // filter switch + bool enable_decimation_filter_ = false; + bool enable_hdr_merge_ = false; + bool enable_sequence_id_filter_ = false; + bool enable_disaparity_to_depth_ = true; + bool enable_threshold_filter_ = false; + bool enable_hardware_noise_removal_filter_ = true; + bool enable_noise_removal_filter_ = true; + bool enable_spatial_filter_ = true; + bool enable_temporal_filter_ = false; + bool enable_hole_filling_filter_ = false; + // filter params + int decimation_filter_scale_ = -1; + int sequence_id_filter_id_ = -1; + int threshold_filter_max_ = -1; + int threshold_filter_min_ = -1; + float hardware_noise_removal_filter_threshold_ = -1.0; + int noise_removal_filter_min_diff_ = 256; + int noise_removal_filter_max_size_ = 80; + float spatial_filter_alpha_ = -1; + int spatial_filter_diff_threshold_ = -1; + int spatial_filter_magnitude_ = -1; + int spatial_filter_radius_ = -1; + float temporal_filter_diff_threshold_ = -1.0; + float temporal_filter_weight_ = -1.0; + std::string hole_filling_filter_mode_; + int hdr_merge_exposure_1_ = -1; + int hdr_merge_gain_1_ = -1; + int hdr_merge_exposure_2_ = -1; + int hdr_merge_gain_2_ = -1; + int gmsl_trigger_fd_ = -1; + int gmsl_trigger_fps_ = -1; + bool enable_gmsl_trigger_ = false; + + rclcpp::Publisher::SharedPtr filter_status_pub_; + nlohmann::json filter_status_; + std::string align_mode_ = "HW"; + std::unique_ptr diagnostic_updater_ = nullptr; + double diagnostic_period_ = 1.0; + bool enable_laser_ = false; + std::unique_ptr align_filter_ = nullptr; + OBStreamType align_target_stream_ = OB_STREAM_COLOR; + bool retry_on_usb3_detection_failure_ = false; + std::atomic_bool is_camera_node_initialized_{false}; + int laser_energy_level_ = -1; + ob::PointCloudFilter depth_point_cloud_filter_; + ob::PointCloudFilter color_point_cloud_filter_; + std::optional calibration_param_; + std::optional xy_tables_; + float* xy_table_data_ = nullptr; + uint32_t xy_table_data_size_ = 0; + uint8_t* rgb_point_cloud_buffer_ = nullptr; + uint32_t rgb_point_cloud_buffer_size_ = 0; + std::optional depth_xy_tables_; + float* depth_xy_table_data_ = nullptr; + uint32_t depth_xy_table_data_size_ = 0; + uint8_t* depth_point_cloud_buffer_ = nullptr; + uint32_t depth_point_cloud_buffer_size_ = 0; + int min_depth_limit_ = 0; + int max_depth_limit_ = 0; + std::string time_domain_ = "global"; // device, system, global + std::string exposure_range_mode_ = "default"; + std::string load_config_json_file_path_ = ""; + std::string export_config_json_file_path_ = ""; + // soft ware trigger + rclcpp::TimerBase::SharedPtr software_trigger_timer_; + rclcpp::TimerBase::SharedPtr diagnostic_timer_; + std::chrono::milliseconds software_trigger_period_{33}; + bool enable_heartbeat_ = false; + bool enable_color_undistortion_ = false; + std::shared_ptr color_undistortion_publisher_; + bool has_first_color_frame_ = false; + bool use_intra_process_ = false; + std::string cloud_frame_id_; + std::vector> depth_filter_list_; + std::vector> color_filter_list_; + std::vector> left_ir_filter_list_; + std::vector> right_ir_filter_list_; + + // interleave AE + std::string interleave_ae_mode_ = "hdr"; // hdr or laser + bool interleave_frame_enable_ = false; + bool interleave_skip_enable_ = false; + int interleave_skip_index_ = 1; + + // hdr and laser interleave params + int hdr_index1_laser_control_ = 1; + int hdr_index1_depth_exposure_ = 1; + int hdr_index1_depth_gain_ = 16; + int hdr_index1_ir_brightness_ = 20; + int hdr_index1_ir_ae_max_exposure_ = 2000; + int hdr_index0_laser_control_ = 1; + int hdr_index0_depth_exposure_ = 7500; + int hdr_index0_depth_gain_ = 16; + int hdr_index0_ir_brightness_ = 60; + int hdr_index0_ir_ae_max_exposure_ = 10000; + + int laser_index1_laser_control_ = 0; + int laser_index1_depth_exposure_ = 3000; + int laser_index1_depth_gain_ = 16; + int laser_index1_ir_brightness_ = 60; + int laser_index1_ir_ae_max_exposure_ = 7000; + int laser_index0_laser_control_ = 1; + int laser_index0_depth_exposure_ = 3000; + int laser_index0_depth_gain_ = 16; + int laser_index0_ir_brightness_ = 60; + int laser_index0_ir_ae_max_exposure_ = 17000; + + int disparity_range_mode_ = -1; + int disparity_search_offset_ = -1; + bool disparity_offset_config_ = false; + int offset_index0_ = -1; + int offset_index1_ = -1; + + std::string frame_aggregate_mode_ = "ANY"; // # full_frame, color_frame, ANY or disable + std::string echo_mode_ = "single channel"; +}; +} // namespace orbbec_camera diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index cb8c9d76..3ab9eec0 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -51,6 +51,7 @@ def load_parameters(context, args): def generate_launch_description(): args = [ + DeclareLaunchArgument('device_type', default_value='camera'), DeclareLaunchArgument('camera_name', default_value='camera'), DeclareLaunchArgument('depth_registration', default_value='true'), DeclareLaunchArgument('serial_number', default_value=''), diff --git a/orbbec_camera/launch/lidar.launch.py b/orbbec_camera/launch/lidar.launch.py new file mode 100644 index 00000000..d606cc40 --- /dev/null +++ b/orbbec_camera/launch/lidar.launch.py @@ -0,0 +1,120 @@ +import os +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +def load_yaml(file_path): + with open(file_path, 'r') as f: + return yaml.safe_load(f) + + +def merge_params(default_params, yaml_params): + for key, value in yaml_params.items(): + if key in default_params: + default_params[key] = value + return default_params + + +def convert_value(value): + if isinstance(value, str): + try: + return int(value) + except ValueError: + pass + try: + return float(value) + except ValueError: + pass + if value.lower() == 'true': + return True + 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) + 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'} + return { + key: (value if key in skip_convert else convert_value(value)) + for key, value in default_params.items() + } + + +def generate_launch_description(): + args = [ + DeclareLaunchArgument('device_type', default_value='lidar'), + DeclareLaunchArgument('camera_name', default_value='lidar'), + DeclareLaunchArgument('device_num', default_value='1'), + DeclareLaunchArgument('upgrade_firmware', default_value=''), + DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('publish_tf', default_value='true'), + DeclareLaunchArgument('tf_publish_rate', default_value='0.0'), + DeclareLaunchArgument('lidar_format', default_value='ANY'), + DeclareLaunchArgument('scan_rate', default_value='0'), + DeclareLaunchArgument('echo_mode', default_value='single channel'), + DeclareLaunchArgument('point_cloud_qos', default_value='default'), + # Network device settings: default enumerate_net_device is set to true, which will automatically enumerate network devices + # If you do not want to automatically enumerate network devices, + # you can set enumerate_net_device to true, net_device_ip to the device's IP address, and net_device_port to the default value of 8090 + DeclareLaunchArgument('enumerate_net_device', default_value='true'), + DeclareLaunchArgument('net_device_ip', default_value=''), + DeclareLaunchArgument('net_device_port', default_value='0'), + DeclareLaunchArgument('log_level', default_value='none'), + DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('config_file_path', default_value=''), + DeclareLaunchArgument('enable_heartbeat', default_value='false'), + ] + + def get_params(context, args): + return [load_parameters(context, args)] + + def create_node_action(context, args): + params = get_params(context, args) + ros_distro = os.environ.get("ROS_DISTRO", "humble") + if ros_distro == "foxy": + return [ + Node( + package="orbbec_camera", + executable="orbbec_camera_node", + name="ob_camera_node", + namespace=LaunchConfiguration("camera_name"), + parameters=params, + output="screen", + ) + ] + 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", + ) + ]) + ] + + return LaunchDescription( + args + [ + OpaqueFunction(function=lambda context: create_node_action(context, args)) + ] + ) diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index b4465f66..fb4d525c 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -127,6 +127,7 @@ void OBCameraNodeDriver::init() { auto log_level_str = declare_parameter("log_level", "none"); auto log_level = obLogSeverityFromString(log_level_str); + device_type_ = declare_parameter("device_type", "camera"); connection_delay_ = static_cast(declare_parameter("connection_delay", 100)); enable_sync_host_time_ = declare_parameter("enable_sync_host_time", true); upgrade_firmware_ = declare_parameter("upgrade_firmware", ""); @@ -445,8 +446,14 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev while (retry_count < max_retries && !initialized) { try { - ob_camera_node_ = std::make_unique(this, device_, parameters_, + if (device_type_ == "camera") { + ob_camera_node_ = std::make_unique(this, device_, parameters_, + node_options_.use_intra_process_comms()); + } else if (device_type_ == "lidar") { + ob_lidar_node_ = std::make_unique(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 " @@ -476,16 +483,17 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev CHECK_NOTNULL(device_info_.get()); device_unique_id_ = device_info_->getUid(); - if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid())) { - TRY_EXECUTE_BLOCK(device_->timerSyncWithHost()); - if (g_time_domain != "global") { - sync_host_time_timer_ = this->create_wall_timer(std::chrono::milliseconds(60000), [this]() { - if (device_) { - TRY_EXECUTE_BLOCK(device_->timerSyncWithHost()); - } - }); - } - } + // if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid())) { + // TRY_EXECUTE_BLOCK(device_->timerSyncWithHost()); + // if (g_time_domain != "global") { + // sync_host_time_timer_ = this->create_wall_timer(std::chrono::milliseconds(60000), + // [this]() { + // if (device_) { + // TRY_EXECUTE_BLOCK(device_->timerSyncWithHost()); + // } + // }); + // } + // } RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->getName() << " connected"); RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->getSerialNumber()); @@ -504,12 +512,12 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev std::placeholders::_2, std::placeholders::_3), false); } - if (ob_camera_node_) { - ob_camera_node_->startIMU(); - ob_camera_node_->startStreams(); - } else { - RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr"); - } + // if (ob_camera_node_) { + // ob_camera_node_->startIMU(); + // ob_camera_node_->startStreams(); + // } else { + // RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr"); + // } } // namespace orbbec_camera diff --git a/orbbec_camera/src/ob_lidar_node.cpp b/orbbec_camera/src/ob_lidar_node.cpp new file mode 100644 index 00000000..4f8537e8 --- /dev/null +++ b/orbbec_camera/src/ob_lidar_node.cpp @@ -0,0 +1,334 @@ +/******************************************************************************* + * Copyright (c) 2023 Orbbec 3D Technology, Inc + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *******************************************************************************/ + +#include "orbbec_camera/ob_lidar_node.h" +#include +#include +#include + +#include "orbbec_camera/utils.h" +#include +#include +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "libobsensor/hpp/Utils.hpp" + +#if defined(USE_RK_HW_DECODER) +#include "orbbec_camera/rk_mpp_decoder.h" +#elif defined(USE_NV_HW_DECODER) +#include "orbbec_camera/jetson_nv_decoder.h" +#endif + +namespace orbbec_camera { +using namespace std::chrono_literals; + +OBLidarNode::OBLidarNode(rclcpp::Node *node, std::shared_ptr device, + std::shared_ptr parameters, bool use_intra_process) + : node_(node), + device_(std::move(device)), + parameters_(std::move(parameters)), + logger_(node->get_logger()), + use_intra_process_(use_intra_process) { + RCLCPP_INFO_STREAM(logger_, + "OBLidarNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF")); + is_running_.store(true); + stream_name_[LIDAR] = "lidar"; + setupTopics(); +#if defined(USE_RK_HW_DECODER) + jpeg_decoder_ = std::make_unique(width_[COLOR], height_[COLOR]); +#elif defined(USE_NV_HW_DECODER) + jpeg_decoder_ = std::make_unique(width_[COLOR], height_[COLOR]); +#endif + is_camera_node_initialized_ = true; +} + +template +void OBLidarNode::setAndGetNodeParameter( + T ¶m, const std::string ¶m_name, const T &default_value, + const rcl_interfaces::msg::ParameterDescriptor ¶meter_descriptor) { + try { + param = parameters_ + ->setParam(param_name, rclcpp::ParameterValue(default_value), + std::function(), parameter_descriptor) + .get(); + } catch (const rclcpp::ParameterTypeException &ex) { + RCLCPP_ERROR_STREAM(logger_, "Failed to set parameter: " << param_name << ". " << ex.what()); + throw; + } +} + +OBLidarNode::~OBLidarNode() noexcept { clean(); } + +void OBLidarNode::rebootDevice() { + RCLCPP_WARN_STREAM(logger_, "Reboot device"); + // clean(); + if (device_) { + device_->reboot(); + RCLCPP_WARN_STREAM(logger_, "Reboot device DONE"); + } +} + +void OBLidarNode::clean() noexcept { + std::lock_guard lock(device_lock_); + RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBLidarNode"); + 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(); + } + + RCLCPP_WARN_STREAM(logger_, "stop streams"); + // stopStreams(); + // stopIMU(); + delete[] rgb_buffer_; + rgb_buffer_ = nullptr; + + delete[] rgb_point_cloud_buffer_; + rgb_point_cloud_buffer_ = nullptr; + + delete[] xy_table_data_; + xy_table_data_ = nullptr; + + delete[] depth_xy_table_data_; + depth_xy_table_data_ = nullptr; + + delete[] depth_point_cloud_buffer_; + depth_point_cloud_buffer_ = nullptr; + + RCLCPP_WARN_STREAM(logger_, "Destroy ~OBLidarNode DONE"); +} + +void OBLidarNode::setupTopics() { + try { + getParameters(); + setupDevices(); + // setupProfiles(); + // selectBaseStream(); + setupPublishers(); + } 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"); + } +} + +void OBLidarNode::getParameters() { + setAndGetNodeParameter(camera_name_, "camera_name", "camera"); + camera_link_frame_id_ = camera_name_ + "_link"; + for (auto stream_index : IMAGE_STREAMS) { + std::string param_name; + param_name = stream_name_[stream_index] + "_format"; + setAndGetNodeParameter(format_str_[stream_index], param_name, format_str_[stream_index]); + format_[stream_index] = OBFormatFromString(format_str_[stream_index]); + RCLCPP_INFO_STREAM(logger_, "lidar format str: " << format_str_[stream_index]); + RCLCPP_INFO_STREAM(logger_, "lidar format: " << format_[stream_index]); + } + setAndGetNodeParameter(publish_tf_, "publish_tf", true); + setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 0.0); + setAndGetNodeParameter(time_domain_, "time_domain", "global"); + setAndGetNodeParameter(enable_heartbeat_, "enable_heartbeat", false); + setAndGetNodeParameter(echo_mode_, "echo_mode", "single channel"); + setAndGetNodeParameter(point_cloud_qos_, "point_cloud_qos", "default"); +} + +void OBLidarNode::setupDevices() { + 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 (device_->isPropertySupported(OB_PROP_LIDAR_ECHO_MODE_INT, OB_PERMISSION_READ_WRITE)) { + if (echo_mode_ == "single channel") { + TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_ECHO_MODE_INT, 0); + } else if (echo_mode_ == "dual channel") { + TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_ECHO_MODE_INT, 1); + } + RCLCPP_INFO_STREAM( + logger_, "Setting echo mode to " + << (device_->getIntProperty(OB_PROP_LIDAR_ECHO_MODE_INT) ? "single channel" + : "dual channel")); + } +} + +// void OBLidarNode::setupProfiles() { +// // Image stream +// for (const auto &elem : IMAGE_STREAMS) { +// if (enable_stream_[elem]) { +// const auto &sensor = sensors_[elem]; +// CHECK_NOTNULL(sensor.get()); +// auto profiles = sensor->getStreamProfileList(); +// CHECK_NOTNULL(profiles.get()); +// CHECK(profiles->getCount() > 0); +// for (size_t i = 0; i < profiles->getCount(); i++) { +// auto base_profile = profiles->getProfile(i)->as(); +// if (base_profile == nullptr) { +// throw std::runtime_error("Failed to get profile " + std::to_string(i)); +// } +// auto profile = base_profile->as(); +// 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()); +// supported_profiles_[elem].emplace_back(profile); +// } +// std::shared_ptr selected_profile; +// std::shared_ptr default_profile; +// try { +// if (width_[elem] == 0 && height_[elem] == 0 && fps_[elem] == 0 && +// format_[elem] == OB_FORMAT_UNKNOWN) { +// selected_profile = profiles->getProfile(0)->as(); +// } else { +// selected_profile = profiles->getVideoStreamProfile(width_[elem], height_[elem], +// format_[elem], fps_[elem]); +// } + +// } catch (const ob::Error &ex) { +// RCLCPP_ERROR_STREAM( +// logger_, "Failed to get " << stream_name_[elem] << " profile: " << ex.getMessage()); +// RCLCPP_ERROR_STREAM( +// logger_, "Stream: " << magic_enum::enum_name(elem.first) +// << ", Stream Index: " << elem.second << ", Width: " << +// width_[elem] +// << ", Height: " << height_[elem] << ", FPS: " << fps_[elem] +// << ", Format: " << magic_enum::enum_name(format_[elem])); +// RCLCPP_ERROR(logger_, +// "Error: The device might be connected via USB 2.0. Please verify your " +// "configuration and try again. The current process will now exit."); +// RCLCPP_INFO_STREAM(logger_, "Available profiles:"); +// printSensorProfiles(sensor); +// RCLCPP_ERROR(logger_, "Because can not set this stream, so exit."); +// exit(-1); +// } + +// if (!selected_profile) { +// RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! " +// << " Stream: " << magic_enum::enum_name(elem.first) +// << ", Stream Index: " << elem.second +// << ", Width: " << width_[elem] +// << ", Height: " << height_[elem] << ", FPS: " << +// fps_[elem] +// << ", Format: " << magic_enum::enum_name(format_[elem])); +// if (default_profile) { +// RCLCPP_WARN_STREAM(logger_, "Using default profile instead."); +// RCLCPP_WARN_STREAM(logger_, "default FPS " << default_profile->getFps()); +// selected_profile = default_profile; +// } else { +// RCLCPP_ERROR_STREAM( +// logger_, " NO default_profile found , Stream: " << +// magic_enum::enum_name(elem.first) +// << " will be disable"); +// enable_stream_[elem] = false; +// continue; +// } +// } +// CHECK_NOTNULL(selected_profile); +// stream_profile_[elem] = selected_profile; +// height_[elem] = static_cast(selected_profile->getHeight()); +// width_[elem] = static_cast(selected_profile->getWidth()); +// fps_[elem] = static_cast(selected_profile->getFps()); +// format_[elem] = selected_profile->getFormat(); +// updateImageConfig(elem); +// if (selected_profile->format() == OB_FORMAT_BGRA) { +// images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0)); +// encoding_[elem] = sensor_msgs::image_encodings::BGRA8; +// unit_step_size_[COLOR] = 4 * sizeof(uint8_t); +// } else if (selected_profile->format() == OB_FORMAT_RGBA) { +// images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0)); +// encoding_[elem] = sensor_msgs::image_encodings::RGBA8; +// unit_step_size_[COLOR] = 4 * sizeof(uint8_t); +// } else { +// 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())); +// } +// } +// // IMU +// for (const auto &stream_index : HID_STREAMS) { +// if (!enable_stream_[stream_index]) { +// continue; +// } +// try { +// auto profile_list = sensors_[stream_index]->getStreamProfileList(); +// if (stream_index == ACCEL) { +// auto full_scale_range = fullAccelScaleRangeFromString(imu_range_[stream_index]); +// auto sample_rate = sampleRateFromString(imu_rate_[stream_index]); +// auto profile = profile_list->getAccelStreamProfile(full_scale_range, sample_rate); +// stream_profile_[stream_index] = profile; +// } else if (stream_index == GYRO) { +// auto full_scale_range = fullGyroScaleRangeFromString(imu_range_[stream_index]); +// auto sample_rate = sampleRateFromString(imu_rate_[stream_index]); +// auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate); +// stream_profile_[stream_index] = profile; +// } +// RCLCPP_INFO_STREAM(logger_, "stream " << stream_name_[stream_index] << " full scale range " +// << imu_range_[stream_index] << " sample rate " +// << imu_rate_[stream_index]); +// } catch (const ob::Error &e) { +// RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index] +// << " profile: " << e.getMessage()); +// enable_stream_[stream_index] = false; +// stream_profile_[stream_index] = nullptr; +// } +// } +// } + +// void OBLidarNode::selectBaseStream() { +// if (enable_stream_[DEPTH]) { +// base_stream_ = DEPTH; +// } else if (enable_stream_[INFRA0]) { +// base_stream_ = INFRA0; +// } else if (enable_stream_[INFRA1]) { +// base_stream_ = INFRA1; +// } else if (enable_stream_[INFRA2]) { +// base_stream_ = INFRA2; +// } else if (enable_stream_[COLOR]) { +// base_stream_ = COLOR; +// } +// } +void OBLidarNode::setupPublishers() { + using PointCloud2 = sensor_msgs::msg::PointCloud2; + auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_); + if (use_intra_process_) { + point_cloud_qos_profile = rmw_qos_profile_default; + } + cloud_pub_ = node_->create_publisher( + "cloud/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile), + point_cloud_qos_profile)); +} + +} // namespace orbbec_camera diff --git a/orbbec_camera/src/utils.cpp b/orbbec_camera/src/utils.cpp index e1928b44..f67ab8dc 100644 --- a/orbbec_camera/src/utils.cpp +++ b/orbbec_camera/src/utils.cpp @@ -272,6 +272,7 @@ OBFormat OBFormatFromString(const std::string &format) { std::string fixed_format; std::transform(format.begin(), format.end(), std::back_inserter(fixed_format), [](const auto ch) { return std::isalpha(ch) ? toupper(ch) : ch; }); + std::cout << "OBFormatFromString: " << fixed_format << std::endl; if (fixed_format == "MJPG") { return OB_FORMAT_MJPG; } else if (fixed_format == "MJPEG") { @@ -340,6 +341,14 @@ OBFormat OBFormatFromString(const std::string &format) { return OB_FORMAT_BYR2; } else if (fixed_format == "RW16") { return OB_FORMAT_RW16; + } else if (fixed_format == "LIDAR_POINT") { + return OB_FORMAT_LIDAR_POINT; + } else if (fixed_format == "LIDAR_SPHERE_POINT") { + return OB_FORMAT_LIDAR_SPHERE_POINT; + } else if (fixed_format == "LIDAR_SCAN") { + return OB_FORMAT_LIDAR_SCAN; + } else if (fixed_format == "LIDAR_CALIBRATION") { + return OB_FORMAT_LIDAR_CALIBRATION; } // else if (fixed_format == "DISP16") { // return OB_FORMAT_DISP16;