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

Update code structure

Browse Source
This commit is contained in:
jj
2025-06-06 16:30:49 +08:00
parent 5b80656a61
commit b1ee682007
9 changed files with 967 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
+1
View File
@@ -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
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+2 -1
View File
@@ -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<stream_index_pair> IMAGE_STREAMS = {COLOR, DEPTH, INFRA0, INFRA1, INFRA2};
const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR, DEPTH, INFRA0, INFRA1, INFRA2, LIDAR};
const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL};
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+3
View File
@@ -20,6 +20,7 @@
#include <rclcpp/rclcpp.hpp>
#include <semaphore.h>
#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<ob::Context> ctx_ = nullptr;
rclcpp::Logger logger_;
std::unique_ptr<OBCameraNode> ob_camera_node_ = nullptr;
std::unique_ptr<OBLidarNode> ob_lidar_node_ = nullptr;
std::shared_ptr<ob::Device> device_ = nullptr;
std::shared_ptr<ob::DeviceInfo> 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
orbbec_camera/include/orbbec_camera/ob_lidar_node.h
+472
View File
@@ -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 <nlohmann/json.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
#include <atomic>
#include "ob_camera_node.h"
#include <opencv2/opencv.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Transform.h>
#include <std_srvs/srv/set_bool.hpp>
#include <std_srvs/srv/empty.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_publisher/image_publisher.hpp>
#include <image_transport/publisher.hpp>
#include <sensor_msgs/msg/imu.hpp>
#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 <std_msgs/msg/string.hpp>
#include <fcntl.h>
#include <unistd.h>
#if defined(ROS_JAZZY) || defined(ROS_IRON)
#include <cv_bridge/cv_bridge.hpp>
#else
#include <cv_bridge/cv_bridge.h>
#endif
#define STREAM_NAME(sip) \
(static_cast<std::ostringstream&&>(std::ostringstream() \
<< _stream_name[sip.first] \
<< ((sip.second > 0) ? std::to_string(sip.second) : ""))) \
.str()
#define FRAME_ID(sip) \
(static_cast<std::ostringstream&&>(std::ostringstream() \
<< getNamespaceStr() << "_" << STREAM_NAME(sip) << "_frame")) \
.str()
#define OPTICAL_FRAME_ID(sip) \
(static_cast<std::ostringstream&&>( \
std::ostringstream() << getNamespaceStr() << "_" << STREAM_NAME(sip) << "_optical_frame")) \
.str()
#define ALIGNED_DEPTH_TO_FRAME_ID(sip) \
(static_cast<std::ostringstream&&>(std::ostringstream() \
<< getNamespaceStr() << "_aligned_depth_to_" \
<< STREAM_NAME(sip) << "_frame")) \
.str()
#define BASE_FRAME_ID() \
(static_cast<std::ostringstream&&>(std::ostringstream() << getNamespaceStr() << "_link")).str()
#define ODOM_FRAME_ID() \
(static_cast<std::ostringstream&&>(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<ob::Device> device,
std::shared_ptr<Parameters> parameters, bool use_intra_process = false);
template <class T>
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<ob::Device> device_ = nullptr;
std::shared_ptr<Parameters> parameters_ = nullptr;
rclcpp::Logger logger_;
std::atomic_bool is_running_{false};
std::unique_ptr<ob::Pipeline> pipeline_ = nullptr;
std::unique_ptr<ob::Pipeline> 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<ob::Config> pipeline_config_ = nullptr;
std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_;
std::map<stream_index_pair, ob_camera_intrinsic> stream_intrinsics_;
std::map<stream_index_pair, sensor_msgs::msg::CameraInfo> camera_infos_;
std::map<stream_index_pair, OBCameraParam> ob_camera_param_;
std::map<stream_index_pair, OBExtrinsic> depth_to_other_extrinsics_;
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::Extrinsics>::SharedPtr>
depth_to_other_extrinsics_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
metadata_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::IMUInfo>::SharedPtr>
imu_info_publishers_;
std::map<stream_index_pair, int> width_;
std::map<stream_index_pair, int> height_;
std::map<stream_index_pair, int> fps_;
std::map<stream_index_pair, std::string> frame_id_;
std::map<stream_index_pair, std::string> optical_frame_id_;
std::map<stream_index_pair, std::string> depth_aligned_frame_id_;
std::string camera_link_frame_id_;
bool depth_registration_ = false;
std::map<stream_index_pair, std::string> image_qos_;
std::map<stream_index_pair, std::string> camera_info_qos_;
std::map<stream_index_pair, ob_format> format_;
std::map<stream_index_pair, std::string> format_str_;
std::map<stream_index_pair, int> image_format_;
std::map<stream_index_pair, std::vector<std::shared_ptr<ob::VideoStreamProfile>>>
supported_profiles_;
std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_;
stream_index_pair base_stream_ = DEPTH;
std::map<stream_index_pair, uint32_t> seq_;
std::map<stream_index_pair, cv::Mat> images_;
std::map<stream_index_pair, std::string> encoding_;
std::map<stream_index_pair, int> unit_step_size_;
std::vector<int> compression_params_;
ob::FormatConvertFilter format_convert_filter_;
std::map<stream_index_pair, bool> enable_stream_;
std::map<stream_index_pair, bool> flip_stream_;
std::map<stream_index_pair, bool> mirror_stream_;
std::map<stream_index_pair, int> rotation_stream_;
std::map<stream_index_pair, std::string> stream_name_;
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_;
std::map<stream_index_pair, rclcpp::Service<GetInt32>::SharedPtr> get_exposure_srv_;
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_exposure_srv_;
std::map<stream_index_pair, rclcpp::Service<GetInt32>::SharedPtr> get_gain_srv_;
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_gain_srv_;
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> toggle_sensor_srv_;
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> set_mirror_srv_;
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> set_flip_srv_;
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_rotation_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_white_balance_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_white_balance_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_auto_white_balance_srv_;
rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
rclcpp::Service<SetString>::SharedPtr switch_ir_camera_srv_;
rclcpp::Service<SetString>::SharedPtr set_write_customerdata_srv_;
rclcpp::Service<SetString>::SharedPtr set_read_customerdata_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ir_long_exposure_srv_;
std::map<stream_index_pair, rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr>
set_auto_exposure_srv_;
std::map<stream_index_pair, rclcpp::Service<SetArrays>::SharedPtr> set_ae_roi_srv_;
rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ldp_status_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_floor_enable_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_lrm_measure_distance_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_reset_timestamp_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_interleaver_laser_sync_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_sync_host_time_srv_;
rclcpp::Service<SetFilter>::SharedPtr set_filter_srv_;
bool enable_sync_output_accel_gyro_ = false;
bool publish_tf_ = false;
bool tf_published_ = false;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_ = nullptr;
std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_ = nullptr;
std::vector<geometry_msgs::msg::TransformStamped> tf_msgs;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_registration_cloud_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::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<geometry_msgs::msg::TransformStamped> static_tf_msgs_;
std::shared_ptr<std::thread> tf_thread_ = nullptr;
std::condition_variable tf_cv_;
double tf_publish_rate_ = 10.0;
std::unique_ptr<camera_info_manager::CameraInfoManager> ir_info_manager_ = nullptr;
std::unique_ptr<camera_info_manager::CameraInfoManager> color_info_manager_ = nullptr;
std::string color_info_url_;
std::string ir_info_url_;
std::optional<OBCameraParam> camera_param_;
bool enable_d2c_viewer_ = false;
std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
std::map<stream_index_pair, std::atomic_bool> save_images_;
std::map<stream_index_pair, int> 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<std_srvs::srv::Empty>::SharedPtr save_images_srv_;
rclcpp::Service<std_srvs::srv::Empty>::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<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> imu_publishers_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_gyro_accel_publisher_;
bool imu_sync_output_start_ = false;
std::map<stream_index_pair, std::string> imu_rate_;
std::map<stream_index_pair, std::string> imu_range_;
std::map<stream_index_pair, std::string> imu_qos_;
std::map<stream_index_pair, bool> 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<JPEGDecoder> jpeg_decoder_ = nullptr;
uint8_t* rgb_buffer_ = nullptr;
bool is_color_frame_decoded_ = false;
std::mutex device_lock_;
// For color
std::queue<std::shared_ptr<ob::FrameSet>> color_frame_queue_;
std::shared_ptr<std::thread> 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<std_msgs::msg::String>::SharedPtr filter_status_pub_;
nlohmann::json filter_status_;
std::string align_mode_ = "HW";
std::unique_ptr<diagnostic_updater::Updater> diagnostic_updater_ = nullptr;
double diagnostic_period_ = 1.0;
bool enable_laser_ = false;
std::unique_ptr<ob::Align> 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<OBCalibrationParam> calibration_param_;
std::optional<OBXYTables> 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<OBXYTables> 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<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>> depth_filter_list_;
std::vector<std::shared_ptr<ob::Filter>> color_filter_list_;
std::vector<std::shared_ptr<ob::Filter>> left_ir_filter_list_;
std::vector<std::shared_ptr<ob::Filter>> 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
orbbec_camera/launch/gemini_330_series.launch.py
+1
View File
@@ -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=''),
orbbec_camera/launch/lidar.launch.py
+120
View File
@@ -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))
]
)
orbbec_camera/src/ob_camera_node_driver.cpp
+25 -17
View File
@@ -127,6 +127,7 @@ void OBCameraNodeDriver::init() {
auto log_level_str = declare_parameter<std::string>("log_level", "none");
auto log_level = obLogSeverityFromString(log_level_str);
device_type_ = declare_parameter<std::string>("device_type", "camera");
connection_delay_ = static_cast<int>(declare_parameter<int>("connection_delay", 100));
enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
upgrade_firmware_ = declare_parameter<std::string>("upgrade_firmware", "");
@@ -445,8 +446,14 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
while (retry_count < max_retries && !initialized) {
try {
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
if (device_type_ == "camera") {
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
node_options_.use_intra_process_comms());
} else if (device_type_ == "lidar") {
ob_lidar_node_ = std::make_unique<OBLidarNode>(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<ob::Device> &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<ob::Device> &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
orbbec_camera/src/ob_lidar_node.cpp
+334
View File
@@ -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 <rclcpp/rclcpp.hpp>
#include <thread>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include "orbbec_camera/utils.h"
#include <filesystem>
#include <fstream>
#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<ob::Device> device,
std::shared_ptr<Parameters> 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<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
#elif defined(USE_NV_HW_DECODER)
jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
#endif
is_camera_node_initialized_ = true;
}
template <class T>
void OBLidarNode::setAndGetNodeParameter(
T &param, const std::string &param_name, const T &default_value,
const rcl_interfaces::msg::ParameterDescriptor &parameter_descriptor) {
try {
param = parameters_
->setParam(param_name, rclcpp::ParameterValue(default_value),
std::function<void(const rclcpp::Parameter &)>(), parameter_descriptor)
.get<T>();
} 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<decltype(device_lock_)> 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<std::string>(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<bool>(publish_tf_, "publish_tf", true);
setAndGetNodeParameter<double>(tf_publish_rate_, "tf_publish_rate", 0.0);
setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global");
setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
setAndGetNodeParameter<std::string>(echo_mode_, "echo_mode", "single channel");
setAndGetNodeParameter<std::string>(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<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());
// supported_profiles_[elem].emplace_back(profile);
// }
// std::shared_ptr<ob::VideoStreamProfile> selected_profile;
// std::shared_ptr<ob::VideoStreamProfile> default_profile;
// try {
// if (width_[elem] == 0 && height_[elem] == 0 && fps_[elem] == 0 &&
// format_[elem] == OB_FORMAT_UNKNOWN) {
// selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
// } 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<int>(selected_profile->getHeight());
// width_[elem] = static_cast<int>(selected_profile->getWidth());
// fps_[elem] = static_cast<int>(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<PointCloud2>(
"cloud/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
point_cloud_qos_profile));
}
} // namespace orbbec_camera
orbbec_camera/src/utils.cpp
+9
View File
@@ -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;