Big refactoring code

* Removal of unnecessary files
* Optimized multi-camera launch
* Explicitly list the parameters in the launch file

orbbec_camera/CMakeLists.txt

@@ -19,9 +19,9 @@ find_package(ament_index_cpp REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(cv_bridge REQUIRED)
+find_package(camera_info_manager REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(image_publisher REQUIRED)
-find_package(magic_enum REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(orbbec_camera_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -40,7 +40,7 @@ pkg_search_module(GLOG REQUIRED libglog)
 
 if (NOT GLOG_FOUND)
   message(FATAL_ERROR "glog is not found")
-endif()
+endif ()
 execute_process(COMMAND uname -m OUTPUT_VARIABLE MACHINES)
 execute_process(COMMAND getconf LONG_BIT OUTPUT_VARIABLE MACHINES_BIT)
 message(STATUS "ORRBEC Machine : ${MACHINES}")
@@ -71,6 +71,7 @@ set(dependencies
   ament_index_cpp
   builtin_interfaces
   cv_bridge
+  camera_info_manager
   Eigen3
   image_transport
   image_publisher
@@ -89,10 +90,7 @@ set(dependencies
 ament_target_dependencies(${PROJECT_NAME}
   ${dependencies}
   )
-rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "orbbec_camera::OBCameraNodeFactory"
-  EXECUTABLE orbbec_camera_node
-  )
+
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -108,11 +106,93 @@ target_link_libraries(${PROJECT_NAME}
   ${OpenCV_LIBS}
   Eigen3::Eigen
   ${GLOG_LIBRARIES}
-  magic_enum::magic_enum
   -lOrbbecSDK
   -L${ORBBEC_LIBS}
   )
 
+add_executable(${PROJECT_NAME}_node
+  src/main.cpp
+  )
+
+target_include_directories(${PROJECT_NAME}_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${ORBBEC_INCLUDE_DIR}
+  ${OpenCV_INCLUDED_DIRS}
+  ${GLOG_INCLUDED_DIRS}
+  )
+
+
+target_link_libraries(${PROJECT_NAME}_node
+  ${ORBBEC_SDK_LIBRARIES}
+  ${OpenCV_LIBS}
+  Eigen3::Eigen
+  ${GLOG_LIBRARIES}
+  -lOrbbecSDK
+  -L${ORBBEC_LIBS}
+  ${PROJECT_NAME}
+  )
+
+ament_target_dependencies(${PROJECT_NAME}_node
+  ${dependencies}
+  )
+add_executable(list_devices_node
+  src/list_devices_node.cpp
+  )
+
+target_include_directories(list_devices_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${ORBBEC_INCLUDE_DIR}
+  ${OpenCV_INCLUDED_DIRS}
+  ${GLOG_INCLUDED_DIRS}
+  )
+
+
+target_link_libraries(list_devices_node
+  ${ORBBEC_SDK_LIBRARIES}
+  ${OpenCV_LIBS}
+  Eigen3::Eigen
+  ${GLOG_LIBRARIES}
+  -lOrbbecSDK
+  -L${ORBBEC_LIBS}
+  ${PROJECT_NAME}
+  )
+
+ament_target_dependencies(list_devices_node
+  ${dependencies}
+  )
+
+add_executable(ob_cleanup_shm_node
+  src/ob_cleanup_shm.cpp
+  )
+
+target_include_directories(ob_cleanup_shm_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${ORBBEC_INCLUDE_DIR}
+  ${OpenCV_INCLUDED_DIRS}
+  ${GLOG_INCLUDED_DIRS}
+  )
+
+
+target_link_libraries(ob_cleanup_shm_node
+  ${ORBBEC_SDK_LIBRARIES}
+  ${OpenCV_LIBS}
+  Eigen3::Eigen
+  ${GLOG_LIBRARIES}
+  -lOrbbecSDK
+  -L${ORBBEC_LIBS}
+  ${PROJECT_NAME}
+  )
+
+ament_target_dependencies(ob_cleanup_shm_node
+  ${dependencies}
+  )
+
 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -134,11 +214,6 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
   )
 
-
-install(DIRECTORY
-  params
-  DESTINATION share/${PROJECT_NAME}/
-  )
 install(DIRECTORY
   ${ORBBEC_INCLUDE_DIR}
   DESTINATION include
@@ -151,6 +226,12 @@ install(DIRECTORY
   PATTERN "*.so.*"
   )
 
+
+install(TARGETS list_devices_node
+  ob_cleanup_shm_node
+  ${PROJECT_NAME}_node
+  DESTINATION lib/${PROJECT_NAME}/
+  )
 if (BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

orbbec_camera/SDK/include/libobsensor/h/Device.h

@@ -444,6 +444,49 @@ void ob_device_set_structured_data(ob_device *device, ob_property_id property_id
  */
 void ob_device_get_structured_data(ob_device *device, ob_property_id property_id, void *data, uint32_t *data_size, ob_error **error);
 
+/**
+ * \if English
+ * @brief Set property struct data.
+ *
+ * @param[in] device Device object
+ * @param[in] property_id Id of the property
+ * @param[in] data_bundle Target data to set
+ * @param[in] Set data callback
+ * @param[in] user_data User-defined data will be returned in the callback
+ * @param[out] error Log error messages
+ * \else
+ * @brief 设置结构体类型的设备属性
+
+ * @param[in] device 设备对象
+ * @param[in] property_id 要设置的属性id
+ * @param[in] data_bundle
+ 要设置的数据内容，注意data_bundle指针为OrbbecSDK外部构建，所以自行释放指针资源，禁止调用ob_delete_data_bundle释放资源，会造成不可预知的错误；
+ * @param[in] cb 设置进度回调
+ * @param[in] user_data 用户自定义数据，会在回调中返回
+ * @param[out] error 记录错误信息
+ * @return void 无返回值
+ * \endif
+ */
+void ob_device_set_structured_data_ext(ob_device *device, ob_property_id property_id, ob_data_bundle *data_bundle, ob_set_data_callback cb, void *user_data,
+                                       ob_error **error);
+
+/**
+ * \if English
+ * @brief Get property struct data.
+ * @param[in] device Device object
+ * @param[in] property_id Id of the property
+ * @param[out] error Log error messages
+ * @return ob_data_dundle, NOTE: ob_data_dundle must free by ob_delete_data_bundle() because it come from OrbbecSDK's API
+ * \else
+ * @brief 获取结构体类型的设备属性
+ * @param[in] device 设备对象
+ * @param[in] property_id 要获取的属性id
+ * @param[out] error 记录错误信息
+ * @return 返回ob_data_dundle 结构体对象指针，返回的必须调用ob_delete_data_bundle()函数释放资源，否则会造成内存泄漏
+ * \endif
+ */
+ob_data_bundle *ob_device_get_structured_data_ext(ob_device *device, ob_property_id property_id, ob_error **error);
+
 /**
  * \if English
  * @brief Set raw data type of device property
@@ -495,6 +538,44 @@ void ob_device_set_raw_data(ob_device *device, ob_property_id property_id, void
  */
 void ob_device_get_raw_data(ob_device *device, ob_property_id property_id, ob_get_data_callback cb, bool async, void *user_data, ob_error **error);
 
+/**
+ * \if English
+ * @brief Get the property protocol version
+ * 
+ * @param[in] device Device object
+ * @param[out] error Log error messages
+ * @return ob_protocol_version
+ * 
+ * \else
+ * 
+ * @brief 获取设备的控制命令协议版本
+ * 
+ * @param[in] device 设备对象
+ * @param[out] error 记录错误信息
+ * @return ob_protocol_version
+ * \endif
+ */
+ob_protocol_version ob_device_get_protocol_version(ob_device *device, ob_error **error);
+
+/**
+ * \if English
+ * @brief Get cmdVersion of property
+ *
+ * @param[in] device Device object
+ * @param[in] propertyId Property id
+ * @param[out] error Log error messages
+ * @return ob_cmd_version 
+ * \else
+ * @brief 获取控制命令的版本号
+ *
+ * @param[in] device 设备对象
+ * @param[in] propertyId 属性id
+ * @param[out] error 记录错误信息
+ * @return ob_cmd_version 
+ * \endif
+ */
+ob_cmd_version ob_device_get_cmd_version(ob_device *device, ob_property_id property_id, ob_error **error);
+
 /**
  * \if English
  * @brief Get supported device properties count
@@ -903,6 +984,72 @@ void ob_device_write_authorization_code(ob_device *device, const char *auth_code
  */
 ob_camera_param_list *ob_device_get_calibration_camera_param_list(ob_device *device, ob_error **error);
 
+/**
+ * \if English
+ * @brief Get current depth work mode
+ * @param[in] device Device object
+ * @param[out] error Log error messages
+ * @return ob_depth_work_mode Current depth work mode
+ * \else
+ * @brief 查询当前相机深度模式
+ * @param[in] device 设备对象
+ * @param[out] error 记录错误信息
+ * @return ob_depth_work_mode 当前深度工作模式
+ * \endif
+ */
+ob_depth_work_mode ob_device_get_current_depth_work_mode(ob_device *device, ob_error **error);
+
+/**
+ * \if English
+ * @brief Switch depth work mode by ob_depth_work_mode. Prefer invoke ob_device_switch_depth_work_mode_by_name to switch depth mode
+ *        when known the complete name of depth work mode.
+ * @param[in] device Device object
+ * @param[in] work_mode Depth work mode come from ob_depth_work_mode_list which return by ob_device_get_depth_work_mode_list
+ * @param[out] error Log error messages
+ * @return Return switch result. OB_STATUS_OK: success, other failed.
+ * \else
+ * @brief 切换相机深度模式（根据深度工作模式对象），如果知道设备支持的深度工作模式名称，那么推荐用ob_device_switch_depth_work_mode_by_name
+ * @param[in] device 设备对象
+ * @param[in] work_mode 深度工作模式，从深度工作模式列表ob_depth_work_mode_list获取的模式对象(ob_device_get_depth_work_mode_list返回的对象)
+ * @param[out] error 记录错误信息
+ * @return ob_status 设置设置结果，OB_STATUS_OK成功，其他：设置失败
+ * \endif
+ */
+ob_status ob_device_switch_depth_work_mode(ob_device *device, const ob_depth_work_mode *work_mode, ob_error **error);
+
+/**
+ * \if English
+ * @brief Switch depth work mode by work mode name.
+ * @param[in] device Device object
+ * @param[in] mode name Depth work mode name which equals to ob_depth_work_mode.name
+ * @param[out] error Log error messages
+ * 
+ * @return Return switch result. OB_STATUS_OK: success, other failed.
+ * \else
+ * @brief 切换相机深度模式（根据深度工作模式名称）
+ * @param[in] device 设备对象
+ * @param[in] mode_name 深度工作模式名称，模式名称必须与ob_depth_work_mode.name一致
+ * @param[out] error 记录错误信息
+ * @return ob_status 设置设置结果，OB_STATUS_OK成功，其他：设置失败
+ * \endif
+ */
+ob_status ob_device_switch_depth_work_mode_by_name(ob_device *device, const char *mode_name, ob_error **error);
+
+/**
+ * \if English
+ * @brief Request support depth work mode list
+ * @param[in] device Device object
+ * @param[out] error Log error messages
+ * @return ob_depth_work_mode_list list of ob_depth_work_mode
+ * \else
+ * @brief 查询当前相机深度模式的列表
+ * @param[in] device 设备对象
+ * @param[out] error 记录错误信息
+ * @return ob_depth_work_mode_list 深度模式列表。注意：返回对象要用ob_delete_depth_work_mode_list()释放资源，否者会内存泄漏
+ * \endif
+ */
+ob_depth_work_mode_list *ob_device_get_depth_work_mode_list(ob_device *device, ob_error **error);
+
 /**
  * \if English
  * @brief Device reboot
@@ -922,6 +1069,28 @@ ob_camera_param_list *ob_device_get_calibration_camera_param_list(ob_device *dev
  */
 void ob_device_reboot(ob_device *device, ob_error **error);
 
+/**
+ * @brief 获取当前设备同步配置
+ * @brief 设备同步：包括单机内的不同 Sensor 的曝光同步功能 和 多机同步功能
+ *
+ * @param[in] device 设备对象
+ * @param[out] error 记录错误信息
+ * @return ob_device_sync_config 返回设备同步配置
+ */
+ob_device_sync_config ob_device_get_sync_config(ob_device *device, ob_error **error);
+
+/**
+ * @brief 设置设备同步配置
+ * @brief 用于配置 单机内的不同 Sensor 的曝光同步功能 和 多机同步功能
+ *
+ * @attention 调用本函数会直接将配置写入设备Flash，设备重启后依然会生效。为了避免影响Flash寿命，不要频繁更新配置。
+ *
+ * @param[in] device 设备对象
+ * @param[out] device_sync_config 设备同步配置
+ * @param[out] error 记录错误信息
+ */
+void ob_device_set_sync_config(ob_device *device, ob_device_sync_config device_sync_config, ob_error **error);
+
 /**
  * \if English
  * @brief Get device name
@@ -1171,6 +1340,68 @@ ob_camera_param ob_camera_param_list_get_param(ob_camera_param_list *param_list,
  */
 void ob_delete_camera_param_list(ob_camera_param_list *param_list, ob_error **error);
 
+/**
+ * \if English
+ * @brief Get the depth work mode count that ob_depth_work_mode_list hold
+ * @param[in] work_mode_list data struct contain list of ob_depth_work_mode
+ * @param[out] error Log error messages
+ * @return The total number contain in ob_depth_work_mode_list
+ * \else
+ * @brief 获取深度工作模式列表的元素数量
+ * @param[in] work_mode_list 工作模式列表对象
+ * @param[in] index 下标，从0开始
+ * @return work_mode_list包含的深度模式总数
+ * \endif
+ *
+ */
+uint32_t ob_depth_work_mode_list_count(ob_depth_work_mode_list *work_mode_list, ob_error **error);
+
+/**
+ * \if English
+ * @brief Get the index target of ob_depth_work_mode from work_mode_list
+ * @param[in] work_mode_list data struct contain list of ob_depth_work_mode
+ * @param[in] index index of target ob_depth_work_mode
+ * @param[out] error Log error messages
+ * @return ob_depth_work_mode
+ * \else
+ * @brief 从深度工作模式列表获取第N个对象
+ * @param[in] work_mode_list 工作模式列表对象
+ * @param[in] index 下标，从0开始
+ * @param[out] error 记录错误信息
+ * @return 深度工作模式对象
+ * \endif
+ *
+ */
+ob_depth_work_mode ob_depth_work_mode_list_get_item(ob_depth_work_mode_list *work_mode_list, uint32_t index, ob_error **error);
+
+/**
+ * \if English
+ * @brief Free resource of ob_depth_work_mode_list
+ * @param[in] work_mode_list data struct contain list of ob_depth_work_mode
+ * @param[out] error Log error messages
+ * \else
+ * @brief 删除深度工作模式列表对象，释放资源
+ * @param[in] work_mode_list 工作模式列表对象
+ * @param[out] error 记录错误信息
+ * \endif
+ *
+ */
+void ob_delete_depth_work_mode_list(ob_depth_work_mode_list *work_mode_list, ob_error **error);
+
+/**
+ * \if English
+ * @brief Free resource of data_bundle which come from OrbbecSDK's API
+ * @param data_bundle Data bundle
+ * @param[out] error Log error messages
+ * \else
+ * @brief 删除ob_data_bundle对象，释放资源；
+ * 注意：只能释放OrbbecSDK库返回的ob_data_bundle对象，非OrbbecSDK构建的对象不能调用该接口，否则会造成意想不到的错误
+ * @param[in] data_bundle 数据集
+ * @param[out] error 记录错误信息
+ * \endif
+ */
+void ob_delete_data_bundle(ob_data_bundle *data_bundle, ob_error **error);
+
 #ifdef __cplusplus
 }
 #endif

orbbec_camera/SDK/include/libobsensor/h/Filter.h

@@ -84,12 +84,18 @@ void ob_pointcloud_filter_set_frame_align_state(ob_filter *filter, bool state, o
 
 /**
  * \if English
- * @brief  Set point cloud position data scale ratio
- * @param[in] filter pointcloud_filter object
- * @param[in] scale scale ratio
+ * @brief  Set the point cloud data scaling factor
+ * @attention Calling this function to set the scale will change the point coordinate scaling factor of the output point cloud frame: posScale = posScale /
+ * scale.The point coordinate scaling factor for the output point cloud frame can be obtained via @ref ob_points_frame_get_position_value_scale function
+ * @param[in] filter object
+ * @param[in] scale Set the point cloud coordinate data zoom factor
  * @param[out] error Log error messages
  * \else
  * @brief  设置点云数据缩放比例
+ *
+ * @attention 调用该函数设置缩放比例会改变输出点云帧的点坐标缩放系数：posScale = posScale / scale;
+ *  输出点云帧的点坐标缩放系数可通过 @ref ob_points_frame_get_position_value_scale 函数获取
+ *
  * @param[in] filter pointcloud_filter对象
  * @param[in] scale 设置点云坐标数据缩放比例
  * @param[out] error 记录错误信息
@@ -100,8 +106,8 @@ void ob_pointcloud_filter_set_position_data_scale(ob_filter *filter, float scale
 /**
  * \if English
  * @brief  Set point cloud color data normalization
- * @param[in] filter pointcloud_filter object
- * @param[in] state Whether to normalize
+ * @param[in] filter object
+ * @param[in] state Sets whether the point cloud color data is normalized
  * @param[out] error Log error messages
  * \else
  * @brief  设置点云颜色数据归一化
@@ -146,6 +152,60 @@ ob_filter *ob_create_format_convert_filter(ob_error **error);
  */
 void ob_format_convert_filter_set_format(ob_filter *filter, ob_convert_format type, ob_error **error);
 
+/**
+ * \if English
+ * @brief Create compression Filter
+ *
+ * @param[out] error Log error messages
+ *
+ * @return filter depth_filter object
+ * \else
+ * @brief 创建compression Filter
+ *
+ * @param[out] error 记录错误信息
+ *
+ * @return filter depth_filter 对象
+ * \endif
+ */
+ob_filter *ob_create_compression_filter(ob_error **error);
+
+/**
+ * \if English
+ * @brief Set compression parameters
+ *
+ * @param[in] filter compression_filter object
+ * @param[in] mode Compression mode OB_COMPRESSION_LOSSLESS or OB_COMPRESSION_LOSSY
+ * @param[in] params Compression params, struct ob_compression_params, when mode is OB_COMPRESSION_LOSSLESS, params is NULL
+ * @param[out] error Log error messages
+ * \else
+ * @brief 设置压缩类型
+ *
+ * @param[in] filter compression_filter对象
+ * @param[in] mode 压缩模式 OB_COMPRESSION_LOSSLESS or OB_COMPRESSION_LOSSY
+ * @param[in] params 压缩参数，结构体 ob_compression_params，当mode为OB_COMPRESSION_LOSSLESS时，params为NULL
+ * @param[out] error 记录错误信息
+ * \endif
+ */
+void ob_compression_filter_set_compression_params(ob_filter *filter, ob_compression_mode mode, void *params, ob_error **error);
+
+/**
+ * \if English
+ * @brief Create decompression Filter
+ *
+ * @param[out] error Log error messages
+ *
+ * @return filter decompression Filter object
+ * \else
+ * @brief 创建decompressionr Filter
+ *
+ * @param[out] error 记录错误信息
+ *
+ * @return filter decompression Filter 对象
+ * \endif
+ */
+
+ob_filter *ob_create_decompression_filter(ob_error **error);
+
 /**
  * \if English
  * @brief  Filter reset, cache clear, state reset. If the asynchronous interface is used, the processing thread will also be stopped and the pending cache
@@ -234,4 +294,4 @@ void ob_delete_filter(ob_filter *filter, ob_error **error);
 
 #ifdef __cplusplus
 }
-#endif
+#endif

orbbec_camera/SDK/include/libobsensor/h/Frame.h

@@ -241,27 +241,53 @@ uint32_t ob_video_frame_metadata_size(ob_frame *frame, ob_error **error);
  */
 uint8_t ob_video_frame_pixel_available_bit_size(ob_frame *frame, ob_error **error);
 
+/* @brief 查询IR frame的最原始数据来源（即使回放视频，其原始的数据源也是sensor）
+ * @param[in] frame 视频帧对象
+ * @param[out] error 记录错误信息
+ * @return uint8_t 返回
+ * \endif
+ */
+ob_sensor_type ob_ir_frame_get_source_sensor_type(ob_frame *frame, ob_error **ob_error);
+
 /**
  * \if English
- * @brief Get the value scale of the depth frame, the unit is mm/step,
- *      such as valueScale=0.1, and a certain coordinate pixel value is pixelValue=10000,
- *      then the depth value = pixelValue*valueScale = 10000*0.1=1000mm。
+ * @brief Get the value scale of the depth frame. The pixel value of depth frame is multiplied by the scale to give a depth value in millimeter.
+ *      such as valueScale=0.1, and a certain coordinate pixel value is pixelValue=10000, then the depth value = pixelValue*valueScale = 10000*0.1=1000mm。
  *
  * @param[in] frame Frame object
  * @param[out] error Log error messages
  * @return float value scale
  * \else
- * @brief 获取深度帧的值刻度，单位为 mm/step，
- *      如valueScale=0.1, 某坐标像素值为pixelValue=10000，
+ * @brief 获取深度帧的值缩放系数，深度像素值乘以缩放系数后，可以得到单位为毫米的深度值； 如valueScale=0.1, 某坐标像素值为pixelValue=10000，
  *     则表示深度值value = pixelValue*valueScale = 10000*0.1=1000mm。
  *
  * @param[in] frame 帧对象
  * @param[out] error 记录错误信息
- * @return float 值刻度
+ * @return float 缩放系数
  * \endif
  */
 float ob_depth_frame_get_value_scale(ob_frame *frame, ob_error **error);
 
+/**
+ * \if English
+ * @brief Get the point position value scale of the points frame. the point position value of points frame is multiplied by the scale to give a position value
+ * in millimeter. such as scale=0.1, The x-coordinate value of a point is x = 10000, which means that the actual x-coordinate value = x*scale = 10000*0.1 =
+ * 1000mm.
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return float position value scale
+ * \else
+ * @brief 获取点云帧的点坐标值缩放系数，点坐标值乘以缩放系数后，可以得到单位为毫米的坐标值； 如scale=0.1, 某个点的x坐标值为x=10000，
+ *     则表示实际x坐标value = x*scale = 10000*0.1=1000mm。
+ *
+ * @param[in] frame 点云帧对象
+ * @param[out] error 记录错误信息
+ * @return float 缩放系数
+ * \endif
+ */
+float ob_points_frame_get_position_value_scale(ob_frame *frame, ob_error **error);
+
 /**
  * \if English
  * @brief Delete frame
@@ -362,6 +388,11 @@ ob_frame *ob_frameset_ir_frame(ob_frame *frameset, ob_error **error);
  */
 ob_frame *ob_frameset_points_frame(ob_frame *frameset, ob_error **error);
 
+/**
+ * 获取指定类型的frame
+ */
+ob_frame *ob_frameset_get_frame(ob_frame *frameset, ob_frame_type frame_type, ob_error **error);
+
 /**
  * \if English
  * @brief Get accelerometer frame data

orbbec_camera/SDK/include/libobsensor/h/Pipeline.h

@@ -366,7 +366,7 @@ ob_rect ob_get_d2c_valid_area(ob_pipeline *pipeline, uint32_t distance, ob_error
  * @return ob_rect 返回在工作距离下D2C后有效的区域信息
  * \endif
  */
-ob_rect ob_get_d2c_range_valid_area(ob_pipeline *pipeline, uint32_t minimum_distance,uint32_t maximum_distance, ob_error **error);
+ob_rect ob_get_d2c_range_valid_area(ob_pipeline *pipeline, uint32_t minimum_distance, uint32_t maximum_distance, ob_error **error);
 
 /**
  * \if English
@@ -398,6 +398,7 @@ void ob_pipeline_start_record(ob_pipeline *pipeline, const char *file_name, ob_e
  * @param[out] error 记录错误信息
  * \endif
  */
+
 void ob_pipeline_stop_record(ob_pipeline *pipeline, ob_error **error);
 
 /**
@@ -511,13 +512,12 @@ void ob_config_disable_all_stream(ob_config *config, ob_error **error);
  */
 void ob_config_set_align_mode(ob_config *config, ob_align_mode mode, ob_error **error);
 
-
 /**
  * \if English
- * @brief Whether scale depth frame after D2C
+ * @brief Whether the depth needs to be scaled after setting D2C
  *
  * @param[in] config config object
- * @param[in] enable Scale or not
+ * @param[in] enable Whether scaling is required
  * @param[out] error Log error messages
  * \else
  * @brief 设置D2C后是否需要缩放深度
@@ -527,15 +527,17 @@ void ob_config_set_align_mode(ob_config *config, ob_align_mode mode, ob_error **
  * @param[out] error 记录错误信息
  * \endif
  */
-void ob_config_set_depth_scale_require(ob_config *config,bool enable,ob_error **error);
+void ob_config_set_depth_scale_require(ob_config *config, bool enable, ob_error **error);
 
 /**
  * \if English
- * @brief 
+ * @brief  Set the D2C target resolution, which is applicable to cases where the Color stream is not enabled using the OrbbecSDK and the depth needs to be D2C
+ * Note: When you use OrbbecSDK to enable the Color stream, you also use this interface to set the D2C target resolution. The configuration of the enabled Color
+ * stream is preferred for D2C
  *
  * @param[in] config config object
- * @param[in] d2c_target_width D2C target width
- * @param[in] d2c_target_height D2C target height
+ * @param[in] d2c_target_width The D2C target has a wide resolution
+ * @param[in] d2c_target_height The D2C targets has a high resolution
  * @param[out] error Log error messages
  * \else
  * @brief 设置D2C目标分辨率，适用于未使用OrbbecSDK开启Color流，且需要对深度进行D2C的情况
@@ -547,8 +549,8 @@ void ob_config_set_depth_scale_require(ob_config *config,bool enable,ob_error **
  * @param[out] error 记录错误信息
  * \endif
  */
-void ob_config_set_d2c_target_resolution(ob_config *config,uint32_t d2c_target_width,uint32_t d2c_target_height,ob_error **error);
+void ob_config_set_d2c_target_resolution(ob_config *config, uint32_t d2c_target_width, uint32_t d2c_target_height, ob_error **error);
 
 #ifdef __cplusplus
 }
-#endif
+#endif

orbbec_camera/SDK/include/libobsensor/h/Property.h

@@ -11,6 +11,9 @@
  * \endif
  */
 
+#ifdef OB_SENSOR_SDK_DEVELOPER
+#include "libobsensor/internal/InternalProperty.h"
+#else  // not define OB_SENSOR_SDK_DEVELOPER
 #ifndef _OB_PROPERTY_H_
 #define _OB_PROPERTY_H_
 
@@ -78,9 +81,26 @@ typedef enum {
         91, /**< \if English D2C preprocessing switch (such as RGB cropping), 0: off, 1: on \else  D2C前处理开关（如RGB裁剪），0:关闭，1:打开 \endif */
     OB_PROP_RGB_CUSTOM_CROP_BOOL = 94, /**< \if English Custom RGB cropping switch, 0 is off, 1 is on custom cropping, and the ROI cropping area is issued \else
                                           自定义RGB裁剪开关，0为关闭，1为开启自定义裁剪，下发ROI裁剪区域\endif */
-    OB_PROP_DEVICE_WORK_MODE_INT          = 95, /**< \if English Device operating mode (power consumption) \else  设备工作模式（功耗） \endif */
-    OB_PROP_DEVICE_COMMUNICATION_TYPE_INT = 97, /**< 设备通信方式 0: USB; 1: Ethernet(RTSP)*/
-    OB_PROP_SWITCH_IR_MODE_INT            = 98, /**< 切换IR模式，0为主动IR模式,1为被动IR模式*/
+
+    OB_PROP_DEVICE_WORK_MODE_INT                = 95, /**< \if English Device operating mode (power consumption) \else  设备工作模式（功耗） \endif */
+    OB_PROP_DEVICE_COMMUNICATION_TYPE_INT       = 97, /**< 设备通信方式 0: USB; 1: Ethernet(RTSP)*/
+    OB_PROP_SWITCH_IR_MODE_INT                  = 98, /**< 切换IR模式，0为主动IR模式,1为被动IR模式*/
+    OB_PROP_LASER_ENERGY_LEVEL_INT              = 99, /**< 激光能量层级 */
+    OB_PROP_TIMER_RESET_SIGNAL_BOOL             = 104, /**< 触发设备时间归零 */
+    OB_PROP_TIMER_RESET_TRIGGLE_OUT_ENABLE_BOOL = 105, /**< 向外发送时间归零信号开关, true:打开, false: 关闭; 默认为true */
+    OB_PROP_TIMER_RESET_DELAY_US_INT            = 106, /**< 设置硬件时间归零延迟时间, 单位: 微妙 */
+    OB_PROP_CAPTURE_IMAGE_SIGNAL_BOOL           = 107, /**< 软触发信号, 触发抓拍图片 */
+    OB_PROP_IR_RIGHT_MIRROR_BOOL                = 112, /**< 右IR的镜像 */
+    OB_PROP_CAPTURE_IMAGE_FRAME_NUMBER_INT      = 113, /**  单次软触发抓拍的帧数, 范围：[1, 255] */
+    OB_PROP_IR_RIGHT_FLIP_BOOL                  = 114, /**< 右IR的翻转, true：翻转,false：不翻转；默认为false */
+    OB_PROP_COLOR_ROTATE_INT                    = 115, /**< 彩色旋转, 翻转角度范围{0, 90, 180, 270}, 默认为0 */
+    OB_PROP_IR_ROTATE_INT                       = 116, /**< IR旋转, 翻转角度范围{0, 90, 180, 270}, 默认为0 */
+    OB_PROP_IR_RIGHT_ROTATE_INT                 = 117, /**< 右IR旋转, 翻转角度范围{0, 90, 180, 270}, 默认为0 */
+    OB_PROP_DEPTH_ROTATE_INT                    = 118, /**< 深度旋转, 翻转角度范围{0, 90, 180, 270}, 默认为0 */
+    OB_PROP_LASER_HW_ENERGY_LEVEL_INT =
+        119, /**< 查询激光硬件的实际能量层级, OB_PROP_LASER_ENERGY_LEVEL_INT（99）指令用于设置能级,该指令用于查询设置后硬件实际能级 */
+    OB_PROP_USB_POWER_STATE_INT = 121, /**< USB供电状态，状态值枚举: OBUSBPowerState */
+    OB_PROP_DC_POWER_STATE_INT  = 122, /**< DC供电状态,状态值枚举: OBDCPowerState */
 
     OB_STRUCT_BASELINE_CALIBRATION_PARAM     = 1002, /**< \if English Baseline calibration parameters \else  基线标定参数 \endif */
     OB_STRUCT_DEVICE_TEMPERATURE             = 1003, /**< \if English Device temperature information \else  设备温度信息 \endif */
@@ -89,8 +109,10 @@ typedef enum {
     OB_STRUCT_DEVICE_TIME                    = 1037, /**< \if English get/set device time \else 获取/设置设备时间 \endif */
     OB_STRUCT_MULTI_DEVICE_SYNC_CONFIG =
         1038, /**< \if English Multi-device synchronization mode and parameter configuration \else  多设备同步模式和参数配置 \endif */
-    OB_STRUCT_RGB_CROP_ROI          = 1040, /**< \if English RGB cropping ROI \else  RGB裁剪ROI \endif */
-    OB_STRUCT_DEVICE_IP_ADDR_CONFIG = 1041, /**< 设备ip地址配置 */
+    OB_STRUCT_RGB_CROP_ROI                 = 1040, /**< \if English RGB cropping ROI \else  RGB裁剪ROI \endif */
+    OB_STRUCT_DEVICE_IP_ADDR_CONFIG        = 1041, /**< 设备ip地址配置 */
+    OB_STRUCT_CURRENT_DEPTH_ALG_MODE       = 1043, /**< 当前的相机深度模式 */
+    OB_STRUCT_DEPTH_PRECISION_SUPPORT_LIST = 1045, /**< 深度精度等级列表，返回uin16_t数组，对应精度等级的枚举定义 */
 
     OB_PROP_COLOR_AUTO_EXPOSURE_BOOL         = 2000, /**< \if English Color camera auto exposure \else  彩色相机自动曝光 \endif */
     OB_PROP_COLOR_EXPOSURE_INT               = 2001, /**< \if English Color camera exposure adjustment \else  彩色相机曝光调节 \endif */
@@ -119,17 +141,21 @@ typedef enum {
                                        红外相机曝光调节（某些型号设备下会同步设置深度相机） \endif */
     OB_PROP_IR_GAIN_INT = 2027, /**< \if English Infrared camera gain adjustment (the depth camera will be set synchronously under some models of devices) \else
                                    红外相机增益调节（某些型号设备下会同步设置深度相机） \endif */
-    OB_PROP_IR_SWITCH_INT = 2028,
-    /**< 双目红外相机左右IR切换 */  // todo: id有冲突@千手修罗
+    OB_PROP_IR_CHANNEL_DATA_SOURCE_INT = 2028, /**< \if English Select Infrared camera data source channel. If not support throw exception. 0 : IR stream from IR Left sensor; 1 : IR stream from IR Left sensor; \else 读写IR通道的输出目标sensor,不支持时返回错误。0: 左侧IR  sensor,1: 右侧IR sensor; \endif */
+    OB_PROP_DEPTH_RM_FILTER_BOOL = 2029, /**< 深度效果去畸变, true：打开，false：关闭, 与D2C功能互斥。软硬件D2C开启时，不能使用mask功能 */
 
     OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL = 3004, /**< 视差转深度 */
-    OB_PROP_SDK_DEPTH_FRAME_UNPACK_BOOL = 3007, /**< Depth数据解包功能开关(每次开流都会默认打开，支持RLE/Y10/Y11/Y12/Y14格式) */
-    OB_PROP_SDK_IR_FRAME_UNPACK_BOOL    = 3008, /**< Ir数据解包功能开关(每次开流都会默认打开，支持RLE/Y10/Y11/Y12/Y14格式) */
-
-    OB_RAW_DATA_CAMERA_CALIB_JSON_FILE   = 4029, /**< 从设备端读取的标定Json文件(Femto Mega, read only)*/
-    OB_RAW_DATA_D2C_ROT_LUT_FILE         = 4030, /**< D2C初始化表文件(Femto Mega)*/
-    OB_STRUCT_DEBUG_SENSOR_EXPOSURE_TIME = 5504, /**< \if English LDP status \else  曝光时间读写 \endif */
-
+    OB_PROP_SDK_DEPTH_FRAME_UNPACK_BOOL =
+        3007, /**< \if English Depth data unpacking function switch (each open stream will be turned on by default, support RLE/Y10/Y11/Y12/Y14 format) \else
+                 Depth数据解包功能开关(每次开流都会默认打开，支持RLE/Y10/Y11/Y12/Y14格式) \endif */
+    OB_PROP_SDK_IR_FRAME_UNPACK_BOOL =
+        3008, /**< \if English IR data unpacking function switch (each current will be turned on by default, support RLE/Y10/Y11/Y12/Y14 format) \else
+                 Ir数据解包功能开关(每次开流都会默认打开，支持RLE/Y10/Y11/Y12/Y14格式) \endif */
+    OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL = 3009, /**< Accel数据转换功能开关(默认打开) */
+    OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL  = 3010, /**< Gyro数据转换功能开关(默认打开) */
+    OB_PROP_SDK_IR_LEFT_FRAME_UNPACK_BOOL    = 3011, /**< [左]Ir数据解包功能开关(每次开流都会默认打开，支持RLE/Y10/Y11/Y12/Y14格式) */
+    OB_PROP_SDK_IR_RIGHT_FRAME_UNPACK_BOOL   = 3012, /**< [右]Ir数据解包功能开关(每次开流都会默认打开，支持RLE/Y10/Y11/Y12/Y14格式) */
+    OB_RAW_DATA_CAMERA_CALIB_JSON_FILE              = 4029, /**< 从设备端读取的标定Json文件(Femto Mega, read only)*/
 } OBPropertyID,
     ob_property_id;
 
@@ -167,3 +193,4 @@ typedef struct OBPropertyItem {
 #endif
 
 #endif  // _OB_PROPERTY_H_
+#endif  // OB_SENSOR_SDK_DEVELOPER

orbbec_camera/SDK/include/libobsensor/hpp/Device.hpp

@@ -15,6 +15,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 struct DeviceImpl;
 struct DeviceInfoImpl;
@@ -27,6 +28,7 @@ class Context;
 class DeviceInfo;
 class Sensor;
 class CameraParamList;
+class OBDepthWorkModeList;
 
 class OB_EXTENSION_API Device {
 private:
@@ -407,6 +409,66 @@ public:
      */
     void getStructuredData(OBPropertyID propertyId, void *data, uint32_t *dataSize);
 
+    /**
+     * \if English
+     * @brief Get structured data type of device property
+     *
+     * @param propertyId Property id
+     * @param dataBundle Target data
+     * @param callback Callback of setting
+     * \else
+     * @brief Set the Structured Data Ext object
+     *
+     * @param propertyId 属性id
+     * @param dataBundle 设置的目标数据
+     * @param callback 设置数据回调
+     * \endif
+     */
+    void setStructuredDataExt(OBPropertyID propertyId, std::shared_ptr<OBDataBundle> dataBundle, SetDataCallback callback);
+
+    /**
+     * \if English
+     * @brief Get structured data type of device property
+     *
+     * @param propertyId Property id
+     * @return Data bundle
+     * \else
+     * @brief 获取structured data类型的数据
+     *
+     * @param propertyId 属性id
+     * @return std::shared_ptr<OBDataBundle> 数据集
+     * \endif
+     */
+    std::shared_ptr<OBDataBundle> getStructuredDataExt(OBPropertyID propertyId);
+
+    /**
+     * \if English
+     * @brief Get the property protocol version
+     * 
+     * @return OBProtocolVersion
+     * \else
+     * @brief 获取设备的控制命令协议版本
+     *
+     * @return OBProtocolVersion
+     * \endif
+     */
+    OBProtocolVersion getProtocolVersion();
+
+    /**
+     * \if English
+     * @brief Get cmdVersion of property
+     *
+     * @param propertyId Property id
+     * @return OBCmdVersion
+     * \else
+     * @brief 获取控制命令的版本号
+     *
+     * @param propertyId 属性id
+     * @return OBCmdVersion
+     * \endif
+     */
+    OBCmdVersion getCmdVersion(OBPropertyID propertyId);
+
     /**
      * \if English
      * @brief Get number of devices supported property
@@ -563,6 +625,61 @@ public:
      */
     std::shared_ptr<CameraParamList> getCalibrationCameraParamList();
 
+    /**
+     * \if English
+     * @brief Get current depth work mode
+     * 
+     * @return ob_depth_work_mode Current depth work mode
+     * \else
+     * @brief 查询当前的相机深度模式
+     *
+     * @return 返回当前的相机深度模式
+     * \endif
+     */
+    OBDepthWorkMode getCurrentDepthWorkMode();
+
+    /**
+     * \if English
+     * @brief Switch depth work mode by OBDepthWorkMode. Prefer invoke switchDepthWorkMode(const char *modeName) to switch depth mode
+     *        when known the complete name of depth work mode.
+     * @param[in] workMode Depth work mode come from ob_depth_work_mode_list which return by ob_device_get_depth_work_mode_list
+     * \else
+     * @brief 切换相机深度模式（根据深度工作模式对象），如果知道设备支持的深度工作模式名称，那么推荐用switchDepthWorkMode(const char *modeName)
+     *
+     * @param workMode 要切换的相机深度模式
+     *
+     * @return 返回指令是否切换成功
+     * \endif
+     */
+    OBStatus switchDepthWorkMode(const OBDepthWorkMode &workMode);
+
+    /**
+     * \if English
+     * @brief Switch depth work mode by work mode name.
+     * 
+     * @param[in] mode name Depth work mode name which equals to ob_depth_work_mode.name
+     * \else
+     * @brief 切换相机深度模式（根据深度工作模式名称）
+     *
+     * @param workMode 
+     *
+     * @return ob_status 设置设置结果，OB_STATUS_OK成功，其他：设置失败
+     * \endif
+     */
+    OBStatus switchDepthWorkMode(const char *modeName);
+
+    /**
+     * \if English
+     * @brief Request support depth work mode list
+     * @return OBDepthWorkModeList list of ob_depth_work_mode
+     * \else
+     * @brief 查询相机深度模式列表
+     *
+     * @return 相机深度模式列表
+     * \endif
+     */
+    std::shared_ptr<OBDepthWorkModeList> getDepthWorkModeList();
+
     /**
      * \if English
      * @brief Device restart
@@ -576,6 +693,24 @@ public:
      */
     void reboot();
 
+    /**
+     * @brief 获取当前设备同步配置
+     * @brief 设备同步：包括单机内的不同 Sensor 的曝光同步功能 和 多机同步功能
+     *
+     * @return OBDeviceSyncConfig 返回设备同步配置
+     */
+    OBDeviceSyncConfig getSyncConfig();
+
+    /**
+     * @brief 设置设备同步配置
+     * @brief 用于配置 单机内的不同 Sensor 的曝光同步功能 和 多机同步功能
+     *
+     * @attention 调用本函数会直接将配置写入设备Flash，设备重启后依然会生效。为了避免影响Flash寿命，不要频繁更新配置。
+     *
+     * @param[out] deviceSyncConfig 设备同步配置
+     */
+    void setSyncConfig(const OBDeviceSyncConfig &deviceSyncConfig);
+
     friend class Pipeline;
     friend class Recorder;
 };
@@ -934,4 +1069,79 @@ public:
     OBCameraParam getCameraParam(uint32_t index);
 };
 
+/**
+ * \if English
+ * @brief Type contain list of OBDepthWorkMode
+ * \else
+ * @brief 相机深度模式列表
+ * \endif
+ *
+ */
+class OB_EXTENSION_API OBDepthWorkModeList {
+private:
+    std::unique_ptr<OBDepthWorkModeListImpl> impl_;
+
+public:
+    OBDepthWorkModeList(std::unique_ptr<OBDepthWorkModeListImpl> impl_);
+    ~OBDepthWorkModeList();
+    /**
+     * \if English
+     * @brief Get the count of OBDepthWorkMode
+     * 
+     * @return Count of OBDepthWorkMode
+     * \else
+     * @brief 获取相机深度模式的数量
+     *
+     * @return 列表中的相机深度模式数量
+     * \endif
+     */
+    uint32_t count();
+
+    /**
+     * \if English
+     * @brief Get OBDepthWorkMode at index in OBDepthWorkModeList
+     * 
+     * @param[in] index Target OBDepthWorkMode's index
+     * 
+     * @return OBDepthWorkMode at index
+     * \else
+     * @brief 根据下标获取相机深度模式
+     *
+     * @param index 对应模式列表的下标
+     * @return 相机深度模式
+     * \endif
+     */
+    OBDepthWorkMode getOBDepthWorkMode(uint32_t index);
+
+    /**
+     * \if English
+     * @brief Get depth work mode name at index
+     * 
+     * @return Depth work mode name 
+     * \else
+     * @brief 模式名称
+     *
+     * @param index 对应模式列表的下标
+     * @return 模式名称
+     * \endif
+     */
+    std::string getName(uint32_t index);
+
+    /**
+     * \if English
+     * @brief Get OBDepthWorkMode at index in OBDepthWorkModeList
+     * 
+     * @param[in] index Target OBDepthWorkMode's index
+     * 
+     * @return OBDepthWorkMode at index
+     * \else
+     * @brief 运算符重载，根据下标获取相机深度模式
+     *
+     * @param index 对应模式列表的下标
+     * @return 相机深度模式
+     * \endif
+     */
+    OBDepthWorkMode operator[](uint32_t index);
+};
+
 }  // namespace ob

orbbec_camera/SDK/include/libobsensor/hpp/Filter.hpp

@@ -123,28 +123,34 @@ public:
     void setFrameAlignState(bool state);
 
     /**
-	 * \if English
-	 * @brief  Set point cloud position data scale ratio
+     * \if English
+     * @brief  Set the point cloud coordinate data zoom factor
      *
-     * @param scale scale ratio
-	 * \else
+     * @attention Calling this function to set the scale will change the point coordinate scaling factor of the output point cloud frame: posScale = posScale /
+     * scale.The point coordinate scaling factor for the output point cloud frame can be obtained via @ref PointsFrame::getPositionValueScale function
+     *
+     * @param scale Zoom factor
+     * \else
      * @brief  设置点云坐标数据缩放比例
      *
+     * @attention 调用该函数设置缩放比例会改变输出点云帧的点坐标缩放系数：posScale = posScale / scale;
+     *  输出点云帧的点坐标缩放系数可通过 @ref PointsFrame::getPositionValueScale 函数获取
+     *
      * @param scale 缩放比例
-	 * \endif
+     * \endif
      */
     void setPositionDataScaled(float scale);
 
     /**
-	 * \if English
-	 * @brief  Set point cloud color data normalization
+     * \if English
+     * @brief  Set point cloud color data normalization
      *
-     * @param state Whether to normalize
-	 * \else
+     * @param state Whether normalization is required
+     * \else
      * @brief  设置点云颜色数据归一化
      *
      * @param state 是否需要归一化
-	 * \endif
+     * \endif
      */
     void setColorDataNormalization(bool state);
 };
@@ -165,4 +171,29 @@ public:
      */
     void setFormatConvertType(OBConvertFormat type);
 };
-}  // namespace ob
+
+class OB_EXTENSION_API CompressionFilter : public Filter {
+public:
+    CompressionFilter();
+    /**
+     * \if English
+     * @brief Set compression params
+     *
+     * @param mode Compression mode OB_COMPRESSION_LOSSLESS or OB_COMPRESSION_LOSSY
+     * @param params Compression params, when mode is OB_COMPRESSION_LOSSLESS, params is NULL
+     * \else
+     * @brief 设置压缩参数
+     *
+     * @param mode 压缩模式 OB_COMPRESSION_LOSSLESS or OB_COMPRESSION_LOSSY
+     * @param params 压缩参数, 当mode为OB_COMPRESSION_LOSSLESS时，params为NULL
+     * \endif
+     */
+    void setCompressionParams(OBCompressionMode mode, void *params);
+};
+
+class OB_EXTENSION_API DecompressionFilter : public Filter {
+public:
+    DecompressionFilter();
+};
+
+}  // namespace ob

orbbec_camera/SDK/include/libobsensor/hpp/Frame.hpp

@@ -13,6 +13,7 @@
 #include "Types.hpp"
 
 #include <memory>
+#include <iostream>
 #include <typeinfo>
 
 /**
@@ -26,7 +27,10 @@
  *       +--+------+---------+
  *      |          |         |
  *  ColorFrame DepthFrame IRFrame
- *
+ *                           |
+ *                     +-----+-----+
+ *                     |           |
+ *                IRLeftFrame IRRightFrame
  */
 
 struct FrameImpl;
@@ -280,17 +284,15 @@ public:
 
     /**
      * \if English
-     * @brief Get the value scale of the depth frame, the unit is mm/step,
-     *        such as valueScale=0.1, and a certain coordinate pixel value is pixelValue=10000,
-     *        then the depth value value = pixelValue*valueScale = 10000*0.1=1000mm.
+     * @brief Get the value scale of the depth frame. The pixel value of depth frame is multiplied by the scale to give a depth value in millimeter.
+     *      such as valueScale=0.1, and a certain coordinate pixel value is pixelValue=10000, then the depth value = pixelValue*valueScale = 10000*0.1=1000mm。
      *
-     * @return float
+     * @return float scale
      * \else
-     * @brief 获取深度帧的值刻度，单位为 mm/step，
-     *      如valueScale=0.1, 某坐标像素值为pixelValue=10000，
+     * @brief 获取深度帧的值缩放系数，深度像素值乘以缩放系数后，可以得到单位为毫米的深度值； 如valueScale=0.1, 某坐标像素值为pixelValue=10000，
      *     则表示深度值value = pixelValue*valueScale = 10000*0.1=1000mm。
      *
-     * @return float
+     * @return float 缩放系数
      * \endif
      */
     float getValueScale();
@@ -300,12 +302,33 @@ class OB_EXTENSION_API IRFrame : public VideoFrame {
 public:
     IRFrame(Frame &frame);
     virtual ~IRFrame() noexcept {};
+
+public:
+    OBSensorType getDataSource();
 };
 
 class OB_EXTENSION_API PointsFrame : public Frame {
 public:
     PointsFrame(Frame &frame);
     ~PointsFrame() noexcept {};
+
+    /**
+     * \if English
+     * @brief Get the point position value scale of the points frame. the point position value of points frame is multiplied by the scale to give a position
+     * value in millimeter. such as scale=0.1, The x-coordinate value of a point is x = 10000, which means that the actual x-coordinate value = x*scale =
+     * 10000*0.1 = 1000mm.
+     *
+     * @param[in] frame Frame object
+     * @param[out] error Log error messages
+     * @return float position value scale
+     * \else
+     * @brief 获取点云帧的点坐标值缩放系数，点坐标值乘以缩放系数后，可以得到单位为毫米的坐标值； 如scale=0.1, 某个点的x坐标值为x=10000，
+     *     则表示实际x坐标value = x*scale = 10000*0.1=1000mm。
+     *
+     * @return float 缩放系数
+     * \endif
+     */
+    float getPositionValueScale();
 };
 
 class OB_EXTENSION_API FrameSet : public Frame {
@@ -393,7 +416,7 @@ public:
      * @return std::shared_ptr<Frame> 返回相应类型的帧
      * \endif
      */
-    std::shared_ptr<Frame> getFrame(OBSensorType sensorType);
+    std::shared_ptr<Frame> getFrame(OBFrameType frameType);
 
     friend class Pipeline;
     friend class Filter;
@@ -523,6 +546,8 @@ public:
 
 template <typename T> bool Frame::is() {
     switch(this->type()) {
+    case OB_FRAME_IR_LEFT:   // follow
+    case OB_FRAME_IR_RIGHT:  // follow
     case OB_FRAME_IR:
         return (typeid(T) == typeid(IRFrame) || typeid(T) == typeid(VideoFrame));
     case OB_FRAME_DEPTH:
@@ -538,6 +563,7 @@ template <typename T> bool Frame::is() {
     case OB_FRAME_POINTS:
         return (typeid(T) == typeid(PointsFrame));
     default:
+        std::cout << "ob::Frame::is() not catch frame type: " << (int)this->type() << std::endl;
         break;
     }
     return false;

orbbec_camera/SDK/include/libobsensor/hpp/Pipeline.hpp

@@ -230,13 +230,13 @@ public:
     std::shared_ptr<StreamProfileList> getD2CDepthProfileList(std::shared_ptr<StreamProfile> colorProfile, OBAlignMode alignMode);
 
     /**
-	 * \if English
-	 * @brief Get valid area between minimum distance and maximum distance after D2C
+     * \if English
+     * @brief Get valid area between minimum distance and maximum distance after D2C
      *
      * @param minimumDistance minimum working distance
      * @param maximumDistance maximum working distance
      * @return OBRect returns the area information valid after D2C at the working distance
-	 * \else
+     * \else
      * @brief 获取D2C后给定工作范围的有效区域
      * 如果需要获取指定距离D2C后的ROI区域，将minimum_distance与maximum_distance设置成一样或者将maximum_distance设置成0
      *
@@ -245,7 +245,7 @@ public:
      * @return OBRect 返回在工作距离下D2C后有效的区域信息
      * \endif
      */
-    OBRect getD2CValidArea(uint32_t minimumDistance,uint32_t maximumDistance = 0);
+    OBRect getD2CValidArea(uint32_t minimumDistance, uint32_t maximumDistance = 0);
 
     /**
      * \if English
@@ -346,33 +346,34 @@ public:
     void setAlignMode(OBAlignMode mode);
 
     /**
-	 * \if English
-	 * @brief Whether scale depth frame after D2C
+     * \if English
+     * @brief Whether the depth needs to be scaled after setting D2C
      *
-     * @param enable Scale or not
-	 * \else
+     * @param enable Whether scaling is required
+     * \else
      * @brief 设置D2C后是否需要缩放深度
      *
      * @param enable 是否需要缩放
-	 * \endif
+     * \endif
      */
     void setDepthScaleRequire(bool enable);
 
     /**
-	 * \if English
-	 * @brief
-     *
-     * @param d2cTargetWidth  D2C target width
-     * @param d2cTargetHeight D2C target height
-	 * \else
+     * \if English
+     * @brief Set the D2C target resolution, which is applicable to cases where the Color stream is not enabled using the OrbbecSDK and the depth needs to be
+     * D2C Note: When you use OrbbecSDK to enable the Color stream, you also use this interface to set the D2C target resolution. The configuration of the
+     * enabled Color stream is preferred for D2C.
+     * @param d2cTargetWidth  The D2C target has a wide resolution
+     * @param d2cTargetHeight The D2C target has a high resolutio
+     * \else
      * @brief 设置D2C目标分辨率，适用于未使用OrbbecSDK开启Color流，且需要对深度进行D2C的情况
      * 注意:当使用OrbbecSDK开启Color流时，同时使用了此接口设置了D2C目标分辨率时。优先使用开启的Color流的配置进行D2C。
      *
      * @param d2cTargetWidth  D2C目标分辨率宽
      * @param d2cTargetHeight D2C目标分辨率高
-	 * \endif
+     * \endif
      */
-    void setD2CTargetResolution(uint32_t d2cTargetWidth,uint32_t d2cTargetHeight);
+    void setD2CTargetResolution(uint32_t d2cTargetWidth, uint32_t d2cTargetHeight);
 
     friend class Pipeline;
 };

orbbec_camera/SDK/include/libobsensor/hpp/RecordPlayback.hpp

@@ -1,5 +1,6 @@
 /**
  * \if English
+ *
  * @file RecordPlayback.hpp
  * @brief For recording and playback functions
  * \else

orbbec_camera/SDK/include/libobsensor/hpp/StreamProfile.hpp

@@ -11,6 +11,7 @@
 
 #include "Types.hpp"
 
+#include <iostream>
 #include <memory>
 
 struct StreamProfileImpl;
@@ -43,7 +44,7 @@ public:
      * @return OBFormat 返回流的格式
      * \endif
      */
-    OBFormat format();
+    OBFormat format() const;
     /**
      * \if English
      * @brief Get the type of stream
@@ -55,7 +56,7 @@ public:
      * @return OBStreamType 返回流的类型
      * \endif
      */
-    OBStreamType type();
+    OBStreamType type() const;
 
     /**
      * \if English
@@ -113,7 +114,7 @@ public:
      * @return uint32_t 返回流的帧率
      * \endif
      */
-    uint32_t fps();
+    uint32_t fps() const;
     /**
      * \if English
      * @brief Get stream width
@@ -125,7 +126,7 @@ public:
      * @return uint32_t 返回流的宽
      * \endif
      */
-    uint32_t width();
+    uint32_t width() const;
     /**
      * \if English
      * @brief Get stream height
@@ -137,7 +138,7 @@ public:
      * @return uint32_t 返回流的高
      * \endif
      */
-    uint32_t height();
+    uint32_t height() const;
 };
 
 class OB_EXTENSION_API AccelStreamProfile : public StreamProfile {
@@ -156,7 +157,7 @@ public:
      * @return OBAccelFullScaleRange  返回量程范围值
      * \endif
      */
-    OBAccelFullScaleRange fullScaleRange();
+    OBAccelFullScaleRange fullScaleRange() const;
 
     /**
      * \if English
@@ -169,7 +170,7 @@ public:
      * @return OBAccelFullScaleRange  返回采样频率
      * \endif
      */
-    OBAccelSampleRate sampleRate();
+    OBAccelSampleRate sampleRate() const;
 };
 
 class OB_EXTENSION_API GyroStreamProfile : public StreamProfile {
@@ -188,7 +189,7 @@ public:
      * @return OBAccelFullScaleRange  返回量程范围值
      * \endif
      */
-    OBGyroFullScaleRange fullScaleRange();
+    OBGyroFullScaleRange fullScaleRange() const;
 
     /**
      * \if English
@@ -201,13 +202,15 @@ public:
      * @return OBAccelFullScaleRange  返回采样频率
      * \endif
      */
-    OBGyroSampleRate sampleRate();
+    OBGyroSampleRate sampleRate() const;
 };
 
 template <typename T> bool StreamProfile::is() {
     switch(this->type()) {
     case OB_STREAM_VIDEO:
     case OB_STREAM_IR:
+    case OB_STREAM_IR_LEFT:
+    case OB_STREAM_IR_RIGHT:
     case OB_STREAM_COLOR:
     case OB_STREAM_DEPTH:
         return typeid(T) == typeid(VideoStreamProfile);
@@ -240,7 +243,7 @@ public:
      * @return uint32_t 返回StreamProfile的数量
      * \endif
      */
-    uint32_t count();
+    uint32_t count() const;
 
     /**
      * \if English
@@ -280,4 +283,4 @@ public:
     const std::shared_ptr<VideoStreamProfile> getVideoStreamProfile(int width = 0, int height = 0, OBFormat format = OB_FORMAT_UNKNOWN, int fps = 0);
 };
 
-}  // namespace ob
+}  // namespace ob

orbbec_camera/SDK/lib/x64/libOrbbecSDK.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:3b4db8103740dd48a22d9025115f7d3a43f147676945464a51ff03c688a18d11
-size 64682616
+oid sha256:8bd285bf5f9989a9431fd9806761c1bdcb7e898e2053864db8184f801178cf2f
+size 84978856

orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.4

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:3b4db8103740dd48a22d9025115f7d3a43f147676945464a51ff03c688a18d11
-size 64682616

orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.4.3

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:3b4db8103740dd48a22d9025115f7d3a43f147676945464a51ff03c688a18d11
-size 64682616

orbbec_camera/dependencies/.gitignore

@@ -1,3 +0,0 @@
-magic_enum-0.8.0/*
-SDK/*
-glog-0.6.0/*

orbbec_camera/include/orbbec_camera/constants.h

@@ -18,7 +18,7 @@
 #define THREAD_NUM 4
 
 #define OB_ROS_MAJOR_VERSION 1
-#define OB_ROS_MINOR_VERSION 0
+#define OB_ROS_MINOR_VERSION 1
 #define OB_ROS_PATCH_VERSION 4
 
 #ifndef STRINGIFY
@@ -48,7 +48,7 @@ const double DIAGNOSTICS_PERIOD = 0;  // Static transform
 
 const int IMAGE_WIDTH = 640;
 const int IMAGE_HEIGHT = 480;
-const double IMAGE_FPS = 30.0;
+const int IMAGE_FPS = 30;
 
 const std::string IMAGE_QOS = "SYSTEM_DEFAULT";
 const std::string DEFAULT_QOS = "DEFAULT";

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -14,7 +14,6 @@
 
 #include <glog/logging.h>
 #include <nlohmann/json.hpp>
-#include <magic_enum.hpp>
 
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
@@ -32,6 +31,7 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <std_srvs/srv/set_bool.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>
@@ -47,6 +47,7 @@
 
 #include "orbbec_camera/constants.h"
 #include "orbbec_camera/dynamic_params.h"
+#include "magic_enum/magic_enum.hpp"
 
 #define STREAM_NAME(sip)                                                                       \
   (static_cast<std::ostringstream&&>(std::ostringstream()                                      \
@@ -123,6 +124,8 @@ class OBCameraNode {
 
   void startPipeline();
 
+  void setupDefaultImageFormat();
+
   void setupPublishers();
 
   void publishStaticTF(const rclcpp::Time& t, const std::vector<float>& trans,
@@ -195,15 +198,16 @@ class OBCameraNode {
 
   bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
 
-  void publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
+  void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
 
-  void publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
+  void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
 
-  void publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
+  void publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
 
-  void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
+  void onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet>& frame_set);
 
-  void onNewFrameCallback(std::shared_ptr<ob::Frame> frame, const stream_index_pair& stream_index);
+  void onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
+                          const stream_index_pair& stream_index);
 
   bool setupFormatConvertType(OBFormat format);
 
@@ -221,19 +225,17 @@ class OBCameraNode {
   std::map<stream_index_pair, OBCameraParam> ob_camera_param_;
   std::map<stream_index_pair, int> width_;
   std::map<stream_index_pair, int> height_;
-  std::map<stream_index_pair, double> fps_;
+  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_align_ = false;
-  bool publish_rgb_point_cloud_;
-  std::string d2c_mode_;  // sw, hw, none
-  std::map<stream_index_pair, std::string> qos_;
-  std::map<stream_index_pair, std::string> info_qos_;
+  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<ob_stream_type, int> image_format_;
+  std::map<stream_index_pair, int> image_format_;
   std::map<stream_index_pair, std::vector<std::shared_ptr<ob::VideoStreamProfile>>>
       enabled_profiles_;
   std::map<stream_index_pair, uint32_t> seq_;
@@ -243,10 +245,8 @@ class OBCameraNode {
   std::vector<int> compression_params_;
   ob::FormatConvertFilter format_convert_filter_;
 
-  std::map<ob_frame_type, bool> is_first_frame_;
-
-  std::map<stream_index_pair, bool> enable_;
-  std::map<ob_stream_type, std::string> stream_name_;
+  std::map<stream_index_pair, bool> enable_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, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
       camera_info_publishers_;
@@ -271,21 +271,28 @@ class OBCameraNode {
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
 
   bool publish_tf_ = false;
-  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
-  std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_;
+  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 colored_point_cloud_publisher_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_publisher_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_point_cloud_publisher_;
-
+  bool enable_point_cloud_ = true;
+  bool enable_colored_point_cloud_ = false;
   ob::PointCloudFilter point_cloud_filter_;
   sensor_msgs::msg::PointCloud2 point_cloud_msg_;
 
   rclcpp::Publisher<Extrinsics>::SharedPtr extrinsics_publisher_;
+  bool enable_publish_extrinsic_ = false;
   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_;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h

@@ -33,13 +33,13 @@ class OBCameraNodeFactory : public rclcpp::Node {
 
   void startDevice(const std::shared_ptr<ob::DeviceList>& list);
 
-  void deviceConnectCallback(const std::shared_ptr<ob::DeviceList>& device_list);
+  void onDeviceConnected(const std::shared_ptr<ob::DeviceList>& device_list);
 
-  void deviceDisconnectCallback(const std::shared_ptr<ob::DeviceList>& device_list);
+  void onDeviceDisconnected(const std::shared_ptr<ob::DeviceList>& device_list);
 
-  static OBLogSeverity obLogSeverityFromString(const std::string& log_level);
+  static OBLogSeverity obLogSeverityFromString(const std::string_view& log_level);
 
-  void checkConnectTimer();
+  void checkConnectTimer() const;
 
   void queryDevice();
 
@@ -51,16 +51,11 @@ class OBCameraNodeFactory : public rclcpp::Node {
   std::shared_ptr<ob::DeviceInfo> device_info_ = nullptr;
   std::atomic_bool is_alive_{false};
   std::atomic_bool device_connected_{false};
-  std::string log_level_;
   std::string serial_number_;
-  std::shared_ptr<Parameters> parameters_;
+  std::shared_ptr<Parameters> parameters_ = nullptr;
   std::shared_ptr<std::thread> query_thread_ = nullptr;
   std::recursive_mutex device_lock_;
   size_t device_num_ = 1;
   rclcpp::TimerBase::SharedPtr check_connect_timer_ = nullptr;
 };
 }  // namespace orbbec_camera
-
-#include <rclcpp_components/register_node_macro.hpp>
-
-RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeFactory)

orbbec_camera/include/orbbec_camera/utils.h

@@ -13,7 +13,6 @@
 #pragma once
 #include <ostream>
 #include <glog/logging.h>
-#include <magic_enum.hpp>
 #include <Eigen/Dense>
 #include <tf2/LinearMath/Quaternion.h>
 #include <rclcpp/rclcpp.hpp>
@@ -22,6 +21,7 @@
 #include "sensor_msgs/distortion_models.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
 #include "orbbec_camera_msgs/msg/extrinsics.hpp"
+#include "magic_enum/magic_enum.hpp"
 
 namespace orbbec_camera {
 sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
@@ -46,4 +46,6 @@ OBFormat OBFormatFromString(const std::string& format);
 
 std::string ObDeviceTypeToString(const OBDeviceType& type);
 
+rmw_qos_profile_t getRMWQosProfileFromString(const std::string& str_qos);
+
 }  // namespace orbbec_camera

orbbec_camera/launch/astra.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="RGB"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/astra_adv.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y16"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y16"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/astra_embedded_s.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="400"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y16"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="400"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y16"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/astra_mini.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="RGB"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/astra_mini_pro.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/astra_plus.launch.py

@@ -1,21 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from ament_index_python import get_package_share_directory
-
-
-def generate_launch_description():
-    ob_params_file = (
-        get_package_share_directory("orbbec_camera") + "/params/astra_plus_params.yaml"
-    )
-    return LaunchDescription(
-        [
-            Node(
-                package="orbbec_camera",
-                namespace="camera",
-                name="camera",
-                executable="orbbec_camera_node",
-                output="screen",
-                parameters=[ob_params_file],
-            ),
-        ]
-    )

orbbec_camera/launch/astra_pro_plus.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="RGB"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/astra_stereo_u3.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="400"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="400"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/dabai.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="400"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="400"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/dabai_d1.launch.xml

@@ -0,0 +1,67 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    ]
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="400"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="400"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/dabai_dcw.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/dabai_dw.launch.xml

@@ -0,0 +1,66 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/deeya.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="400"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="400"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/femto.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y16"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y16"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/femto.luanch.py

@@ -1,21 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from ament_index_python import get_package_share_directory
-
-
-def generate_launch_description():
-    ob_params_file = (
-        get_package_share_directory("orbbec_camera") + "/params/femto_params.yaml"
-    )
-    return LaunchDescription(
-        [
-            Node(
-                package="orbbec_camera",
-                namespace="camera",
-                name="camera",
-                executable="orbbec_camera_node",
-                output="screen",
-                parameters=[ob_params_file],
-            ),
-        ]
-    )

orbbec_camera/launch/femto_w.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y16"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y16"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/gemini.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="400"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="400"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/gemini2.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="400"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y14"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="400"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y8"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/gemini_e.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="MJPG"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/multi_camera.launch.xml

@@ -0,0 +1,26 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="3d_sensor" default="astra"/>
+    <!-- stereo_s_u3, astrapro, astra -->
+    <arg name="camera1_prefix" default="01"/>
+    <arg name="camera2_prefix" default="02"/>
+    <arg name="camera1_serila_number" default="ADA611300CE"/>
+    <arg name="camera2_serila_number" default="sn123456789"/>
+    <arg name="device_num" default="2"/>
+    <node name="camera" pkg="orbbec_camera" type="ob_cleanup_shm_node" output="screen"/>
+    <include file="$(find-pkg-share orbbec_camera)/launch/$(arg 3d_sensor).launch.xml">
+        <arg name="camera_name" value="$(var camera_name)_$(var camera1_prefix)"/>
+        <arg name="serial_number" value="$(var camera1_serila_number)"/>
+        <arg name="device_num" value="$(var device_num)"/>
+    </include>
+
+    <include file="$(ind-pkg-share orbbec_camera)/launch/$(var 3d_sensor).launch.xml">
+        <arg name="camera_name" value="$(var camera_name)_$(var camera2_prefix)"/>
+        <arg name="serial_number" value="$(var camera2_serila_number)"/>
+        <arg name="device_num" value="$(var device_num)"/>
+    </include>
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="camera_tf"
+          args="0 0 0 0 0 0 camera01_link camera02_link"/>
+</launch>

orbbec_camera/launch/ob_camera.launch.py

@@ -1,22 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from ament_index_python import get_package_share_directory
-
-
-def generate_launch_description():
-    ob_params_file = (
-        get_package_share_directory("orbbec_camera")
-        + "/params/ob_camera_params.yaml"
-    )
-    return LaunchDescription(
-        [
-            Node(
-                package="orbbec_camera",
-                namespace="camera",
-                name="camera",
-                executable="orbbec_camera_node",
-                output="screen",
-                parameters=[ob_params_file],
-            ),
-        ]
-    )

orbbec_camera/launch/ob_camera.launch.xml

@@ -0,0 +1,90 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="false"/>
+    <arg name="point_cloud_qos" default="default"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="30"/>
+    <arg name="color_format" default="RGB"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_qos" default="default"/>
+    <arg name="color_camera_info_qos" default="default"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="30"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_qos" default="default"/>
+    <arg name="depth_camera_info_qos" default="default"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="30"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="ir_qos" default="default"/>
+    <arg name="ir_camera_info_qos" default="default"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_url" default=""/>
+    <arg name="color_info_url" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_publish_extrinsic" default="false"/>
+    <group>
+        <push-ros-namespace namespace="$(var camera_name)"/>
+        <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
+            <param name="camera_name" value="$(var camera_name)"/>
+            <param name="depth_registration" value="$(var depth_registration)"/>
+            <param name="serial_number" value="$(var serial_number)"/>
+            <param name="device_num" value="$(var device_num)"/>
+            <param name="vendor_id" value="$(var vendor_id)"/>
+            <param name="product_id" value="$(var product_id)"/>
+            <param name="enable_point_cloud" value="$(var enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(var enable_colored_point_cloud)"/>
+            <param name="point_cloud_qos" value="$(var point_cloud_qos)"/>
+            <param name="connection_delay" value="$(var connection_delay)"/>
+            <param name="color_width" value="$(var color_width)"/>
+            <param name="color_height" value="$(var color_height)"/>
+            <param name="color_fps" value="$(var color_fps)"/>
+            <param name="color_format" value="$(var color_format)"/>
+            <param name="enable_color" value="$(var enable_color)"/>
+            <param name="flip_color" value="$(var flip_color)"/>
+            <param name="color_qos" value="$(var color_qos)"/>
+            <param name="color_camera_info_qos" value="$(var color_camera_info_qos)"/>
+            <param name="depth_width" value="$(var depth_width)"/>
+            <param name="depth_height" value="$(var depth_height)"/>
+            <param name="depth_fps" value="$(var depth_fps)"/>
+            <param name="depth_format" value="$(var depth_format)"/>
+            <param name="flip_depth" value="$(var flip_depth)"/>
+            <param name="enable_depth" value="$(var enable_depth)"/>
+            <param name="depth_qos" value="$(var depth_qos)"/>
+            <param name="depth_camera_info_qos" value="$(var depth_camera_info_qos)"/>
+            <param name="ir_width" value="$(var ir_width)"/>
+            <param name="ir_height" value="$(var ir_height)"/>
+            <param name="ir_fps" value="$(var ir_fps)"/>
+            <param name="ir_format" value="$(var ir_format)"/>
+            <param name="enable_ir" value="$(var enable_ir)"/>
+            <param name="flip_ir" value="$(var flip_ir)"/>
+            <param name="ir_qos" value="$(var ir_qos)"/>
+            <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
+            <param name="publish_tf" value="$(var publish_tf)"/>
+            <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
+            <param name="ir_info_url" value="$(var ir_info_url)"/>
+            <param name="color_info_url" value="$(var color_info_url)"/>
+            <param name="log_level" value="$(var log_level)"/>
+            <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>

orbbec_camera/launch/ob_camera_gdb.launch.py

@@ -1,23 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from ament_index_python import get_package_share_directory
-
-
-def generate_launch_description():
-    params_config_path = (
-        get_package_share_directory("orbbec_camera")
-        + "/params/ob_camera_params.yaml"
-    )
-    return LaunchDescription(
-        [
-            Node(
-                package="orbbec_camera",
-                namespace="camera",
-                name="camera",
-                executable="orbbec_camera_node",
-                output="screen",
-                parameters=[params_config_path],
-                prefix=["xterm -e gdb -ex run --args"],
-            ),
-        ]
-    )

orbbec_camera/launch/ob_multi_camera.launch.py

@@ -1,49 +0,0 @@
-from launch import LaunchDescription
-import launch_ros.actions
-from ament_index_python import get_package_share_directory
-
-
-def generate_launch_description():
-    ob_params_file1 = (
-        get_package_share_directory("orbbec_camera")
-        + "/params/multi_camera/ob_camera1_params.yaml"
-    )
-    ob_params_file2 = (
-        get_package_share_directory("orbbec_camera")
-        + "/params/multi_camera/ob_camera2_params.yaml"
-    )
-    return LaunchDescription(
-        [
-            launch_ros.actions.Node(
-                package="orbbec_camera",
-                namespace="camera1",
-                name="camera1",
-                executable="orbbec_camera_node",
-                output="screen",
-                parameters=[ob_params_file1],
-            ),
-            launch_ros.actions.Node(
-                package="orbbec_camera",
-                namespace="camera2",
-                name="camera2",
-                executable="orbbec_camera_node",
-                output="screen",
-                parameters=[ob_params_file2],
-            ),
-            # dummy static transformation from camera1 to camera2
-            launch_ros.actions.Node(
-                package="tf2_ros",
-                executable="static_transform_publisher",
-                arguments=[
-                    "0",
-                    "0",
-                    "0",
-                    "0",
-                    "0",
-                    "0",
-                    "camera1_link",
-                    "camera2_link",
-                ],
-            ),
-        ]
-    )

orbbec_camera/package.xml

@@ -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>1.0.4</version>
+  <version>1.1.4</version>
   <description>Orbbec Camera package</description>
   <maintainer email="mocun@orbbec.com">Joe Dong</maintainer>
   <license>TODO: License declaration</license>
@@ -15,6 +15,7 @@
   <depend>image_publisher</depend>
   <depend>rclcpp_components</depend>
   <depend>cv_bridge</depend>
+  <depend>camera_info_manager</depend>
   <depend>orbbec_camera_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>rclcpp</depend>

orbbec_camera/params/astra_plus_params.yaml

@@ -1,31 +0,0 @@
-/**:
-  ros__parameters:
-    color_width: 2048
-    color_height: 1536
-    color_fps: 30.0
-    color_format : "MJPG"
-    color_frame_id: "color_frame"
-    color_optical_frame_id: "color_optical_frame"
-    enable_color: true
-    ir_width: 640
-    ir_height: 480
-    ir_fps: 30.0
-    ir_format : "Y16"
-    ir_frame_id: "ir_frame"
-    ir_optical_frame_id: "ir_optical_frame"
-    enable_ir: true
-    depth_width: 640
-    depth_height: 480
-    depth_fps: 30.0
-    depth_format : "Y16"
-    depth_frame_id: "depth_frame"
-    depth_optical_frame_id: "depth_optical_frame"
-    enable_depth: true
-    publish_tf: true
-    tf_publish_rate: 10.0
-    wait_for_device_timeout: 120.0
-    reconnect_timeout: 6.0
-    d2c_mode: "none"
-    serial_number: ""
-    camera_link_frame_id: "camera_link"
-    ob_log_level: "none"

orbbec_camera/params/femto_params.yaml

@@ -1,28 +0,0 @@
-/**:
-  ros__parameters:
-    color_width: 2048
-    color_height: 1536
-    color_fps: 30.0
-    color_frame_id: "color_frame"
-    color_optical_frame_id: "color_optical_frame"
-    enable_color: true
-    ir_width: 640
-    ir_height: 480
-    ir_fps: 30.0
-    ir_frame_id: "ir_frame"
-    ir_optical_frame_id: "ir_optical_frame"
-    enable_ir: true
-    depth_width: 1280
-    depth_height: 1024
-    depth_fps: 30.0
-    depth_frame_id: "depth_frame"
-    depth_optical_frame_id: "depth_optical_frame"
-    enable_depth: true
-    publish_tf: true
-    tf_publish_rate: 10.0
-    wait_for_device_timeout: 120.0
-    reconnect_timeout: 6.0
-    d2c_mode: "none"
-    serial_number: ""
-    camera_link_frame_id: "camera_link"
-    ob_log_level: "none"

orbbec_camera/params/multi_camera/ob_camera1_params.yaml

@@ -1,28 +0,0 @@
-/camera1/camera1:
-  ros__parameters:
-    color_width: 640
-    color_height: 480
-    color_fps: 30
-    color_frame_id: "color_frame1"
-    color_optical_frame_id: "color_optical_frame1"
-    enable_color: true
-    ir_width: 640
-    ir_height: 480
-    ir_fps: 30
-    ir_frame_id: "ir_frame1"
-    ir_optical_frame_id: "ir_optical_frame1"
-    enable_ir: false
-    depth_width: 640
-    depth_height: 480
-    depth_fps: 25
-    depth_frame_id: "depth_frame1"
-    depth_optical_frame_id: "depth_optical_frame1"
-    enable_depth: true
-    publish_tf: true
-    tf_publish_rate: 10.0
-    wait_for_device_timeout: 120.0
-    reconnect_timeout: 6.0
-    d2c_mode: "hw"
-    serial_number: "BY19810004G"
-    camera_link_frame_id: "camera1_link"
-    ob_log_level: "none"

orbbec_camera/params/multi_camera/ob_camera2_params.yaml

@@ -1,28 +0,0 @@
-/camera2/camera2:
-  ros__parameters:
-    color_width: 640
-    color_height: 480
-    color_fps: 30.0
-    color_frame_id: "color_frame2"
-    color_optical_frame_id: "color_optical_frame2"
-    enable_color: true
-    ir_width: 640
-    ir_height: 480
-    ir_fps: 30.0
-    ir_frame_id: "ir_frame2"
-    ir_optical_frame_id: "ir_optical_frame2"
-    enable_ir: false
-    depth_width: 640
-    depth_height: 480
-    depth_fps: 25.0
-    depth_frame_id: "depth_frame2"
-    depth_optical_frame_id: "depth_optical_frame2"
-    enable_depth: true
-    publish_tf: true
-    tf_publish_rate: 10.0
-    wait_for_device_timeout: 120.0
-    reconnect_timeout: 6.0
-    d2c_mode: "hw"
-    serial_number: "BX2H611003M"
-    camera_link_frame_id: "camera2_link"
-    ob_log_level: "none"

orbbec_camera/params/ob_camera_params.yaml

@@ -1,28 +0,0 @@
-/**:
-  ros__parameters:
-    color_width: 1920
-    color_height: 1080
-    color_fps: 30.0
-    color_frame_id: "color_frame"
-    color_optical_frame_id: "color_optical_frame"
-    enable_color: true
-    ir_width: 640
-    ir_height: 480
-    ir_fps: 30.0
-    ir_frame_id: "ir_frame"
-    ir_optical_frame_id: "ir_optical_frame"
-    enable_ir: true
-    depth_width: 640
-    depth_height: 480
-    depth_fps: 30.0
-    depth_frame_id: "depth_frame"
-    depth_optical_frame_id: "depth_optical_frame"
-    enable_depth: true
-    publish_tf: true
-    tf_publish_rate: 10.0
-    wait_for_device_timeout: 120.0
-    reconnect_timeout: 6.0
-    d2c_mode: "hw"
-    serial_number: ""
-    camera_link_frame_id: "camera_link"
-    ob_log_level: "none"

orbbec_camera/src/list_devices_node.cpp

@@ -0,0 +1,14 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include <orbbec_camera/ob_camera_node_factory.h>
+
+int main() {
+  auto context = std::make_unique<ob::Context>();
+  context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
+  auto list = context->queryDeviceList();
+  for (size_t i = 0; i < list->deviceCount(); i++) {
+    auto serial = list->getDevice(i)->getDeviceInfo()->serialNumber();
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "serial: " << serial);
+  }
+  return 0;
+}

orbbec_camera/src/main.cpp

@@ -0,0 +1,13 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include <orbbec_camera/ob_camera_node_factory.h>
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions options;
+  using namespace orbbec_camera;
+  auto node = std::make_shared<OBCameraNodeFactory>(options);
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

orbbec_camera/src/ob_camera_node.cpp

@@ -21,44 +21,20 @@ using namespace std::chrono_literals;
 
 OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
                            std::shared_ptr<Parameters> parameters)
-    : node_(node), device_(device), parameters_(parameters), logger_(node->get_logger()) {
-  static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node);
-  dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node);
-  // FIXME:
+    : node_(node),
+      device_(std::move(device)),
+      parameters_(std::move(parameters)),
+      logger_(node->get_logger()) {
   is_running_.store(true);
-  format_[DEPTH] = OB_FORMAT_Y16;
-  format_str_[DEPTH] = "Y16";
-  image_format_[OB_STREAM_DEPTH] = CV_16UC1;
-  encoding_[DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1;
-  stream_name_[OB_STREAM_DEPTH] = "depth";
-  unit_step_size_[DEPTH] = sizeof(uint16_t);
-  format_[INFRA0] = OB_FORMAT_Y16;
-  format_str_[INFRA0] = "Y16";
-  image_format_[OB_STREAM_IR] = CV_16UC1;
-  encoding_[INFRA0] = sensor_msgs::image_encodings::MONO16;
-  stream_name_[OB_STREAM_IR] = "ir";
-  unit_step_size_[INFRA0] = sizeof(uint8_t);
-  const auto device_pid = device_->getDeviceInfo()->pid();
-  if (device_pid == FEMTO_PID || device_pid == FEMTO_LIVE_PID || device_pid == FEMTO_OW_PID) {
-    format_[COLOR] = OB_FORMAT_I420;
-    format_str_[COLOR] = "I420";
-  } else if (device_pid == ASTRA_PLUS_PID || device_pid == ASTRA_PLUS_S_PID) {
-    format_[COLOR] = OB_FORMAT_YUYV;
-    format_str_[COLOR] = "YUYV";
-  } else {
-    // default RGB888
-    format_[COLOR] = OB_FORMAT_RGB888;
-    format_str_[COLOR] = "RGB888";
-  }
-  image_format_[OB_STREAM_COLOR] = CV_8UC3;
-  encoding_[COLOR] = sensor_msgs::image_encodings::BGR8;
-  stream_name_[OB_STREAM_COLOR] = "color";
-  unit_step_size_[COLOR] = 3;
+  stream_name_[COLOR] = "color";
+  stream_name_[DEPTH] = "depth";
+  stream_name_[INFRA0] = "ir";
 
   compression_params_.push_back(cv::IMWRITE_PNG_COMPRESSION);
   compression_params_.push_back(0);
   compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY);
   compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
+  setupDefaultImageFormat();
   setupTopics();
   startPipeline();
 }
@@ -106,12 +82,12 @@ void OBCameraNode::setupDevices() {
     }
   }
 
-  for (const auto& [stream_index, enable] : enable_) {
+  for (const auto& [stream_index, enable] : enable_stream_) {
     if (enable && sensors_.find(stream_index) == sensors_.end()) {
       RCLCPP_INFO_STREAM(logger_,
                          magic_enum::enum_name(stream_index.first)
                              << "sensor isn't supported by current device! -- Skipping...");
-      enable_[stream_index] = false;
+      enable_stream_[stream_index] = false;
     }
   }
 }
@@ -121,18 +97,13 @@ void OBCameraNode::setupProfiles() {
     config_.reset();
   }
   config_ = std::make_shared<ob::Config>();
-  if (d2c_mode_ == "sw") {
-    config_->setAlignMode(ALIGN_D2C_SW_MODE);
-    depth_align_ = true;
-  } else if (d2c_mode_ == "hw") {
+  if (depth_registration_) {
     config_->setAlignMode(ALIGN_D2C_HW_MODE);
-    depth_align_ = true;
   } else {
     config_->setAlignMode(ALIGN_DISABLE);
-    depth_align_ = false;
   }
   for (const auto& elem : IMAGE_STREAMS) {
-    if (enable_[elem]) {
+    if (enable_stream_[elem]) {
       const auto& sensor = sensors_[elem];
       auto profiles = sensor->getStreamProfileList();
       for (size_t i = 0; i < profiles->count(); i++) {
@@ -164,16 +135,16 @@ void OBCameraNode::setupProfiles() {
           RCLCPP_ERROR_STREAM(
               logger_, " NO default_profile found , Stream: " << magic_enum::enum_name(elem.first)
                                                               << " will be disable");
-          enable_[elem] = false;
+          enable_stream_[elem] = false;
           continue;
         }
       }
       CHECK_NOTNULL(selected_profile);
       config_->enableStream(selected_profile);
       images_[elem] =
-          cv::Mat(height_[elem], width_[elem], image_format_[elem.first], cv::Scalar(0, 0, 0));
+          cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
       RCLCPP_INFO_STREAM(
-          logger_, " stream " << stream_name_[elem.first] << " is enabled - width: " << width_[elem]
+          logger_, " stream " << stream_name_[elem] << " is enabled - width: " << width_[elem]
                               << ", height: " << height_[elem] << ", fps: " << fps_[elem] << ", "
                               << "Format: " << magic_enum::enum_name(selected_profile->format()));
     }
@@ -186,37 +157,75 @@ void OBCameraNode::startPipeline() {
   }
   pipeline_ = std::make_unique<ob::Pipeline>(device_);
   pipeline_->start(config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
-    onNewFrameSetCallback(std::move(frame_set));
+    onNewFrameSetCallback(frame_set);
   });
 }
 
+void OBCameraNode::setupDefaultImageFormat() {
+  format_[DEPTH] = OB_FORMAT_Y16;
+  format_str_[DEPTH] = "Y16";
+  image_format_[DEPTH] = CV_16UC1;
+  encoding_[DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1;
+  unit_step_size_[DEPTH] = sizeof(uint16_t);
+  format_[INFRA0] = OB_FORMAT_Y16;
+  format_str_[INFRA0] = "Y16";
+  image_format_[INFRA0] = CV_16UC1;
+  encoding_[INFRA0] = sensor_msgs::image_encodings::MONO16;
+  unit_step_size_[INFRA0] = sizeof(uint8_t);
+
+  image_format_[COLOR] = CV_8UC3;
+  encoding_[COLOR] = sensor_msgs::image_encodings::RGB8;
+  unit_step_size_[COLOR] = 3 * sizeof(uint8_t);
+}
+
 void OBCameraNode::getParameters() {
   for (auto stream_index : IMAGE_STREAMS) {
-    std::string param_name = stream_name_[stream_index.first] + "_width";
+    std::string param_name = stream_name_[stream_index] + "_width";
     setAndGetNodeParameter(width_[stream_index], param_name, IMAGE_WIDTH);
-    param_name = stream_name_[stream_index.first] + "_height";
+    param_name = stream_name_[stream_index] + "_height";
     setAndGetNodeParameter(height_[stream_index], param_name, IMAGE_HEIGHT);
-    param_name = stream_name_[stream_index.first] + "_fps";
+    param_name = stream_name_[stream_index] + "_fps";
     setAndGetNodeParameter(fps_[stream_index], param_name, IMAGE_FPS);
-    param_name = "enable_" + stream_name_[stream_index.first];
-    setAndGetNodeParameter(enable_[stream_index], param_name, true);
-    param_name = stream_name_[stream_index.first] + "_frame_id";
-    std::string default_frame_id = "camera_" + stream_name_[stream_index.first] + "_frame";
+    param_name = "enable_" + stream_name_[stream_index];
+    setAndGetNodeParameter(enable_stream_[stream_index], param_name, true);
+    param_name = stream_name_[stream_index] + "_frame_id";
+    std::string default_frame_id = "camera_" + stream_name_[stream_index] + "_frame";
     setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
     std::string default_optical_frame_id =
-        "camera_" + stream_name_[stream_index.first] + "_optical_frame";
-    param_name = stream_name_[stream_index.first] + "_optical_frame_id";
+        "camera_" + stream_name_[stream_index] + "_optical_frame";
+    param_name = stream_name_[stream_index] + "_optical_frame_id";
     setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
-    depth_aligned_frame_id_[stream_index] = stream_name_[OB_STREAM_COLOR] + "_optical_frame";
-    param_name = stream_name_[stream_index.first] + "_format";
+    depth_aligned_frame_id_[stream_index] = stream_name_[COLOR] + "_optical_frame";
+    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]);
+    if (format_[stream_index] == OB_FORMAT_Y8) {
+      CHECK(stream_index.first != OB_STREAM_COLOR);
+      image_format_[stream_index] = CV_8UC1;
+      encoding_[stream_index] = stream_index.first == OB_STREAM_DEPTH
+                                    ? sensor_msgs::image_encodings::TYPE_8UC1
+                                    : sensor_msgs::image_encodings::MONO8;
+      unit_step_size_[stream_index] = sizeof(uint8_t);
+    }
+    param_name = stream_name_[stream_index] + "_qos";
+    setAndGetNodeParameter<std::string>(image_qos_[stream_index], param_name, "default");
+    param_name = stream_name_[stream_index] + "_camera_info_qos";
+    setAndGetNodeParameter<std::string>(camera_info_qos_[stream_index], param_name, "default");
   }
   setAndGetNodeParameter(publish_tf_, "publish_tf", true);
   setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 10.0);
-  setAndGetNodeParameter(publish_rgb_point_cloud_, "publish_rgb_point_cloud", false);
-  setAndGetNodeParameter(d2c_mode_, "d2c_mode", DEFAULT_D2C_MODE);
+  setAndGetNodeParameter(depth_registration_, "depth_registration", false);
+  setAndGetNodeParameter(enable_point_cloud_, "enable_point_cloud", true);
+  setAndGetNodeParameter<std::string>(ir_info_url_, "ir_info_url", "");
+  setAndGetNodeParameter<std::string>(color_info_url_, "color_info_url", "");
+  setAndGetNodeParameter(enable_colored_point_cloud_, "enable_colored_point_cloud", false);
   setAndGetNodeParameter(camera_link_frame_id_, "camera_link_frame_id", DEFAULT_BASE_FRAME_ID);
+  setAndGetNodeParameter(enable_point_cloud_, "enable_point_cloud", true);
+  setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
+  setAndGetNodeParameter(enable_publish_extrinsic_, "enable_publish_extrinsic", false);
+  if (enable_colored_point_cloud_) {
+    depth_registration_ = true;
+  }
 }
 
 void OBCameraNode::setupTopics() {
@@ -232,30 +241,46 @@ void OBCameraNode::setupPublishers() {
   static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using CameraInfo = sensor_msgs::msg::CameraInfo;
-  point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
-      "depth/color/points", rclcpp::QoS{1}.best_effort().keep_last(1));
-  depth_point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
-      "depth/points", rclcpp::QoS{1}.best_effort().keep_last(1));
+  auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
+  if (enable_colored_point_cloud_) {
+    colored_point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
+        "depth/color/points",
+        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
+                    point_cloud_qos_profile));
+  }
+  if (enable_point_cloud_) {
+    point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
+        "depth/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
+                                    point_cloud_qos_profile));
+  }
   for (const auto& stream_index : IMAGE_STREAMS) {
-    std::string name = stream_name_[stream_index.first];
+    std::string name = stream_name_[stream_index];
     std::string topic = name + "/image_raw";
-    image_publishers_[stream_index] = image_transport::create_publisher(node_, topic);
+    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);
     topic = name + "/camera_info";
-    camera_info_publishers_[stream_index] =
-        node_->create_publisher<CameraInfo>(topic, rclcpp::QoS{1}.best_effort());
+    auto camera_info_qos = camera_info_qos_[stream_index];
+    auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos);
+    camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
+        topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
+                           camera_info_qos_profile));
+  }
+  if (enable_publish_extrinsic_) {
+    extrinsics_publisher_ = node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
+        "extrinsic/depth_to_color", rclcpp::QoS{1}.transient_local());
   }
-  extrinsics_publisher_ = node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-      "extrinsic/depth_to_color", rclcpp::QoS{1}.transient_local());
 }
 
-void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
+void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set) {
   try {
-    if (depth_align_ && (format_[COLOR] == OB_FORMAT_YUYV || format_[COLOR] == OB_FORMAT_I420)) {
+    if (depth_registration_ || enable_colored_point_cloud_) {
       if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
-        publishColorPointCloud(frame_set);
+        publishColoredPointCloud(frame_set);
       }
     }
-    if (frame_set->depthFrame() != nullptr) {
+    if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
       publishDepthPointCloud(frame_set);
     }
   } catch (const ob::Error& e) {
@@ -267,12 +292,15 @@ void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
   }
 }
 
-void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
-  if (depth_point_cloud_publisher_->get_subscription_count() == 0) {
+void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set) {
+  if (!enable_point_cloud_ || !point_cloud_publisher_ ||
+      point_cloud_publisher_->get_subscription_count() == 0) {
     return;
   }
-  auto camera_param = pipeline_->getCameraParam();
-  point_cloud_filter_.setCameraParam(camera_param);
+  if (!camera_param_) {
+    camera_param_ = pipeline_->getCameraParam();
+  }
+  point_cloud_filter_.setCameraParam(*camera_param_);
   point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
   auto depth_frame = frame_set->depthFrame();
   auto frame = point_cloud_filter_.process(frame_set);
@@ -309,17 +337,21 @@ void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_se
   point_cloud_msg_.width = valid_count;
   point_cloud_msg_.height = 1;
   modifier.resize(valid_count);
-  depth_point_cloud_publisher_->publish(point_cloud_msg_);
+  point_cloud_publisher_->publish(point_cloud_msg_);
 }
 
-void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
-  if (point_cloud_publisher_->get_subscription_count() == 0) {
+void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set) {
+  if (!enable_colored_point_cloud_ || !colored_point_cloud_publisher_ ||
+      colored_point_cloud_publisher_->get_subscription_count() == 0) {
     return;
   }
   auto depth_frame = frame_set->depthFrame();
   auto color_frame = frame_set->colorFrame();
-  auto camera_param = pipeline_->getCameraParam();
-  point_cloud_filter_.setCameraParam(camera_param);
+  if (!camera_param_) {
+    camera_param_ = pipeline_->getCameraParam();
+  }
+  point_cloud_filter_.setCameraParam(*camera_param_);
+  point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
   point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
   auto frame = point_cloud_filter_.process(frame_set);
   size_t point_size = frame->dataSize() / sizeof(OBColorPoint);
@@ -332,7 +364,7 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
   point_cloud_msg_.height = color_frame->height();
   std::string format_str = "rgb";
   point_cloud_msg_.point_step =
-      addPointField(point_cloud_msg_, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32,
+      addPointField(point_cloud_msg_, format_str, 1, sensor_msgs::msg::PointField::FLOAT32,
                     point_cloud_msg_.point_step);
   point_cloud_msg_.row_step = point_cloud_msg_.width * point_cloud_msg_.point_step;
   point_cloud_msg_.data.resize(point_cloud_msg_.height * point_cloud_msg_.row_step);
@@ -370,10 +402,10 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
   point_cloud_msg_.width = valid_count;
   point_cloud_msg_.height = 1;
   modifier.resize(valid_count);
-  point_cloud_publisher_->publish(point_cloud_msg_);
+  colored_point_cloud_publisher_->publish(point_cloud_msg_);
 }
 
-void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
+void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet>& frame_set) {
   if (frame_set == nullptr) {
     return;
   }
@@ -394,12 +426,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
   }
 }
 
-void OBCameraNode::onNewFrameCallback(std::shared_ptr<ob::Frame> frame,
+void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                                       const stream_index_pair& stream_index) {
   if (frame == nullptr) {
     return;
   }
-  std::shared_ptr<ob::VideoFrame> video_frame = nullptr;
+  std::shared_ptr<ob::VideoFrame> video_frame;
   if (frame->type() == OB_FRAME_COLOR && frame->format() != OB_FORMAT_RGB888) {
     if (!setupFormatConvertType(frame->format())) {
       RCLCPP_ERROR(logger_, "Unsupported color format: %d", frame->format());
@@ -429,14 +461,17 @@ void OBCameraNode::onNewFrameCallback(std::shared_ptr<ob::Frame> frame,
   int height = static_cast<int>(video_frame->height());
   auto& image = images_[stream_index];
   if (image.empty() || image.cols != width || image.rows != height) {
-    image.create(height, width, image_format_[stream_index.first]);
+    image.create(height, width, image_format_[stream_index]);
   }
   image.data = (uchar*)video_frame->data();
   auto timestamp = frameTimeStampToROSTime(video_frame->systemTimeStamp());
-  auto camera_param = pipeline_->getCameraParam();
-  auto& intrinsic = stream_index == COLOR ? camera_param.rgbIntrinsic : camera_param.depthIntrinsic;
+  if (!camera_param_) {
+    camera_param_ = pipeline_->getCameraParam();
+  }
+  auto& intrinsic =
+      stream_index == COLOR ? camera_param_->rgbIntrinsic : camera_param_->depthIntrinsic;
   auto& distortion =
-      stream_index == COLOR ? camera_param.rgbDistortion : camera_param.depthDistortion;
+      stream_index == COLOR ? camera_param_->rgbDistortion : camera_param_->depthDistortion;
   auto camera_info = convertToCameraInfo(intrinsic, distortion, width);
   CHECK(camera_info_publishers_.count(stream_index) > 0);
   camera_info_publishers_[stream_index]->publish(camera_info);
@@ -446,7 +481,7 @@ void OBCameraNode::onNewFrameCallback(std::shared_ptr<ob::Frame> frame,
   image_msg->is_bigendian = false;
   image_msg->step = width * unit_step_size_[stream_index];
   image_msg->header.frame_id =
-      depth_align_ ? depth_aligned_frame_id_[stream_index] : optical_frame_id_[stream_index];
+      depth_registration_ ? depth_aligned_frame_id_[stream_index] : optical_frame_id_[stream_index];
   CHECK(image_publishers_.count(stream_index) > 0);
   image_publishers_[stream_index].publish(image_msg);
 }
@@ -492,7 +527,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
   quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
   std::vector<float> zero_trans = {0, 0, 0};
   auto camera_param = findDefaultCameraParam();
-  if (camera_param.has_value()) {
+  if (enable_publish_extrinsic_ && extrinsics_publisher_ && camera_param.has_value()) {
     auto ex = camera_param->transform;
     Q = rotationMatrixToQuaternion(ex.rot);
     Q = quaternion_optical * Q * quaternion_optical.inverse();
@@ -512,6 +547,11 @@ void OBCameraNode::calcAndPublishStaticTransform() {
 }
 
 void OBCameraNode::publishStaticTransforms() {
+  if (!publish_tf_) {
+    return;
+  }
+  static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
+  dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
   calcAndPublishStaticTransform();
   if (tf_publish_rate_ > 0) {
     tf_thread_ = std::make_shared<std::thread>([this]() { publishDynamicTransforms(); });

orbbec_camera/src/ob_camera_node_factory.cpp

@@ -12,8 +12,6 @@
 
 #include "orbbec_camera/ob_camera_node_factory.h"
 #include <fcntl.h>
-#include <sys/stat.h>
-#include <sys/types.h>
 #include <unistd.h>
 #include <semaphore.h>
 #include <sys/shm.h>
@@ -25,6 +23,7 @@ OBCameraNodeFactory::OBCameraNodeFactory(const rclcpp::NodeOptions &node_options
       logger_(this->get_logger()) {
   init();
 }
+
 OBCameraNodeFactory::OBCameraNodeFactory(const std::string &node_name, const std::string &ns,
                                          const rclcpp::NodeOptions &node_options)
     : Node(node_name, ns, node_options),
@@ -36,8 +35,7 @@ OBCameraNodeFactory::OBCameraNodeFactory(const std::string &node_name, const std
 OBCameraNodeFactory::~OBCameraNodeFactory() {
   is_alive_.store(false);
   sem_unlink(DEFAULT_SEM_NAME.c_str());
-  int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT);
-  if (shm_id != -1) {
+  if (int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT); shm_id != -1) {
     shmctl(shm_id, IPC_RMID, nullptr);
   }
   if (query_thread_ && query_thread_->joinable()) {
@@ -46,28 +44,29 @@ OBCameraNodeFactory::~OBCameraNodeFactory() {
 }
 
 void OBCameraNodeFactory::init() {
-  log_level_ = declare_parameter<std::string>("ob_log_level", "info");
-  auto ob_log_level = obLogSeverityFromString(log_level_);
-  ctx_->setLoggerSeverity(ob_log_level);
+  auto log_level_str = declare_parameter<std::string>("log_level", "none");
+  auto log_level = obLogSeverityFromString(log_level_str);
+  ob::Context::setLoggerSeverity(log_level);
   is_alive_.store(true);
   parameters_ = std::make_shared<Parameters>(this);
   serial_number_ = declare_parameter<std::string>("serial_number", "");
+  device_num_ = declare_parameter<int>("device_num", 1);
   ctx_->setDeviceChangedCallback([this](std::shared_ptr<ob::DeviceList> removed_list,
                                         std::shared_ptr<ob::DeviceList> added_list) {
-    deviceDisconnectCallback(removed_list);
-    deviceConnectCallback(added_list);
+    onDeviceDisconnected(removed_list);
+    onDeviceConnected(added_list);
   });
   check_connect_timer_ =
       this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); });
+  CHECK_NOTNULL(check_connect_timer_);
   query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
 }
 
-void OBCameraNodeFactory::deviceConnectCallback(
-    const std::shared_ptr<ob::DeviceList> &device_list) {
+void OBCameraNodeFactory::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
   if (device_list->deviceCount() == 0) {
     return;
   }
-  RCLCPP_ERROR_STREAM(logger_, "deviceConnectCallback");
+  RCLCPP_INFO_STREAM(logger_, "onDeviceConnected");
   CHECK_NOTNULL(device_list);
   if (!device_) {
     try {
@@ -80,17 +79,16 @@ void OBCameraNodeFactory::deviceConnectCallback(
   }
 }
 
-void OBCameraNodeFactory::deviceDisconnectCallback(
-    const std::shared_ptr<ob::DeviceList> &device_list) {
+void OBCameraNodeFactory::onDeviceDisconnected(const std::shared_ptr<ob::DeviceList> &device_list) {
   if (device_list->deviceCount() == 0) {
     return;
   }
-  RCLCPP_ERROR_STREAM(logger_, "deviceDisconnectCallback");
+  RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
   CHECK_NOTNULL(device_list);
   for (size_t i = 0; i < device_list->deviceCount(); i++) {
     std::string serial_number = device_list->serialNumber(i);
-    std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-    RCLCPP_INFO_STREAM(logger_, "deviceDisconnectCallback: " << serial_number);
+    std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
+    RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected: " << serial_number);
     if (device_info_ && device_info_->serialNumber() == serial_number) {
       ob_camera_node_.reset();
       device_.reset();
@@ -100,7 +98,7 @@ void OBCameraNodeFactory::deviceDisconnectCallback(
   }
 }
 
-OBLogSeverity OBCameraNodeFactory::obLogSeverityFromString(const std::string &log_level) {
+OBLogSeverity OBCameraNodeFactory::obLogSeverityFromString(const std::string_view &log_level) {
   if (log_level == "debug") {
     return OBLogSeverity::OB_LOG_SEVERITY_DEBUG;
   } else if (log_level == "info") {
@@ -116,67 +114,49 @@ OBLogSeverity OBCameraNodeFactory::obLogSeverityFromString(const std::string &lo
   }
 }
 
-void OBCameraNodeFactory::checkConnectTimer() {
-  if (!device_connected_) {
+void OBCameraNodeFactory::checkConnectTimer() const {
+  if (!device_connected_.load()) {
     RCLCPP_ERROR_STREAM(logger_, "checkConnectTimer: device not connected");
     return;
   }
 }
+
 void OBCameraNodeFactory::queryDevice() {
   while (is_alive_ && rclcpp::ok()) {
-    std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-    if (device_) {
-      break;
-    }
-    auto list = ctx_->queryDeviceList();
-    CHECK_NOTNULL(list);
-    if (list->deviceCount() > 0) {
-      try {
-        startDevice(list);
-      } catch (const ob::Error &e) {
-        RCLCPP_ERROR_STREAM(logger_, "Failed to start device: " << e.getMessage());
-      } catch (const std::exception &e) {
-        RCLCPP_ERROR_STREAM(logger_, "Failed to start device: " << e.what());
-      } catch (...) {
-        RCLCPP_ERROR_STREAM(logger_, "Failed to start device");
+    if (!device_connected_) {
+      RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Waiting for device connection...");
+      auto device_list = ctx_->queryDeviceList();
+      if (device_list->deviceCount() == 0) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        continue;
       }
+      onDeviceConnected(device_list);
+    } else {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1000));
     }
-    std::this_thread::sleep_for(std::chrono::seconds(1));
   }
 }
 
 void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
-  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-  if (device_) {
+  std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
+  if (device_connected_) {
     return;
   }
   if (list->deviceCount() == 0) {
     RCLCPP_WARN(logger_, "No device found");
     return;
   }
-  if (serial_number_.empty()) {
-    RCLCPP_INFO_STREAM(logger_, "Connecting to the default device");
-    device_ = list->getDevice(0);
-  } else {
-    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); });
-    auto device_sem = sem_open(DEFAULT_SEM_NAME.c_str(), O_CREAT, 0644, 1);
-    if (device_sem == SEM_FAILED) {
-      RCLCPP_ERROR_STREAM(logger_, "Failed to open semaphore");
-      return;
-    }
-    size_t connected_device_num = 0;
-    RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_);
-    int sem_value = 0;
-    sem_getvalue(device_sem, reinterpret_cast<int *>(&sem_value));
-    RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value);
-    int ret = sem_wait(device_sem);
-    std::shared_ptr<int> sem_guard(nullptr, [&](auto) {
-      RCLCPP_INFO_STREAM(logger_, "release semaphore");
+  if (device_) {
+    device_.reset();
+  }
+  size_t connected_device_num = 0;
+  sem_t *device_sem = nullptr;
+  std::shared_ptr<int> sem_guard(nullptr, [&](int const *) {
+    if (device_num_ > 1 && device_sem) {
+      RCLCPP_INFO(logger_, "Release device semaphore");
       sem_post(device_sem);
-      sem_value = 0;
-      sem_getvalue(device_sem, reinterpret_cast<int *>(&sem_value));
+      int sem_value = 0;
+      sem_getvalue(device_sem, &sem_value);
       RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value);
       RCLCPP_INFO_STREAM(logger_, "Release device semaphore done");
       if (connected_device_num >= device_num_) {
@@ -185,38 +165,53 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
         sem_unlink(DEFAULT_SEM_NAME.c_str());
         RCLCPP_INFO_STREAM(logger_, "All devices connected,  sem_unlink done..");
       }
-    });
-    RCLCPP_INFO_STREAM(logger_, "sem_wait ret: " << ret);
-    if (!ret) {
-      for (size_t i = 0; i < list->deviceCount(); ++i) {
-        auto device = list->getDevice(i);
-        auto info = device->getDeviceInfo();
-        std::string serial = info->serialNumber();
-        if (serial == serial_number_ || serial == lower_sn) {
-          RCLCPP_INFO_STREAM(logger_, "Connecting to device " << serial);
-          device_ = device;
-          break;
-        }
-      }
-    } else {
-      RCLCPP_ERROR_STREAM(logger_, "Failed to wait semaphore " << strerror(errno));
+    }
+  });
+  if (device_num_ == 1) {
+    RCLCPP_INFO_STREAM(logger_, "Connecting to the default device");
+    device_ = list->getDevice(0);
+  } else {
+    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); });
+    device_sem = sem_open(DEFAULT_SEM_NAME.c_str(), O_CREAT, 0644, 1);
+    if (device_sem == SEM_FAILED) {
+      RCLCPP_INFO_STREAM(logger_, "Failed to open semaphore");
+      return;
+    }
+    RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_);
+    int sem_value = 0;
+    sem_getvalue(device_sem, &sem_value);
+    RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value);
+    if (int ret = sem_wait(device_sem); ret != 0) {
+      RCLCPP_INFO_STREAM(logger_, "Failed to wait semaphore " << strerror(errno));
+      return;
+    }
+    try {
+      auto device = list->getDeviceBySN(serial_number_.c_str());
+      device_ = device;
+    } catch (ob::Error &e) {
+      RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.getMessage());
+    } catch (std::exception &e) {
+      RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.what());
+    } catch (...) {
+      RCLCPP_INFO_STREAM(logger_, "Failed to get device info");
     }
 
     if (device_ == nullptr) {
       RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str());
-      RCLCPP_ERROR(logger_, "Release device semaphore");
-
+      device_connected_ = false;
       return;
     } else {
       // write connected device info to file
       int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT);
       if (shm_id == -1) {
-        RCLCPP_ERROR_STREAM(logger_, "Failed to create shared memory " << strerror(errno));
+        RCLCPP_INFO_STREAM(logger_, "Failed to create shared memory " << strerror(errno));
       } else {
         RCLCPP_INFO_STREAM(logger_, "Created shared memory");
         auto shm_ptr = (int *)shmat(shm_id, nullptr, 0);
         if (shm_ptr == (void *)-1) {
-          RCLCPP_ERROR_STREAM(logger_, "Failed to attach shared memory " << strerror(errno));
+          RCLCPP_INFO_STREAM(logger_, "Failed to attach shared memory " << strerror(errno));
         } else {
           RCLCPP_INFO_STREAM(logger_, "Attached shared memory");
           connected_device_num = *shm_ptr + 1;
@@ -231,16 +226,20 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
         }
       }
     }
-    ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_);
-    device_connected_ = true;
-    device_info_ = device_->getDeviceInfo();
-    CHECK_NOTNULL(device_info_);
-    RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->name() << " connected");
-    RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->serialNumber());
-    RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->firmwareVersion());
-    RCLCPP_INFO_STREAM(logger_, "Hardware version: " << device_info_->hardwareVersion());
-    RCLCPP_INFO_STREAM(logger_,
-                       "device type: " << ObDeviceTypeToString(device_info_->deviceType()));
   }
+  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_);
+  device_connected_ = true;
+  device_info_ = device_->getDeviceInfo();
+  CHECK_NOTNULL(device_info_.get());
+  RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->name() << " connected");
+  RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->serialNumber());
+  RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->firmwareVersion());
+  RCLCPP_INFO_STREAM(logger_, "Hardware version: " << device_info_->hardwareVersion());
+  RCLCPP_INFO_STREAM(logger_, "device type: " << ObDeviceTypeToString(device_info_->deviceType()));
 }
 }  // namespace orbbec_camera

orbbec_camera/src/ob_cleanup_shm.cpp

@@ -0,0 +1,29 @@
+#include <fcntl.h>
+#include <semaphore.h>
+#include <sys/shm.h>
+
+#include <cstring>
+#include <iostream>
+
+#include "orbbec_camera/constants.h"
+#include "rclcpp/rclcpp.hpp"
+
+using namespace orbbec_camera;
+
+int main() {
+  sem_t *sem = sem_open(DEFAULT_SEM_NAME.c_str(), O_CREAT, 0644, 0);
+  if (sem == SEM_FAILED) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("cleanup_shm"), "sem_open failed: " << strerror(errno));
+    return 1;
+  }
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("cleanup_shm"), "sem_open succeeded");
+  sem_close(sem);
+  sem_unlink(DEFAULT_SEM_NAME.c_str());
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("cleanup_shm"), "sem_unlink succeeded");
+  int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT);
+  if (shm_id != -1) {
+    shmctl(shm_id, IPC_RMID, nullptr);
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("cleanup_shm"), "shmctl `IPC_RMID` succeeded");
+  }
+  return 0;
+}

orbbec_camera/src/ros_service.cpp

@@ -21,7 +21,7 @@ namespace orbbec_camera {
 void OBCameraNode::setupCameraCtrlServices() {
   using std_srvs::srv::SetBool;
   for (auto stream_index : IMAGE_STREAMS) {
-    auto stream_name = stream_name_[stream_index.first];
+    auto stream_name = stream_name_[stream_index];
     std::string service_name = "get_" + stream_name + "_exposure";
     get_exposure_srv_[stream_index] = node_->create_service<GetInt32>(
         service_name,
@@ -497,16 +497,16 @@ void OBCameraNode::toggleSensorCallback(const std::shared_ptr<SetBool::Request>&
                                         const stream_index_pair& stream_index) {
   std::string msg;
   if (request->data) {
-    if (enable_[stream_index]) {
-      msg = stream_name_[stream_index.first] + " Already ON";
+    if (enable_stream_[stream_index]) {
+      msg = stream_name_[stream_index] + " Already ON";
     }
-    RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index.first] << " ON");
+    RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " ON");
 
   } else {
-    if (!enable_[stream_index]) {
-      msg = stream_name_[stream_index.first] + " Already OFF";
+    if (!enable_stream_[stream_index]) {
+      msg = stream_name_[stream_index] + " Already OFF";
     }
-    RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index.first] << " OFF");
+    RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " OFF");
   }
   if (!msg.empty()) {
     RCLCPP_ERROR_STREAM(logger_, msg);
@@ -521,7 +521,7 @@ bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enab
                                 std::string& msg) {
   try {
     pipeline_->stop();
-    enable_[stream_index] = enabled;
+    enable_stream_[stream_index] = enabled;
     setupProfiles();
     startPipeline();
     return true;

orbbec_camera/src/utils.cpp

@@ -202,7 +202,7 @@ OBFormat OBFormatFromString(const std::string &format) {
     return OB_FORMAT_RGB_POINT;
   } else if (fixed_format == "REL") {
     return OB_FORMAT_RLE;
-  } else if (fixed_format == "RGB888") {
+  } else if (fixed_format == "RGB888" || fixed_format == "RGB") {
     return OB_FORMAT_RGB888;
   } else if (fixed_format == "BGR") {
     return OB_FORMAT_BGR;
@@ -224,4 +224,27 @@ std::string ObDeviceTypeToString(const OBDeviceType &type) {
   }
   return "unknown technology camera";
 }
+
+rmw_qos_profile_t getRMWQosProfileFromString(const std::string& str_qos) {
+  std::string upper_str_qos = str_qos;
+  std::transform(upper_str_qos.begin(), upper_str_qos.end(), upper_str_qos.begin(), ::toupper);
+  if (upper_str_qos == "SYSTEM_DEFAULT") {
+    return rmw_qos_profile_system_default;
+  } else if (upper_str_qos == "DEFAULT") {
+    return rmw_qos_profile_default;
+  } else if (upper_str_qos == "PARAMETER_EVENTS") {
+    return rmw_qos_profile_parameter_events;
+  } else if (upper_str_qos == "SERVICES_DEFAULT") {
+    return rmw_qos_profile_services_default;
+  } else if (upper_str_qos == "PARAMETERS") {
+    return rmw_qos_profile_parameters;
+  } else if (upper_str_qos == "SENSOR_DATA") {
+    return rmw_qos_profile_sensor_data;
+  } else {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("astra_camera"),
+                        "Invalid QoS profile: " << upper_str_qos << ". Using default QoS profile.");
+    return rmw_qos_profile_default;
+  }
+}
+
 }  // namespace orbbec_camera