mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-03 19:47:46 +08:00
merge master
This commit is contained in:
lixiaobin
@@ -1,5 +1,5 @@
|
|||||||
# orbbec_camera
|
# orbbec_camera
|
||||||
[](http://github.com/badges/stability-badges) 
|
[](http://github.com/badges/stability-badges) 
|
||||||
---
|
---
|
||||||
OrbbecSDK ROS2 is a wrapper for the Orbbec 3D camera that provides seamless integration with the ROS2 environment. It
|
OrbbecSDK ROS2 is a wrapper for the Orbbec 3D camera that provides seamless integration with the ROS2 environment. It
|
||||||
supports ROS2 Foxy, Galactic, and Humble distributions.
|
supports ROS2 Foxy, Galactic, and Humble distributions.
|
||||||
@@ -195,6 +195,9 @@ to `true` in the stream that corresponds to the argument of the launch file.
|
|||||||
is `true`.
|
is `true`.
|
||||||
- `/camera/ir/camera_info`: The IR camera info.
|
- `/camera/ir/camera_info`: The IR camera info.
|
||||||
- `/camera/ir/image_raw`: The IR stream image
|
- `/camera/ir/image_raw`: The IR stream image
|
||||||
|
- `/camera/accel/sample`: Acceleration data stream `enable_sync_output_accel_gyro`turned off，`enable_accel`turned on
|
||||||
|
- `/camera/gyro/sample`: Gyroscope data stream，enable_sync_output_accel_gyro`turned off，`enable_gyro`turned on
|
||||||
|
- `camera/gyro_accel/sample`: Synchronized data stream of acceleration and gyroscope，`enable_sync_output_accel_gyro`turned on
|
||||||
|
|
||||||
### Multi-Camera
|
### Multi-Camera
|
||||||
|
|
||||||
@@ -337,6 +340,7 @@ The following are the launch parameters available:
|
|||||||
camera.
|
camera.
|
||||||
- `enumerate_net_device` : Whether to enable the function of enumerating network devices. True means enabled, false means disabled.
|
- `enumerate_net_device` : Whether to enable the function of enumerating network devices. True means enabled, false means disabled.
|
||||||
This feature is only supported by Femto Mega and Gemini 2 XL devices. When accessing these devices through the network, the IP address of the device needs to be configured in advance. The enable switch needs to be set to true.
|
This feature is only supported by Femto Mega and Gemini 2 XL devices. When accessing these devices through the network, the IP address of the device needs to be configured in advance. The enable switch needs to be set to true.
|
||||||
|
- `depth_filter_config` : Configure the loading path for the depth filtering configuration file. By default, the depth filtering configuration file is located in the /config/depthfilter directory,Supported only on Gemini2.
|
||||||
|
|
||||||
## Depth work mode switch
|
## Depth work mode switch
|
||||||
- Before starting the camera, depth work mode (depth_work_mode) can be configured for the corresponding xxx.launch.py file's support.
|
- Before starting the camera, depth work mode (depth_work_mode) can be configured for the corresponding xxx.launch.py file's support.
|
||||||
@@ -349,6 +353,7 @@ The following are the launch parameters available:
|
|||||||
# Unbinned Dense Default
|
# Unbinned Dense Default
|
||||||
# Unbinned Sparse Default
|
# Unbinned Sparse Default
|
||||||
# Binned Sparse Default
|
# Binned Sparse Default
|
||||||
|
# Obstacle Avoidance
|
||||||
DeclareLaunchArgument('depth_work_mode', default_value='')
|
DeclareLaunchArgument('depth_work_mode', default_value='')
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -378,10 +383,11 @@ The frame fps and resolution of IR must be consistent with the depth. The corres
|
|||||||
| product serials | launch file |
|
| product serials | launch file |
|
||||||
|----------------------------------------------|-------------------------|
|
|----------------------------------------------|-------------------------|
|
||||||
| astra+ | astra_adv.launch.py |
|
| astra+ | astra_adv.launch.py |
|
||||||
| astra /astra mini /astra mini pro /astra pro | astra.launch.py |
|
| astra mini /astra mini pro /astra pro | astra.launch.py |
|
||||||
| astra mini pro s | astra.launch.py |
|
| astra mini pro s | astra.launch.py |
|
||||||
| astra2 | astra2.launch.py |
|
| astra2 | astra2.launch.py |
|
||||||
| astra stereo s | stereo_s_u3.launch.py |
|
| astra stereo s | stereo_s_u3.launch.py |
|
||||||
|
| astra pro2 | astra_pro2.launch.py |
|
||||||
| dabai | dabai.launch.py |
|
| dabai | dabai.launch.py |
|
||||||
| dabai d1 | dabai_d1.launch.py |
|
| dabai d1 | dabai_d1.launch.py |
|
||||||
| dabai dcw | dabai_dcw.launch.py |
|
| dabai dcw | dabai_dcw.launch.py |
|
||||||
@@ -402,27 +408,25 @@ Actually, All launch files all most the same, the only difference is the default
|
|||||||
|
|
||||||
## Supported hardware products
|
## Supported hardware products
|
||||||
|
|
||||||
| **SDK version** | **products list** | **firmware version** |
|
| **products list** | **firmware version** |
|
||||||
|-----------------|-------------------|-------------------------------------------|
|
|-------------------|-------------------------------------------|
|
||||||
| v1.8.1 | Gemini 2 XL | Obox: V1.2.5 VL:1.4.54 |
|
| Gemini 2 XL | Obox: V1.2.5 VL:1.4.54 |
|
||||||
| | Astra 2 | 2.8.20 |
|
| Astra 2 | 2.8.20 |
|
||||||
| | Gemini 2 L | 1.4.32 |
|
| Gemini 2 L | 1.4.32 |
|
||||||
| | Gemini 2 | 1.4.60 /1.4.76 |
|
| Gemini 2 | 1.4.60 /1.4.76 |
|
||||||
| | Femto Mega | 1.1.7 (window10、ubuntu20.04、ubuntu22.04) |
|
| Femto Mega | 1.1.7 (window10、ubuntu20.04、ubuntu22.04) |
|
||||||
| | Astra+ | 1.0.22/1.0.21/1.0.20/1.0.19 |
|
| Astra+ | 1.0.22/1.0.21/1.0.20/1.0.19 |
|
||||||
| | Femto | 1.6.7 |
|
| Femto | 1.6.7 |
|
||||||
| | Femto W | 1.1.8 |
|
| Femto W | 1.1.8 |
|
||||||
| | Femto Bolt | 1.0.6 (unsupported ARNM32) |
|
| Femto Bolt | 1.0.6 (unsupported ARM32) |
|
||||||
| | DaBai | 2436 |
|
| DaBai | 2436 |
|
||||||
| | DaBai DCW | 2460 |
|
| DaBai DCW | 2460 |
|
||||||
| | DaBai DW | 2606 |
|
| DaBai DW | 2606 |
|
||||||
| | Astra Mini Pro | 1007 |
|
| Astra Mini Pro | 1007 |
|
||||||
| | Gemini E | 3460 |
|
| Gemini E | 3460 |
|
||||||
| | Gemini E Lite | 3606 |
|
| Gemini E Lite | 3606 |
|
||||||
| | Gemini | 3.0.18 |
|
| Gemini | 3.0.18 |
|
||||||
| | Astra Mini S Pro | 1.0.05 |
|
| Astra Mini S Pro | 1.0.05 |
|
||||||
| | DaBai Max Pro | 1.0.06 |
|
|
||||||
| | Gemini UW | 1.0.060 |
|
|
||||||
|
|
||||||
## DDS Tuning
|
## DDS Tuning
|
||||||
|
|
||||||
|
|||||||
+26
-22
@@ -196,6 +196,9 @@ ros2 service call /camera/save_point_cloud std_srvs/srv/Empty "{}"
|
|||||||
都设置为 `true`才可用。
|
都设置为 `true`才可用。
|
||||||
- `/camera/ir/camera_info`: IR相机信息。
|
- `/camera/ir/camera_info`: IR相机信息。
|
||||||
- `/camera/ir/image_raw`: 红外数据流图像。
|
- `/camera/ir/image_raw`: 红外数据流图像。
|
||||||
|
- `/camera/accel/sample`: 加速度数据流，`enable_sync_output_accel_gyro`配置关闭，`enable_accel`配置打开
|
||||||
|
- `/camera/gyro/sample`: 陀螺仪数据流，enable_sync_output_accel_gyro`配置关闭，`enable_gyro`配置打开
|
||||||
|
- `camera/gyro_accel/sample`: 加速度和陀螺仪同步数据流，通过`enable_sync_output_accel_gyro`配置打开
|
||||||
|
|
||||||
### 多相机
|
### 多相机
|
||||||
|
|
||||||
@@ -324,6 +327,7 @@ ros2 launch orbbec_camera multi_camera.launch.py
|
|||||||
。具体的值取决于当前的相机型号。
|
。具体的值取决于当前的相机型号。
|
||||||
- `enumerate_net_device` : 是否开启枚举网络设备的功能，true为开启，false为关闭，仅Femto mega和Gemini 2 XL设备支持。
|
- `enumerate_net_device` : 是否开启枚举网络设备的功能，true为开启，false为关闭，仅Femto mega和Gemini 2 XL设备支持。
|
||||||
当通过网络方式访问以上设备时，需提前配置好设备的IP地址，使能开关需要配置成true。
|
当通过网络方式访问以上设备时，需提前配置好设备的IP地址，使能开关需要配置成true。
|
||||||
|
- `depth_filter_config` : 配置深度滤波配置文件加载路径，默认深度滤波配置文件在/config/depthfilter目录下,仅gemini2支持。
|
||||||
|
|
||||||
## 深度模式切换：
|
## 深度模式切换：
|
||||||
- 启动相机前,可通过配置对应相继的xxx.launch.py的深度模式(depth_work_mode)支持。
|
- 启动相机前,可通过配置对应相继的xxx.launch.py的深度模式(depth_work_mode)支持。
|
||||||
@@ -336,6 +340,7 @@ ros2 launch orbbec_camera multi_camera.launch.py
|
|||||||
# Unbinned Dense Default
|
# Unbinned Dense Default
|
||||||
# Unbinned Sparse Default
|
# Unbinned Sparse Default
|
||||||
# Binned Sparse Default
|
# Binned Sparse Default
|
||||||
|
# Obstacle Avoidance
|
||||||
DeclareLaunchArgument('depth_work_mode', default_value='')
|
DeclareLaunchArgument('depth_work_mode', default_value='')
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -365,10 +370,11 @@ IR的分辨率和帧率必须和深度保持一致。不同模式和分辨率对
|
|||||||
| product serials | launch file |
|
| product serials | launch file |
|
||||||
|----------------------------------------------|-------------------------|
|
|----------------------------------------------|-------------------------|
|
||||||
| astra+ | astra_adv.launch.py |
|
| astra+ | astra_adv.launch.py |
|
||||||
| astra /astra mini /astra mini pro /astra pro | astra.launch.py |
|
| astra mini /astra mini pro /astra pro | astra.launch.py |
|
||||||
| astra mini pro s | astra.launch.py |
|
| astra mini pro s | astra.launch.py |
|
||||||
| astra2 | astra2.launch.py |
|
| astra2 | astra2.launch.py |
|
||||||
| astra stereo s | stereo_s_u3.launch.py |
|
| astra stereo s | stereo_s_u3.launch.py |
|
||||||
|
| astra pro2 | astra_pro2.launch.py |
|
||||||
| dabai | dabai.launch.py |
|
| dabai | dabai.launch.py |
|
||||||
| dabai d1 | dabai_d1.launch.py |
|
| dabai d1 | dabai_d1.launch.py |
|
||||||
| dabai dcw | dabai_dcw.launch.py |
|
| dabai dcw | dabai_dcw.launch.py |
|
||||||
@@ -390,27 +396,25 @@ XML launch 文件将会在后续的版本中删除。
|
|||||||
|
|
||||||
## 已经支持的硬件产品列表
|
## 已经支持的硬件产品列表
|
||||||
|
|
||||||
| **SDK version** | **products list** | **firmware version** |
|
| **products list** | **firmware version** |
|
||||||
|-----------------|-------------------|-------------------------------------------|
|
|-------------------|-------------------------------------------|
|
||||||
| v1.8.1 | Gemini 2 XL | Obox: V1.2.5 VL:1.4.54 |
|
| Gemini 2 XL | Obox: V1.2.5 VL:1.4.54 |
|
||||||
| | Astra 2 | 2.8.20 |
|
| Astra 2 | 2.8.20 |
|
||||||
| | Gemini 2 L | 1.4.32 |
|
| Gemini 2 L | 1.4.32 |
|
||||||
| | Gemini 2 | 1.4.60 /1.4.76 |
|
| Gemini 2 | 1.4.60 /1.4.76 |
|
||||||
| | Femto Mega | 1.1.7 (window10、ubuntu20.04、ubuntu22.04) |
|
| Femto Mega | 1.1.7 (window10、ubuntu20.04、ubuntu22.04) |
|
||||||
| | Astra+ | 1.0.22/1.0.21/1.0.20/1.0.19 |
|
| Astra+ | 1.0.22/1.0.21/1.0.20/1.0.19 |
|
||||||
| | Femto | 1.6.7 |
|
| Femto | 1.6.7 |
|
||||||
| | Femto W | 1.1.8 |
|
| Femto W | 1.1.8 |
|
||||||
| | Femto Bolt | 1.0.6 (unsupported ARNM32) |
|
| Femto Bolt | 1.0.6 (unsupported ARM32) |
|
||||||
| | DaBai | 2436 |
|
| DaBai | 2436 |
|
||||||
| | DaBai DCW | 2460 |
|
| DaBai DCW | 2460 |
|
||||||
| | DaBai DW | 2606 |
|
| DaBai DW | 2606 |
|
||||||
| | Astra Mini Pro | 1007 |
|
| Astra Mini Pro | 1007 |
|
||||||
| | Gemini E | 3460 |
|
| Gemini E | 3460 |
|
||||||
| | Gemini E Lite | 3606 |
|
| Gemini E Lite | 3606 |
|
||||||
| | Gemini | 3.0.18 |
|
| Gemini | 3.0.18 |
|
||||||
| | Astra Mini S Pro | 1.0.05 |
|
| Astra Mini S Pro | 1.0.05 |
|
||||||
| | DaBai Max Pro | 1.0.06 |
|
|
||||||
| | Gemini UW | 1.0.060 |
|
|
||||||
|
|
||||||
## DDS Tuning
|
## DDS Tuning
|
||||||
|
|
||||||
|
|||||||
@@ -97,7 +97,7 @@ const char *ob_device_list_get_device_ip_address(ob_device_list *list, uint32_t
|
|||||||
/**
|
/**
|
||||||
* @brief Get the device extension information.
|
* @brief Get the device extension information.
|
||||||
*
|
*
|
||||||
* @param[in] info Device Information
|
* @param[in] list Device list object
|
||||||
* @param[in] index Device index
|
* @param[in] index Device index
|
||||||
* @param[out] error Log error messages
|
* @param[out] error Log error messages
|
||||||
* @return const char* The device extension information
|
* @return const char* The device extension information
|
||||||
@@ -865,6 +865,30 @@ void ob_delete_depth_work_mode_list(ob_depth_work_mode_list *work_mode_list, ob_
|
|||||||
*/
|
*/
|
||||||
void ob_delete_data_bundle(ob_data_bundle *data_bundle, ob_error **error);
|
void ob_delete_data_bundle(ob_data_bundle *data_bundle, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Check if the device supports global timestamp.
|
||||||
|
*
|
||||||
|
* @param[in] device The device object.
|
||||||
|
* @param[out] error Log error messages.
|
||||||
|
* @return bool Whether the device supports global timestamp.
|
||||||
|
*/
|
||||||
|
bool ob_device_is_global_timestamp_supported(ob_device *device, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Load depth filter config from file.
|
||||||
|
* @param[in] device The device object.
|
||||||
|
* @param[in] file_path Path of the config file.
|
||||||
|
* @param[out] error Log error messages.
|
||||||
|
*/
|
||||||
|
void ob_device_load_depth_filter_config(ob_device *device, const char *file_path, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Reset depth filter config to device default define.
|
||||||
|
* @param[in] device The device object.
|
||||||
|
* @param[out] error Log error messages.
|
||||||
|
*/
|
||||||
|
void ob_device_reset_default_depth_filter_config(ob_device *device, ob_error **error);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -68,6 +68,20 @@ uint64_t ob_frame_time_stamp_us(ob_frame *frame, ob_error **error);
|
|||||||
*/
|
*/
|
||||||
uint64_t ob_frame_system_time_stamp(ob_frame *frame, ob_error **error);
|
uint64_t ob_frame_system_time_stamp(ob_frame *frame, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get the global timestamp of the frame in microseconds.
|
||||||
|
* @brief The global timestamp is the time point when the frame was was captured by the device, and has been converted to the host clock domain. The
|
||||||
|
* conversion process base on the device timestamp and can eliminate the timer drift of the device
|
||||||
|
*
|
||||||
|
* @attention Only some devices support getting the global timestamp. If the device does not support it, this function will return 0. Check the device support
|
||||||
|
* status by @ref ob_device_is_global_timestamp_supported() function.
|
||||||
|
*
|
||||||
|
* @param[in] frame Frame object
|
||||||
|
* @param[out] error Log error messages
|
||||||
|
* @return uint64_t The global timestamp of the frame in microseconds.
|
||||||
|
*/
|
||||||
|
uint64_t ob_frame_global_time_stamp_us(ob_frame *frame, ob_error **error);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Get frame data
|
* @brief Get frame data
|
||||||
*
|
*
|
||||||
|
|||||||
@@ -161,6 +161,7 @@ typedef enum {
|
|||||||
OB_SENSOR_IR_LEFT = 6, /**< left IR */
|
OB_SENSOR_IR_LEFT = 6, /**< left IR */
|
||||||
OB_SENSOR_IR_RIGHT = 7, /**< Right IR */
|
OB_SENSOR_IR_RIGHT = 7, /**< Right IR */
|
||||||
OB_SENSOR_RAW_PHASE = 8, /**< Raw Phase */
|
OB_SENSOR_RAW_PHASE = 8, /**< Raw Phase */
|
||||||
|
OB_SENSOR_COUNT,
|
||||||
} OBSensorType,
|
} OBSensorType,
|
||||||
ob_sensor_type;
|
ob_sensor_type;
|
||||||
|
|
||||||
@@ -367,7 +368,7 @@ typedef struct {
|
|||||||
typedef struct {
|
typedef struct {
|
||||||
float rot[9]; ///< Rotation matrix
|
float rot[9]; ///< Rotation matrix
|
||||||
float trans[3]; ///< Transformation matrix
|
float trans[3]; ///< Transformation matrix
|
||||||
} OBD2CTransform, ob_d2c_transform;
|
} OBD2CTransform, ob_d2c_transform, OBTransform, ob_transform;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Structure for camera parameters
|
* @brief Structure for camera parameters
|
||||||
@@ -380,6 +381,7 @@ typedef struct {
|
|||||||
OBD2CTransform transform; ///< Rotation/transformation matrix
|
OBD2CTransform transform; ///< Rotation/transformation matrix
|
||||||
bool isMirrored; ///< Whether the image frame corresponding to this group of parameters is mirrored
|
bool isMirrored; ///< Whether the image frame corresponding to this group of parameters is mirrored
|
||||||
} OBCameraParam, ob_camera_param;
|
} OBCameraParam, ob_camera_param;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Camera parameters
|
* @brief Camera parameters
|
||||||
*/
|
*/
|
||||||
@@ -392,6 +394,15 @@ typedef struct {
|
|||||||
OBD2CTransform transform; ///< Rotation/transformation matrix
|
OBD2CTransform transform; ///< Rotation/transformation matrix
|
||||||
} OBCameraParam_V0, ob_camera_param_v0;
|
} OBCameraParam_V0, ob_camera_param_v0;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief calibration parameters
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
OBCameraIntrinsic intrinsics[OB_SENSOR_COUNT]; ///< Sensor internal parameters
|
||||||
|
OBTransform extrinsics[OB_SENSOR_COUNT][OB_SENSOR_COUNT]; ///< The extrinsic parameters allow 3D coordinate conversions between sensor.To transform from a
|
||||||
|
///< source to a target 3D coordinate system,under extrinsics[source][target].
|
||||||
|
} OBCalibrationParam, ob_calibration_param;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Configuration for depth margin filter
|
* @brief Configuration for depth margin filter
|
||||||
*/
|
*/
|
||||||
@@ -456,6 +467,7 @@ typedef enum {
|
|||||||
FORMAT_MJPG_TO_BGRA, /**< MJPG to BGRA */
|
FORMAT_MJPG_TO_BGRA, /**< MJPG to BGRA */
|
||||||
FORMAT_UYVY_TO_RGB888, /**< UYVY to RGB888 */
|
FORMAT_UYVY_TO_RGB888, /**< UYVY to RGB888 */
|
||||||
FORMAT_BGR_TO_RGB, /**< BGR to RGB */
|
FORMAT_BGR_TO_RGB, /**< BGR to RGB */
|
||||||
|
FORMAT_MJPG_TO_NV12, /**< MJPG to NV12 */
|
||||||
} OBConvertFormat,
|
} OBConvertFormat,
|
||||||
ob_convert_format;
|
ob_convert_format;
|
||||||
|
|
||||||
@@ -632,7 +644,15 @@ typedef struct {
|
|||||||
float x; ///< X coordinate
|
float x; ///< X coordinate
|
||||||
float y; ///< Y coordinate
|
float y; ///< Y coordinate
|
||||||
float z; ///< Z coordinate
|
float z; ///< Z coordinate
|
||||||
} OBPoint, ob_point;
|
} OBPoint, ob_point, OBPoint3f, ob_point3f;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 2D point structure in the SDK
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
float x; ///< X coordinate
|
||||||
|
float y; ///< Y coordinate
|
||||||
|
} OBPoint2f, ob_point2f;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 3D point structure with color information
|
* @brief 3D point structure with color information
|
||||||
|
|||||||
@@ -169,7 +169,6 @@ ob_camera_param ob_pipeline_get_camera_param_with_profile(ob_pipeline *pipeline,
|
|||||||
uint32_t depthHeight, ob_error **error);
|
uint32_t depthHeight, ob_error **error);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* \if English
|
|
||||||
* @brief Get current camera parameters
|
* @brief Get current camera parameters
|
||||||
* @attention If D2C is enabled, it will return the camera parameters after D2C, if not, it will return to the default parameters
|
* @attention If D2C is enabled, it will return the camera parameters after D2C, if not, it will return to the default parameters
|
||||||
*
|
*
|
||||||
@@ -179,6 +178,15 @@ ob_camera_param ob_pipeline_get_camera_param_with_profile(ob_pipeline *pipeline,
|
|||||||
*/
|
*/
|
||||||
ob_camera_param ob_pipeline_get_camera_param(ob_pipeline *pipeline, ob_error **error);
|
ob_camera_param ob_pipeline_get_camera_param(ob_pipeline *pipeline, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get device calibration parameters
|
||||||
|
*
|
||||||
|
* @param[in] pipeline pipeline object
|
||||||
|
* @param[out] error Log error messages
|
||||||
|
* @return ob_calibration_param The calibration parameters
|
||||||
|
*/
|
||||||
|
ob_calibration_param ob_pipeline_get_calibration_param(ob_pipeline *pipeline, ob_config *config, ob_error **error);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Return a list of D2C-enabled depth sensor resolutions corresponding to the input color sensor resolution
|
* @brief Return a list of D2C-enabled depth sensor resolutions corresponding to the input color sensor resolution
|
||||||
*
|
*
|
||||||
@@ -314,7 +322,7 @@ void ob_config_set_d2c_target_resolution(ob_config *config, uint32_t d2c_target_
|
|||||||
* can be caused by different frame rates of each stream, or by the loss of frames of one stream): drop directly or output to the user.
|
* can be caused by different frame rates of each stream, or by the loss of frames of one stream): drop directly or output to the user.
|
||||||
*
|
*
|
||||||
* @param[in] config The pipeline configuration
|
* @param[in] config The pipeline configuration
|
||||||
* @param[in] mode The frame aggregation output mode to be set (default mode is @ref OB_FRAME_AGGREGATE_OUTPUT_ANY_SITUATION)
|
* @param[in] mode The frame aggregation output mode to be set (default mode is @ref OB_FRAME_AGGREGATE_OUTPUT_FULL_FRAME_REQUIRE)
|
||||||
* @param[out] error Log error messages
|
* @param[out] error Log error messages
|
||||||
*/
|
*/
|
||||||
void ob_config_set_frame_aggregate_output_mode(ob_config *config, ob_frame_aggregate_output_mode mode, ob_error **error);
|
void ob_config_set_frame_aggregate_output_mode(ob_config *config, ob_frame_aggregate_output_mode mode, ob_error **error);
|
||||||
|
|||||||
@@ -617,11 +617,6 @@ typedef enum {
|
|||||||
*/
|
*/
|
||||||
OB_PROP_SDK_IR_RIGHT_FRAME_UNPACK_BOOL = 3012,
|
OB_PROP_SDK_IR_RIGHT_FRAME_UNPACK_BOOL = 3012,
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief MGC Filter switch(on by default)
|
|
||||||
*/
|
|
||||||
OB_PROP_SDK_DEPTH_RECTIFY_MGC_FILTER_BOOL = 3018,
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Calibration JSON file read from device (Femto Mega, read only)
|
* @brief Calibration JSON file read from device (Femto Mega, read only)
|
||||||
*/
|
*/
|
||||||
|
|||||||
@@ -0,0 +1,87 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include "ObTypes.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transform a 3d point of a source coordinate system into a 3d point of the target coordinate system.
|
||||||
|
*
|
||||||
|
* @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
|
||||||
|
* @param[in] source_point3f Source 3d point value
|
||||||
|
* @param[in] source_sensor_Type Source sensor type
|
||||||
|
* @param[in] target_sensor_type Target sensor type
|
||||||
|
* @param[out] target_point3f Target 3d point value
|
||||||
|
* @param[out] error Log error messages
|
||||||
|
*
|
||||||
|
* @return bool Transform result
|
||||||
|
*/
|
||||||
|
bool ob_calibration_3d_to_3d(const ob_calibration_param calibration_param, const ob_point3f source_point3f, const ob_sensor_type source_sensor_Type,
|
||||||
|
const ob_sensor_type target_sensor_type, ob_point3f *target_point3f, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
|
||||||
|
*
|
||||||
|
* @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
|
||||||
|
* @param[in] source_point2f Source 2d point value
|
||||||
|
* @param[in] source_depth_pixel_value The depth of sourcePoint2f in millimeters
|
||||||
|
* @param[in] source_sensor_type Source sensor type
|
||||||
|
* @param[in] target_sensor_Type Target sensor type
|
||||||
|
* @param[out] target_point3f Target 3d point value
|
||||||
|
* @param[out] error Log error messages
|
||||||
|
*
|
||||||
|
* @return bool Transform result
|
||||||
|
*/
|
||||||
|
bool ob_calibration_2d_to_3d(const ob_calibration_param calibration_param, const ob_point2f source_point2f, const float source_depth_pixel_value,
|
||||||
|
const ob_sensor_type source_sensor_type, const ob_sensor_type target_sensor_Type, ob_point3f *target_point3f, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the target camera.
|
||||||
|
*
|
||||||
|
* @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
|
||||||
|
* @param[in] source_point3f Source 3d point value
|
||||||
|
* @param[in] source_sensor_type Source sensor type
|
||||||
|
* @param[in] target_sensor_type Target sensor type
|
||||||
|
* @param[out] target_point2f Target 2d point value
|
||||||
|
* @param[out] error Log error messages
|
||||||
|
*
|
||||||
|
* @return bool Transform result
|
||||||
|
*/
|
||||||
|
bool ob_calibration_3d_to_2d(const ob_calibration_param calibration_param, const ob_point3f source_point3f, const ob_sensor_type source_sensor_type,
|
||||||
|
const ob_sensor_type target_sensor_type, ob_point2f *target_point2f, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 2d pixel coordinate of the target camera
|
||||||
|
*
|
||||||
|
* @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
|
||||||
|
* @param[in] source_point2f Source 2d point value
|
||||||
|
* @param[in] source_depth_pixel_value The depth of sourcePoint2f in millimeters
|
||||||
|
* @param[in] source_sensor_type Source sensor type
|
||||||
|
* @param[in] target_sensor_type Target sensor type
|
||||||
|
* @param[out] target_point2f Target 2d point value
|
||||||
|
* @param[out] error Log error messages
|
||||||
|
*
|
||||||
|
* @return bool Transform result
|
||||||
|
*/
|
||||||
|
bool ob_calibration_2d_to_2d(const ob_calibration_param calibration_param, const ob_point2f source_point2f, const float source_depth_pixel_value,
|
||||||
|
const ob_sensor_type source_sensor_type, const ob_sensor_type target_sensor_type, ob_point2f *target_point2f, ob_error **error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transforms the depth frame into the geometry of the color camera.
|
||||||
|
*
|
||||||
|
* @param[in] device Device handle
|
||||||
|
* @param[in] depth_frame Input depth frame
|
||||||
|
* @param[in] target_color_camera_width Target color camera width
|
||||||
|
* @param[in] target_color_camera_height Target color camera height
|
||||||
|
* @param[out] error Log error messages
|
||||||
|
*
|
||||||
|
* @return ob_frame* Transformed depth frame
|
||||||
|
*/
|
||||||
|
ob_frame *transformation_depth_frame_to_color_camera(ob_device *device, ob_frame *depth_frame, uint32_t target_color_camera_width,
|
||||||
|
uint32_t target_color_camera_height, ob_error **error);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
@@ -26,9 +26,11 @@ class CameraParamList;
|
|||||||
class OBDepthWorkModeList;
|
class OBDepthWorkModeList;
|
||||||
|
|
||||||
class OB_EXTENSION_API Device {
|
class OB_EXTENSION_API Device {
|
||||||
private:
|
protected:
|
||||||
std::unique_ptr<DeviceImpl> impl_;
|
std::unique_ptr<DeviceImpl> impl_;
|
||||||
|
|
||||||
|
Device(Device &&device);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* @brief Describe the entity of the RGBD camera, representing a specific model of RGBD camera
|
* @brief Describe the entity of the RGBD camera, representing a specific model of RGBD camera
|
||||||
@@ -299,6 +301,13 @@ public:
|
|||||||
*/
|
*/
|
||||||
bool isPropertySupported(OBPropertyID propertyId, OBPermissionType permission);
|
bool isPropertySupported(OBPropertyID propertyId, OBPermissionType permission);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Check if the global timestamp is supported for the device
|
||||||
|
*
|
||||||
|
* @return Whether the global timestamp is supported
|
||||||
|
*/
|
||||||
|
bool isGlobalTimestampSupported();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Upgrade the device firmware
|
* @brief Upgrade the device firmware
|
||||||
*
|
*
|
||||||
@@ -530,8 +539,20 @@ public:
|
|||||||
*/
|
*/
|
||||||
void timerSyncWithHost();
|
void timerSyncWithHost();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Load depth filter config from file.
|
||||||
|
* @param filePath Path of the config file.
|
||||||
|
*/
|
||||||
|
void loadDepthFilterConfig(const char *filePath);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Reset depth filter config to device default define.
|
||||||
|
*/
|
||||||
|
void resetDefaultDepthFilterConfig();
|
||||||
|
|
||||||
friend class Pipeline;
|
friend class Pipeline;
|
||||||
friend class Recorder;
|
friend class Recorder;
|
||||||
|
friend class CoordinateTransformHelper;
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -108,6 +108,18 @@ public:
|
|||||||
*/
|
*/
|
||||||
uint64_t systemTimeStamp();
|
uint64_t systemTimeStamp();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get the global timestamp of the frame in microseconds.
|
||||||
|
* @brief The global timestamp is the time point when the frame was was captured by the device, and has been converted to the host clock domain. The
|
||||||
|
* conversion process base on the device timestamp and can eliminate the timer drift of the device
|
||||||
|
*
|
||||||
|
* @attention Only some devices support getting the global timestamp. If the device does not support it, this function will return 0. Check the device
|
||||||
|
* support status by @ref Device::isGlobalTimestampSupported() function.
|
||||||
|
*
|
||||||
|
* @return uint64_t The global timestamp of the frame in microseconds.
|
||||||
|
*/
|
||||||
|
uint64_t globalTimeStampUs();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Check if the runtime type of the frame object is compatible with a given type.
|
* @brief Check if the runtime type of the frame object is compatible with a given type.
|
||||||
*
|
*
|
||||||
@@ -134,6 +146,7 @@ private:
|
|||||||
friend class Filter;
|
friend class Filter;
|
||||||
friend class Recorder;
|
friend class Recorder;
|
||||||
friend class FrameHelper;
|
friend class FrameHelper;
|
||||||
|
friend class CoordinateTransformHelper;
|
||||||
};
|
};
|
||||||
|
|
||||||
class OB_EXTENSION_API VideoFrame : public Frame {
|
class OB_EXTENSION_API VideoFrame : public Frame {
|
||||||
|
|||||||
@@ -145,6 +145,15 @@ public:
|
|||||||
*/
|
*/
|
||||||
OBCameraParam getCameraParamWithProfile(uint32_t colorWidth, uint32_t colorHeight, uint32_t depthWidth, uint32_t depthHeight);
|
OBCameraParam getCameraParamWithProfile(uint32_t colorWidth, uint32_t colorHeight, uint32_t depthWidth, uint32_t depthHeight);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get the calibration parameters
|
||||||
|
*
|
||||||
|
* @param config The configured parameters
|
||||||
|
*
|
||||||
|
* @return OBCalibrationParam The calibration parameters
|
||||||
|
*/
|
||||||
|
OBCalibrationParam getCalibrationParam(std::shared_ptr<Config> config);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Return a list of D2C-enabled depth sensor resolutions corresponding to the input color sensor resolution
|
* @brief Return a list of D2C-enabled depth sensor resolutions corresponding to the input color sensor resolution
|
||||||
*
|
*
|
||||||
@@ -253,6 +262,15 @@ public:
|
|||||||
*/
|
*/
|
||||||
void setD2CTargetResolution(uint32_t d2cTargetWidth, uint32_t d2cTargetHeight);
|
void setD2CTargetResolution(uint32_t d2cTargetWidth, uint32_t d2cTargetHeight);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Set the frame aggregation output mode for the pipeline configuration
|
||||||
|
* @brief The processing strategy when the FrameSet generated by the frame aggregation function does not contain the frames of all opened streams (which
|
||||||
|
* can be caused by different frame rates of each stream, or by the loss of frames of one stream): drop directly or output to the user.
|
||||||
|
*
|
||||||
|
* @param mode The frame aggregation output mode to be set (default mode is @ref OB_FRAME_AGGREGATE_OUTPUT_FULL_FRAME_REQUIRE)
|
||||||
|
*/
|
||||||
|
void setFrameAggregateOutputMode(OBFrameAggregateOutputMode mode);
|
||||||
|
|
||||||
friend class Pipeline;
|
friend class Pipeline;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,86 @@
|
|||||||
|
/**
|
||||||
|
* @file Utils.hpp
|
||||||
|
* @brief The SDK utils class
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "Types.hpp"
|
||||||
|
|
||||||
|
namespace ob {
|
||||||
|
class Device;
|
||||||
|
|
||||||
|
class OB_EXTENSION_API CoordinateTransformHelper {
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* @brief Transform a 3d point of a source coordinate system into a 3d point of the target coordinate system.
|
||||||
|
*
|
||||||
|
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
|
||||||
|
* @param sourcePoint3f Source 3d point value
|
||||||
|
* @param sourceSensorType Source sensor type
|
||||||
|
* @param targetSensorType Target sensor type
|
||||||
|
* @param targetPoint3f Target 3d point value
|
||||||
|
*
|
||||||
|
* @return bool Transform result
|
||||||
|
*/
|
||||||
|
static bool calibration3dTo3d(const OBCalibrationParam calibrationParam, const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
|
||||||
|
const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
|
||||||
|
*
|
||||||
|
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
|
||||||
|
* @param sourcePoint2f Source 2d point value
|
||||||
|
* @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
|
||||||
|
* @param sourceSensorType Source sensor type
|
||||||
|
* @param targetSensorType Target sensor type
|
||||||
|
* @param targetPoint3f Target 3d point value
|
||||||
|
*
|
||||||
|
* @return bool Transform result
|
||||||
|
*/
|
||||||
|
static bool calibration2dTo3d(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
|
||||||
|
const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the target camera.
|
||||||
|
*
|
||||||
|
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
|
||||||
|
* @param sourcePoint3f Source 3d point value
|
||||||
|
* @param sourceSensorType Source sensor type
|
||||||
|
* @param targetSensorType Target sensor type
|
||||||
|
* @param targetPoint2f Target 2d point value
|
||||||
|
*
|
||||||
|
* @return bool Transform result
|
||||||
|
*/
|
||||||
|
static bool calibration3dTo2d(const OBCalibrationParam calibrationParam, const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
|
||||||
|
const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 2d pixel coordinate of the target camera
|
||||||
|
*
|
||||||
|
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
|
||||||
|
* @param sourcePoint2f Source 2d point value
|
||||||
|
* @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
|
||||||
|
* @param sourceSensorType Source sensor type
|
||||||
|
* @param targetSensorType Target sensor type
|
||||||
|
* @param targetPoint2f Target 2d point value
|
||||||
|
*
|
||||||
|
* @return bool Transform result
|
||||||
|
*/
|
||||||
|
static bool calibration2dTo2d(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
|
||||||
|
const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Transforms the depth frame into the geometry of the color camera.
|
||||||
|
*
|
||||||
|
* @param device Device handle
|
||||||
|
* @param depthFrame Input depth frame
|
||||||
|
* @param targetColorCameraWidth Target color camera width
|
||||||
|
* @param targetColorCameraHeight Target color camera height
|
||||||
|
*
|
||||||
|
* @return std::shared_ptr<ob::Frame> Transformed depth frame
|
||||||
|
*/
|
||||||
|
static std::shared_ptr<ob::Frame> transformationDepthFrameToColorCamera(std::shared_ptr<ob::Device> device, std::shared_ptr<ob::Frame> depthFrame,
|
||||||
|
uint32_t targetColorCameraWidth, uint32_t targetColorCameraHeight);
|
||||||
|
};
|
||||||
|
} // namespace ob
|
||||||
@@ -1 +1 @@
|
|||||||
libOrbbecSDK.so.1.8
|
libOrbbecSDK.so.1.9
|
||||||
@@ -1 +0,0 @@
|
|||||||
libOrbbecSDK.so.1.8.1
|
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
libOrbbecSDK.so.1.9.1
|
||||||
BIN
Binary file not shown.
@@ -1 +0,0 @@
|
|||||||
libudev.so.1.6.3
|
|
||||||
@@ -1 +0,0 @@
|
|||||||
libudev.so.1.6.3
|
|
||||||
Binary file not shown.
@@ -1 +1 @@
|
|||||||
libOrbbecSDK.so.1.8
|
libOrbbecSDK.so.1.9
|
||||||
@@ -1 +0,0 @@
|
|||||||
libOrbbecSDK.so.1.8.1
|
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
libOrbbecSDK.so.1.9.1
|
||||||
BIN
Binary file not shown.
@@ -1 +0,0 @@
|
|||||||
libudev.so.1.6.3
|
|
||||||
@@ -1 +0,0 @@
|
|||||||
libudev.so.1.6.3
|
|
||||||
Binary file not shown.
@@ -1 +1 @@
|
|||||||
libOrbbecSDK.so.1.8
|
libOrbbecSDK.so.1.9
|
||||||
@@ -1 +0,0 @@
|
|||||||
libOrbbecSDK.so.1.8.1
|
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
libOrbbecSDK.so.1.9.1
|
||||||
BIN
Binary file not shown.
@@ -33,6 +33,14 @@
|
|||||||
<FrameProcessingBlockQueueSize>10</FrameProcessingBlockQueueSize>
|
<FrameProcessingBlockQueueSize>10</FrameProcessingBlockQueueSize>
|
||||||
</Memory>
|
</Memory>
|
||||||
|
|
||||||
|
<Misc>
|
||||||
|
<GlobalTimestampFitterEnable>false</GlobalTimestampFitterEnable>
|
||||||
|
<!--Global timestamp fitter refresh interval, unit: milliseconds, default value: 1000, minimum value: 100, it is recommended not to be greater than 10000 -->
|
||||||
|
<GlobalTimestampFitterInterval>1000</GlobalTimestampFitterInterval>
|
||||||
|
<!--Global timestamp fitter queue size, default value: 10, minimum value: 4 -->
|
||||||
|
<GlobalTimestampFitterQueueSize>10</GlobalTimestampFitterQueueSize>
|
||||||
|
</Misc>
|
||||||
|
|
||||||
<!--Default working configuration of pipeline-->
|
<!--Default working configuration of pipeline-->
|
||||||
<Pipeline>
|
<Pipeline>
|
||||||
<Stream>
|
<Stream>
|
||||||
@@ -461,6 +469,10 @@
|
|||||||
</DaBaiDCW>
|
</DaBaiDCW>
|
||||||
|
|
||||||
<DaBaiDCW2>
|
<DaBaiDCW2>
|
||||||
|
<!-- Depth Filter Params -->
|
||||||
|
<!--
|
||||||
|
<DepthFilterDDOConfig>./Openni_device.json</DepthFilterDDOConfig>
|
||||||
|
-->
|
||||||
<Depth>
|
<Depth>
|
||||||
<!--The width of the default resolution, int type-->
|
<!--The width of the default resolution, int type-->
|
||||||
<Width>640</Width>
|
<Width>640</Width>
|
||||||
@@ -498,6 +510,92 @@
|
|||||||
<StreamFailedRetry>0</StreamFailedRetry>
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
</IR>
|
</IR>
|
||||||
</DaBaiDCW2>
|
</DaBaiDCW2>
|
||||||
|
<GeminiEW>
|
||||||
|
<Depth>
|
||||||
|
<!--The width of the default resolution, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--The height of the default resolution, int type-->
|
||||||
|
<Height>400</Height>
|
||||||
|
<!--Default frame rate, int type-->
|
||||||
|
<FPS>15</FPS>
|
||||||
|
<!--Default frame format-->
|
||||||
|
<Format>Y11</Format>
|
||||||
|
<!--Whether to retry after open stream failure, 0 means no retry, >=1 retry and how many times to retry-->
|
||||||
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
|
<ProfileFilter>
|
||||||
|
<profile>
|
||||||
|
<FPS>30</FPS>
|
||||||
|
</profile>
|
||||||
|
</ProfileFilter>
|
||||||
|
</Depth>
|
||||||
|
<Color>
|
||||||
|
<!--The width of the default resolution, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--The height of the default resolution, int type-->
|
||||||
|
<Height>480</Height>
|
||||||
|
<!--Default frame rate, int type-->
|
||||||
|
<FPS>15</FPS>
|
||||||
|
<!--Default frame format-->
|
||||||
|
<Format>MJPG</Format>
|
||||||
|
<!--Whether to retry after open stream failure, 0 means no retry, >=1 retry and how many times to retry-->
|
||||||
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
|
</Color>
|
||||||
|
<IR>
|
||||||
|
<!--The width of the default resolution, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--The height of the default resolution, int type-->
|
||||||
|
<Height>400</Height>
|
||||||
|
<!--Default frame rate, int type-->
|
||||||
|
<FPS>15</FPS>
|
||||||
|
<!--Default frame format-->
|
||||||
|
<Format>Y10</Format>
|
||||||
|
<!--Whether to retry after open stream failure, 0 means no retry, >=1 retry and how many times to retry-->
|
||||||
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
|
<ProfileFilter>
|
||||||
|
<profile>
|
||||||
|
<FPS>30</FPS>
|
||||||
|
</profile>
|
||||||
|
</ProfileFilter>
|
||||||
|
</IR>
|
||||||
|
</GeminiEW>
|
||||||
|
<DaBaiMax>
|
||||||
|
<Depth>
|
||||||
|
<!--The width of the default resolution, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--The height of the default resolution, int type-->
|
||||||
|
<Height>320</Height>
|
||||||
|
<!--Default frame rate, int type-->
|
||||||
|
<FPS>10</FPS>
|
||||||
|
<!--Default frame format-->
|
||||||
|
<Format>Y12</Format>
|
||||||
|
<!--Whether to retry after open stream failure, 0 means no retry, >=1 retry and how many times to retry-->
|
||||||
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
|
</Depth>
|
||||||
|
<Color>
|
||||||
|
<!--The width of the default resolution, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--The height of the default resolution, int type-->
|
||||||
|
<Height>480</Height>
|
||||||
|
<!--Default frame rate, int type-->
|
||||||
|
<FPS>25</FPS>
|
||||||
|
<!--Default frame format-->
|
||||||
|
<Format>MJPG</Format>
|
||||||
|
<!--Whether to retry after open stream failure, 0 means no retry, >=1 retry and how many times to retry-->
|
||||||
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
|
</Color>
|
||||||
|
<IR>
|
||||||
|
<!--The width of the default resolution, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--The height of the default resolution, int type-->
|
||||||
|
<Height>400</Height>
|
||||||
|
<!--Default frame rate, int type-->
|
||||||
|
<FPS>10</FPS>
|
||||||
|
<!--Default frame format-->
|
||||||
|
<Format>Y10</Format>
|
||||||
|
<!--Whether to retry after open stream failure, 0 means no retry, >=1 retry and how many times to retry-->
|
||||||
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
|
</IR>
|
||||||
|
</DaBaiMax>
|
||||||
<DaBaiMaxPro>
|
<DaBaiMaxPro>
|
||||||
<Depth>
|
<Depth>
|
||||||
<!--The width of the default resolution, int type-->
|
<!--The width of the default resolution, int type-->
|
||||||
@@ -627,7 +725,42 @@
|
|||||||
<StreamFailedRetry>0</StreamFailedRetry>
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
</IR>
|
</IR>
|
||||||
</DaBaiDW2>
|
</DaBaiDW2>
|
||||||
|
<GeminiEWLite>
|
||||||
|
<Depth>
|
||||||
|
<!--The width of the default resolution, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--The height of the default resolution, int type-->
|
||||||
|
<Height>400</Height>
|
||||||
|
<!--Default frame rate, int type-->
|
||||||
|
<FPS>15</FPS>
|
||||||
|
<!--Default frame format-->
|
||||||
|
<Format>Y11</Format>
|
||||||
|
<!--Whether to retry after open stream failure, 0 means no retry, >=1 retry and how many times to retry-->
|
||||||
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
|
<ProfileFilter>
|
||||||
|
<profile>
|
||||||
|
<FPS>30</FPS>
|
||||||
|
</profile>
|
||||||
|
</ProfileFilter>
|
||||||
|
</Depth>
|
||||||
|
<IR>
|
||||||
|
<!--The width of the default resolution, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--The height of the default resolution, int type-->
|
||||||
|
<Height>400</Height>
|
||||||
|
<!--Default frame rate, int type-->
|
||||||
|
<FPS>15</FPS>
|
||||||
|
<!--Default frame format-->
|
||||||
|
<Format>Y10</Format>
|
||||||
|
<!--Whether to retry after open stream failure, 0 means no retry, >=1 retry and how many times to retry-->
|
||||||
|
<StreamFailedRetry>0</StreamFailedRetry>
|
||||||
|
<ProfileFilter>
|
||||||
|
<profile>
|
||||||
|
<FPS>30</FPS>
|
||||||
|
</profile>
|
||||||
|
</ProfileFilter>
|
||||||
|
</IR>
|
||||||
|
</GeminiEWLite>
|
||||||
<DaBaiDC1>
|
<DaBaiDC1>
|
||||||
<Depth>
|
<Depth>
|
||||||
<!--The width of the default resolution, int type-->
|
<!--The width of the default resolution, int type-->
|
||||||
@@ -1229,6 +1362,11 @@
|
|||||||
<DefaultHeartBeat>0</DefaultHeartBeat>
|
<DefaultHeartBeat>0</DefaultHeartBeat>
|
||||||
<!--Whether to enable firmware upgrade foolproof by default, only internal version supports-->
|
<!--Whether to enable firmware upgrade foolproof by default, only internal version supports-->
|
||||||
<FirmwareUpgradeFoolproof>1</FirmwareUpgradeFoolproof>
|
<FirmwareUpgradeFoolproof>1</FirmwareUpgradeFoolproof>
|
||||||
|
|
||||||
|
<!-- Depth Filter Params -->
|
||||||
|
<!--
|
||||||
|
<DepthFilterDDOConfig>./Gemini2_v1.0.json</DepthFilterDDOConfig>
|
||||||
|
-->
|
||||||
|
|
||||||
<!--TODO lumiaozi It is cumbersome to consider the recommended configuration in different camera depth modes-->
|
<!--TODO lumiaozi It is cumbersome to consider the recommended configuration in different camera depth modes-->
|
||||||
<!--TODO lumiaozi have to consider the uncorresponding resolutions in different camera depth modes-->
|
<!--TODO lumiaozi have to consider the uncorresponding resolutions in different camera depth modes-->
|
||||||
@@ -1294,7 +1432,7 @@
|
|||||||
<UnbinnedSparseDefault>
|
<UnbinnedSparseDefault>
|
||||||
<!--You can configure the default resolution corresponding to the sensor in the current mode here, if not configured, then use the Device node to correspond to the default resolution of the sensor -->
|
<!--You can configure the default resolution corresponding to the sensor in the current mode here, if not configured, then use the Device node to correspond to the default resolution of the sensor -->
|
||||||
</UnbinnedSparseDefault>
|
</UnbinnedSparseDefault>
|
||||||
<!--Deep working mode: Binned Sparse Default -->
|
<!--Deep working mode: Binned Sparse Default -->
|
||||||
<BinnedSparseDefault>
|
<BinnedSparseDefault>
|
||||||
<!--You can configure the default resolution corresponding to the sensor in the current mode here, if not configured, then use the Device node to correspond to the default resolution of the sensor -->
|
<!--You can configure the default resolution corresponding to the sensor in the current mode here, if not configured, then use the Device node to correspond to the default resolution of the sensor -->
|
||||||
<Depth>
|
<Depth>
|
||||||
@@ -1316,6 +1454,18 @@
|
|||||||
<Format>Y8</Format>
|
<Format>Y8</Format>
|
||||||
</IR>
|
</IR>
|
||||||
</BinnedSparseDefault>
|
</BinnedSparseDefault>
|
||||||
|
<!-- Deep working mode: Obstacle Avoidance -->
|
||||||
|
<ObstacleAvoidance>
|
||||||
|
<Depth>
|
||||||
|
<!--The resolution width is enabled by default, int type-->
|
||||||
|
<Width>640</Width>
|
||||||
|
<!--High resolution is enabled by default, int type-->
|
||||||
|
<Height>400</Height>
|
||||||
|
<!--The frame rate of the resolution enabled by default, int type-->
|
||||||
|
<FPS>15</FPS>
|
||||||
|
<Format>RLE</Format>
|
||||||
|
</Depth>
|
||||||
|
</ObstacleAvoidance>
|
||||||
<FactoryCalibration>
|
<FactoryCalibration>
|
||||||
<Color>
|
<Color>
|
||||||
<!--The resolution width is enabled by default, int type-->
|
<!--The resolution width is enabled by default, int type-->
|
||||||
@@ -2076,7 +2226,7 @@
|
|||||||
<StreamInterruptedRestart>5</StreamInterruptedRestart>
|
<StreamInterruptedRestart>5</StreamInterruptedRestart>
|
||||||
<!--The maximum frame interval time, if this value is exceeded, it will be judged that the stream is interrupted-->
|
<!--The maximum frame interval time, if this value is exceeded, it will be judged that the stream is interrupted-->
|
||||||
<MaxFrameIntervalMs>5000</MaxFrameIntervalMs>
|
<MaxFrameIntervalMs>5000</MaxFrameIntervalMs>
|
||||||
</IR_RIGHT>
|
</IR_RIGHT>
|
||||||
</BinnedSparseDefault>
|
</BinnedSparseDefault>
|
||||||
<FactoryCalibration>
|
<FactoryCalibration>
|
||||||
<Color>
|
<Color>
|
||||||
|
|||||||
@@ -0,0 +1,251 @@
|
|||||||
|
{
|
||||||
|
"device": "Orbbec Gemini 2",
|
||||||
|
"structVersion": "0.0.c",
|
||||||
|
"dataVersion": "1.7.0",
|
||||||
|
"description": "Gemini2 depth filter params",
|
||||||
|
"configDate": "20231212",
|
||||||
|
"vid": "0x2bc5",
|
||||||
|
"pid": "0x0670",
|
||||||
|
"depthFilters": [
|
||||||
|
{
|
||||||
|
"depthWorkMode": "Unbinned Dense Default",
|
||||||
|
"width": 1280,
|
||||||
|
"height":800,
|
||||||
|
"bxf": 30500,
|
||||||
|
"invalid_value":0,
|
||||||
|
"NoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"size": 200,
|
||||||
|
"disp_diff": 100,
|
||||||
|
"type": "NR_OVERALL",
|
||||||
|
"lut": [
|
||||||
|
200, 100, 100, 200,
|
||||||
|
200, 100, 100, 200,
|
||||||
|
200, 100, 100, 200,
|
||||||
|
200, 100, 100, 200
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"EdgeNoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"type": "MG_FILTER",
|
||||||
|
"x1_th": 6,
|
||||||
|
"x2_th": 6,
|
||||||
|
"y1_th": 6,
|
||||||
|
"y2_th": 6,
|
||||||
|
"limit_x": 70,
|
||||||
|
"limit_y": 70,
|
||||||
|
"R": 750,
|
||||||
|
"width1": 40,
|
||||||
|
"width2": 40
|
||||||
|
},
|
||||||
|
"SpatialFastFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 3
|
||||||
|
},
|
||||||
|
"SpatialModerateFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 3,
|
||||||
|
"iters": 1,
|
||||||
|
"disp_diff": 100
|
||||||
|
},
|
||||||
|
"SpatialAdvancedFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "SFA_ALL",
|
||||||
|
"iters": 1,
|
||||||
|
"alpha": 0.4,
|
||||||
|
"disp_diff": 100,
|
||||||
|
"radius": 5
|
||||||
|
},
|
||||||
|
"HoleFillingFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "FILL_TOP"
|
||||||
|
},
|
||||||
|
"TemporalFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"fill": false,
|
||||||
|
"scale": 0.05,
|
||||||
|
"weight": 0.4
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"depthWorkMode": "Binned Sparse Default",
|
||||||
|
"width": 640,
|
||||||
|
"height":400,
|
||||||
|
"bxf": 15250,
|
||||||
|
"invalid_value":0,
|
||||||
|
"NoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"size": 50,
|
||||||
|
"disp_diff": 120,
|
||||||
|
"type": "NR_OVERALL",
|
||||||
|
"lut": [
|
||||||
|
50, 25, 25, 50,
|
||||||
|
50, 25, 25, 50,
|
||||||
|
50, 25, 25, 50,
|
||||||
|
50, 25, 25, 50
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"EdgeNoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"type": "MG_FILTER",
|
||||||
|
"x1_th": 3,
|
||||||
|
"x2_th": 3,
|
||||||
|
"y1_th": 3,
|
||||||
|
"y2_th": 3,
|
||||||
|
"limit_x": 35,
|
||||||
|
"limit_y": 35,
|
||||||
|
"R": 375,
|
||||||
|
"width1": 20,
|
||||||
|
"width2": 20
|
||||||
|
},
|
||||||
|
"SpatialFastFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 3
|
||||||
|
},
|
||||||
|
"SpatialModerateFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 3,
|
||||||
|
"iters": 1,
|
||||||
|
"disp_diff": 120
|
||||||
|
},
|
||||||
|
"SpatialAdvancedFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "SFA_ALL",
|
||||||
|
"iters": 1,
|
||||||
|
"alpha": 0.4,
|
||||||
|
"disp_diff": 120,
|
||||||
|
"radius": 5
|
||||||
|
},
|
||||||
|
"HoleFillingFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "FILL_TOP"
|
||||||
|
},
|
||||||
|
"TemporalFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"fill": false,
|
||||||
|
"scale": 0.05,
|
||||||
|
"weight": 0.4
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"depthWorkMode": "Unbinned Sparse Default",
|
||||||
|
"width": 1280,
|
||||||
|
"height": 800,
|
||||||
|
"bxf": 30500,
|
||||||
|
"invalid_value":0,
|
||||||
|
"NoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"size": 200,
|
||||||
|
"disp_diff": 100,
|
||||||
|
"type": "NR_OVERALL",
|
||||||
|
"lut": [
|
||||||
|
200, 100, 100, 200,
|
||||||
|
200, 100, 100, 200,
|
||||||
|
200, 100, 100, 200,
|
||||||
|
200, 100, 100, 200
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"EdgeNoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"type": "MG_FILTER",
|
||||||
|
"x1_th": 6,
|
||||||
|
"x2_th": 6,
|
||||||
|
"y1_th": 6,
|
||||||
|
"y2_th": 6,
|
||||||
|
"limit_x": 70,
|
||||||
|
"limit_y": 70,
|
||||||
|
"R": 750,
|
||||||
|
"width1": 40,
|
||||||
|
"width2": 40
|
||||||
|
},
|
||||||
|
"SpatialFastFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 3
|
||||||
|
},
|
||||||
|
"SpatialModerateFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 3,
|
||||||
|
"iters": 1,
|
||||||
|
"disp_diff": 100
|
||||||
|
},
|
||||||
|
"SpatialAdvancedFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "SFA_ALL",
|
||||||
|
"iters": 1,
|
||||||
|
"alpha": 0.4,
|
||||||
|
"disp_diff": 100,
|
||||||
|
"radius": 5
|
||||||
|
},
|
||||||
|
"HoleFillingFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "FILL_TOP"
|
||||||
|
},
|
||||||
|
"TemporalFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"fill": false,
|
||||||
|
"scale": 0.05,
|
||||||
|
"weight": 0.4
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"depthWorkMode": "Obstacle Avoidance",
|
||||||
|
"width": 640,
|
||||||
|
"height":400,
|
||||||
|
"bxf": 30500,
|
||||||
|
"invalid_value":0,
|
||||||
|
"NoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"size": 50,
|
||||||
|
"disp_diff": 100,
|
||||||
|
"type": "NR_OVERALL",
|
||||||
|
"lut": [
|
||||||
|
50, 25, 25, 50,
|
||||||
|
50, 25, 25, 50,
|
||||||
|
50, 25, 25, 50,
|
||||||
|
50, 25, 25, 50
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"EdgeNoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"type": "MG_FILTER",
|
||||||
|
"x1_th": 3,
|
||||||
|
"x2_th": 3,
|
||||||
|
"y1_th": 3,
|
||||||
|
"y2_th": 3,
|
||||||
|
"limit_x": 20,
|
||||||
|
"limit_y": 20,
|
||||||
|
"R": 375,
|
||||||
|
"width1": 20,
|
||||||
|
"width2": 20
|
||||||
|
},
|
||||||
|
"SpatialFastFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 5
|
||||||
|
},
|
||||||
|
"SpatialModerateFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 5,
|
||||||
|
"iters": 1,
|
||||||
|
"disp_diff": 100
|
||||||
|
},
|
||||||
|
"SpatialAdvancedFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"type": "SFA_ALL",
|
||||||
|
"iters": 1,
|
||||||
|
"alpha": 0.6,
|
||||||
|
"disp_diff": 100,
|
||||||
|
"radius": 5
|
||||||
|
},
|
||||||
|
"HoleFillingFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "FILL_TOP"
|
||||||
|
},
|
||||||
|
"TemporalFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"fill": false,
|
||||||
|
"scale": 0.05,
|
||||||
|
"weight": 0.4
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,71 @@
|
|||||||
|
{
|
||||||
|
"device": "Orbbec Openni deivce",
|
||||||
|
"structVersion": "0x0000000a",
|
||||||
|
"dataVersion": "1.5.0",
|
||||||
|
"description": "Orbbec openni depth filter params",
|
||||||
|
"configDate": "20231204",
|
||||||
|
"vid": "0x2bc5",
|
||||||
|
"pid": "0x0670",
|
||||||
|
"depthFilters": [
|
||||||
|
{
|
||||||
|
"depthWorkMode": "",
|
||||||
|
"width": 640,
|
||||||
|
"height":400,
|
||||||
|
"bxf": 30500,
|
||||||
|
"invalid_value":0,
|
||||||
|
"NoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"size": 50,
|
||||||
|
"disp_diff": 6,
|
||||||
|
"type": "NR_LUT",
|
||||||
|
"lut": [
|
||||||
|
100, 25, 25, 100,
|
||||||
|
100, 25, 25, 100,
|
||||||
|
100, 25, 25, 100,
|
||||||
|
100, 25, 25, 100
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"EdgeNoiseRemovalFilter": {
|
||||||
|
"enable": true,
|
||||||
|
"type": "MGC_FILTER",
|
||||||
|
"x1_th": 3,
|
||||||
|
"x2_th": 3,
|
||||||
|
"y1_th": 0,
|
||||||
|
"y2_th": 0,
|
||||||
|
"limit_x": 60,
|
||||||
|
"limit_y": 60,
|
||||||
|
"R": 320,
|
||||||
|
"width1": 40,
|
||||||
|
"width2": 40
|
||||||
|
},
|
||||||
|
"SpatialFastFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 3
|
||||||
|
},
|
||||||
|
"SpatialModerateFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"size": 3,
|
||||||
|
"iters": 1,
|
||||||
|
"disp_diff": 100
|
||||||
|
},
|
||||||
|
"SpatialAdvancedFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "SFA_ALL",
|
||||||
|
"iters": 1,
|
||||||
|
"alpha": 0.4,
|
||||||
|
"disp_diff": 100,
|
||||||
|
"radius": 5
|
||||||
|
},
|
||||||
|
"HoleFillingFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"type": "FILL_TOP"
|
||||||
|
},
|
||||||
|
"TemporalFilter": {
|
||||||
|
"enable": false,
|
||||||
|
"fill": false,
|
||||||
|
"scale": 0.05,
|
||||||
|
"weight": 0.4
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -23,7 +23,7 @@
|
|||||||
|
|
||||||
#define OB_ROS_MAJOR_VERSION 1
|
#define OB_ROS_MAJOR_VERSION 1
|
||||||
#define OB_ROS_MINOR_VERSION 4
|
#define OB_ROS_MINOR_VERSION 4
|
||||||
#define OB_ROS_PATCH_VERSION 0
|
#define OB_ROS_PATCH_VERSION 3
|
||||||
|
|
||||||
#ifndef STRINGIFY
|
#ifndef STRINGIFY
|
||||||
#define STRINGIFY(arg) #arg
|
#define STRINGIFY(arg) #arg
|
||||||
|
|||||||
@@ -262,7 +262,7 @@ class OBCameraNode {
|
|||||||
void switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request,
|
void switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request,
|
||||||
std::shared_ptr<SetString::Response>& response);
|
std::shared_ptr<SetString::Response>& response);
|
||||||
|
|
||||||
void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set, bool isColorPointCloud);
|
void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
|
||||||
|
|
||||||
void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
|
void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
|
||||||
|
|
||||||
@@ -284,6 +284,9 @@ class OBCameraNode {
|
|||||||
void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
|
void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
|
||||||
const sensor_msgs::msg::Image::SharedPtr& image_msg);
|
const sensor_msgs::msg::Image::SharedPtr& image_msg);
|
||||||
|
|
||||||
|
void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
|
||||||
|
const std::shared_ptr<ob::Frame>& gryoframe);
|
||||||
|
|
||||||
void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
|
void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
|
||||||
const stream_index_pair& stream_index);
|
const stream_index_pair& stream_index);
|
||||||
|
|
||||||
@@ -305,6 +308,7 @@ class OBCameraNode {
|
|||||||
rclcpp::Logger logger_;
|
rclcpp::Logger logger_;
|
||||||
std::atomic_bool is_running_{false};
|
std::atomic_bool is_running_{false};
|
||||||
std::unique_ptr<ob::Pipeline> pipeline_ = nullptr;
|
std::unique_ptr<ob::Pipeline> pipeline_ = nullptr;
|
||||||
|
std::unique_ptr<ob::Pipeline> imuPipeline_ = nullptr;
|
||||||
std::atomic_bool pipeline_started_{false};
|
std::atomic_bool pipeline_started_{false};
|
||||||
std::string camera_name_ = "camera";
|
std::string camera_name_ = "camera";
|
||||||
std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
|
std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
|
||||||
@@ -364,6 +368,7 @@ class OBCameraNode {
|
|||||||
rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
|
rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
|
||||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
|
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
|
||||||
|
|
||||||
|
bool enable_sync_output_accel_gyro_ = false;
|
||||||
bool publish_tf_ = false;
|
bool publish_tf_ = false;
|
||||||
bool tf_published_ = false;
|
bool tf_published_ = false;
|
||||||
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_ = nullptr;
|
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_ = nullptr;
|
||||||
@@ -393,6 +398,8 @@ class OBCameraNode {
|
|||||||
std::atomic_bool save_colored_point_cloud_{false};
|
std::atomic_bool save_colored_point_cloud_{false};
|
||||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_images_srv_;
|
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_images_srv_;
|
||||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_point_cloud_srv_;
|
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_point_cloud_srv_;
|
||||||
|
std::string depth_filter_config_;
|
||||||
|
bool enable_depth_filter_ = false;
|
||||||
bool enable_soft_filter_ = true;
|
bool enable_soft_filter_ = true;
|
||||||
bool enable_color_auto_exposure_ = true;
|
bool enable_color_auto_exposure_ = true;
|
||||||
bool enable_ir_auto_exposure_ = true;
|
bool enable_ir_auto_exposure_ = true;
|
||||||
@@ -414,6 +421,8 @@ class OBCameraNode {
|
|||||||
OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8;
|
OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8;
|
||||||
// IMU
|
// IMU
|
||||||
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> imu_publishers_;
|
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> imu_publishers_;
|
||||||
|
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_gyro_accel_publisher_;
|
||||||
|
bool imu_sync_output_start_ = false;
|
||||||
std::map<stream_index_pair, std::string> imu_rate_;
|
std::map<stream_index_pair, std::string> imu_rate_;
|
||||||
std::map<stream_index_pair, std::string> imu_range_;
|
std::map<stream_index_pair, std::string> imu_range_;
|
||||||
std::map<stream_index_pair, std::string> imu_qos_;
|
std::map<stream_index_pair, std::string> imu_qos_;
|
||||||
@@ -433,5 +442,6 @@ class OBCameraNode {
|
|||||||
std::mutex colorFrameMtx_;
|
std::mutex colorFrameMtx_;
|
||||||
std::condition_variable colorFrameCV_;
|
std::condition_variable colorFrameCV_;
|
||||||
|
|
||||||
|
bool ordered_pc_ = false;
|
||||||
};
|
};
|
||||||
} // namespace orbbec_camera
|
} // namespace orbbec_camera
|
||||||
|
|||||||
@@ -61,6 +61,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -69,6 +69,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -61,6 +61,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -61,6 +61,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -0,0 +1,115 @@
|
|||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch_ros.actions import PushRosNamespace
|
||||||
|
from launch.actions import GroupAction
|
||||||
|
from launch_ros.actions import ComposableNodeContainer
|
||||||
|
from launch_ros.descriptions import ComposableNode
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
import os
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
# Declare arguments
|
||||||
|
args = [
|
||||||
|
DeclareLaunchArgument('camera_name', default_value='camera'),
|
||||||
|
DeclareLaunchArgument('depth_registration', default_value='false'),
|
||||||
|
DeclareLaunchArgument('serial_number', default_value=''),
|
||||||
|
DeclareLaunchArgument('usb_port', default_value=''),
|
||||||
|
DeclareLaunchArgument('device_num', default_value='1'),
|
||||||
|
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
|
||||||
|
DeclareLaunchArgument('product_id', default_value=''),
|
||||||
|
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
|
||||||
|
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
|
||||||
|
DeclareLaunchArgument('connection_delay', default_value='100'),
|
||||||
|
DeclareLaunchArgument('color_width', default_value='640'),
|
||||||
|
DeclareLaunchArgument('color_height', default_value='480'),
|
||||||
|
DeclareLaunchArgument('color_fps', default_value='10'),
|
||||||
|
DeclareLaunchArgument('color_format', default_value='UYVY'),
|
||||||
|
DeclareLaunchArgument('enable_color', default_value='true'),
|
||||||
|
DeclareLaunchArgument('flip_color', default_value='false'),
|
||||||
|
DeclareLaunchArgument('color_qos', default_value='default'),
|
||||||
|
DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
|
||||||
|
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('depth_width', default_value='640'),
|
||||||
|
DeclareLaunchArgument('depth_height', default_value='480'),
|
||||||
|
DeclareLaunchArgument('depth_fps', default_value='10'),
|
||||||
|
DeclareLaunchArgument('depth_format', default_value='Y11'),
|
||||||
|
DeclareLaunchArgument('enable_depth', default_value='true'),
|
||||||
|
DeclareLaunchArgument('flip_depth', default_value='false'),
|
||||||
|
DeclareLaunchArgument('depth_qos', default_value='default'),
|
||||||
|
DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
|
||||||
|
DeclareLaunchArgument('ir_width', default_value='640'),
|
||||||
|
DeclareLaunchArgument('ir_height', default_value='480'),
|
||||||
|
DeclareLaunchArgument('ir_fps', default_value='10'),
|
||||||
|
DeclareLaunchArgument('ir_format', default_value='Y10'),
|
||||||
|
DeclareLaunchArgument('enable_ir', default_value='true'),
|
||||||
|
DeclareLaunchArgument('flip_ir', default_value='false'),
|
||||||
|
DeclareLaunchArgument('ir_qos', default_value='default'),
|
||||||
|
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
||||||
|
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('publish_tf', default_value='true'),
|
||||||
|
DeclareLaunchArgument('tf_publish_rate', default_value='10.0'),
|
||||||
|
DeclareLaunchArgument('ir_info_url', default_value=''),
|
||||||
|
DeclareLaunchArgument('color_info_url', default_value=''),
|
||||||
|
DeclareLaunchArgument('log_level', default_value='none'),
|
||||||
|
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
|
||||||
|
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
|
||||||
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_ldp', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
|
]
|
||||||
|
|
||||||
|
# Node configuration
|
||||||
|
parameters = [{arg.name: LaunchConfiguration(arg.name)} for arg in args]
|
||||||
|
# get ROS_DISTRO
|
||||||
|
ros_distro = os.environ["ROS_DISTRO"]
|
||||||
|
if ros_distro == "foxy":
|
||||||
|
return LaunchDescription(
|
||||||
|
args
|
||||||
|
+ [
|
||||||
|
Node(
|
||||||
|
package="orbbec_camera",
|
||||||
|
executable="orbbec_camera_node",
|
||||||
|
name="ob_camera_node",
|
||||||
|
namespace=LaunchConfiguration("camera_name"),
|
||||||
|
parameters=parameters,
|
||||||
|
output="screen",
|
||||||
|
)
|
||||||
|
]
|
||||||
|
)
|
||||||
|
# Define the ComposableNode
|
||||||
|
else:
|
||||||
|
# Define the ComposableNode
|
||||||
|
compose_node = ComposableNode(
|
||||||
|
package="orbbec_camera",
|
||||||
|
plugin="orbbec_camera::OBCameraNodeDriver",
|
||||||
|
name=LaunchConfiguration("camera_name"),
|
||||||
|
namespace="",
|
||||||
|
parameters=parameters,
|
||||||
|
)
|
||||||
|
# Define the ComposableNodeContainer
|
||||||
|
container = ComposableNodeContainer(
|
||||||
|
name="camera_container",
|
||||||
|
namespace="",
|
||||||
|
package="rclcpp_components",
|
||||||
|
executable="component_container",
|
||||||
|
composable_node_descriptions=[
|
||||||
|
compose_node,
|
||||||
|
],
|
||||||
|
output="screen",
|
||||||
|
)
|
||||||
|
# Launch description
|
||||||
|
ld = LaunchDescription(
|
||||||
|
args
|
||||||
|
+ [
|
||||||
|
GroupAction(
|
||||||
|
[PushRosNamespace(LaunchConfiguration("camera_name")), container]
|
||||||
|
)
|
||||||
|
]
|
||||||
|
)
|
||||||
|
return ld
|
||||||
@@ -61,6 +61,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -61,6 +61,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -60,7 +60,8 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_ldp', default_value='true'),
|
DeclareLaunchArgument('enable_ldp', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1')
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -60,7 +60,8 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_ldp', default_value='true'),
|
DeclareLaunchArgument('enable_ldp', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1')
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,8 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'),
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
DeclareLaunchArgument('video_convert', default_value='videoconvert'),
|
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -60,6 +60,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('ir_qos', default_value='default'),
|
DeclareLaunchArgument('ir_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_accel', default_value='false'),
|
DeclareLaunchArgument('enable_accel', default_value='false'),
|
||||||
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
||||||
DeclareLaunchArgument('accel_range', default_value='4g'),
|
DeclareLaunchArgument('accel_range', default_value='4g'),
|
||||||
@@ -70,6 +71,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
DeclareLaunchArgument('enable_frame_sync', default_value='false'),
|
DeclareLaunchArgument('enable_frame_sync', default_value='false'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('ir_qos', default_value='default'),
|
DeclareLaunchArgument('ir_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_accel', default_value='false'),
|
DeclareLaunchArgument('enable_accel', default_value='false'),
|
||||||
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
||||||
DeclareLaunchArgument('accel_range', default_value='4g'),
|
DeclareLaunchArgument('accel_range', default_value='4g'),
|
||||||
@@ -75,6 +76,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
|
DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
|
||||||
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
||||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('ir_qos', default_value='default'),
|
DeclareLaunchArgument('ir_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_accel', default_value='false'),
|
DeclareLaunchArgument('enable_accel', default_value='false'),
|
||||||
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
||||||
DeclareLaunchArgument('accel_range', default_value='4g'),
|
DeclareLaunchArgument('accel_range', default_value='4g'),
|
||||||
@@ -77,6 +78,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
||||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('ir_qos', default_value='default'),
|
DeclareLaunchArgument('ir_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_accel', default_value='false'),
|
DeclareLaunchArgument('enable_accel', default_value='false'),
|
||||||
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
||||||
DeclareLaunchArgument('accel_range', default_value='4g'),
|
DeclareLaunchArgument('accel_range', default_value='4g'),
|
||||||
@@ -64,15 +65,14 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('log_level', default_value='none'),
|
DeclareLaunchArgument('log_level', default_value='none'),
|
||||||
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
|
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
|
||||||
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
|
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
|
||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
|
||||||
DeclareLaunchArgument('enable_ldp', default_value='true'),
|
DeclareLaunchArgument('enable_ldp', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
# Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('depth_filter_config', default_value=''),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
|
||||||
# Depth work mode support is as follows:
|
# Depth work mode support is as follows:
|
||||||
# Unbinned Dense Default
|
# Unbinned Dense Default
|
||||||
# Unbinned Sparse Default
|
# Unbinned Sparse Default
|
||||||
# Binned Sparse Default
|
# Binned Sparse Default
|
||||||
|
# Obstacle Avoidance
|
||||||
DeclareLaunchArgument('depth_work_mode', default_value=''),
|
DeclareLaunchArgument('depth_work_mode', default_value=''),
|
||||||
DeclareLaunchArgument('sync_mode', default_value='free_run'),
|
DeclareLaunchArgument('sync_mode', default_value='free_run'),
|
||||||
DeclareLaunchArgument('depth_delay_us', default_value='0'),
|
DeclareLaunchArgument('depth_delay_us', default_value='0'),
|
||||||
@@ -81,6 +81,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
||||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('ir_qos', default_value='default'),
|
DeclareLaunchArgument('ir_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_accel', default_value='false'),
|
DeclareLaunchArgument('enable_accel', default_value='false'),
|
||||||
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
||||||
DeclareLaunchArgument('accel_range', default_value='4g'),
|
DeclareLaunchArgument('accel_range', default_value='4g'),
|
||||||
@@ -81,6 +82,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
||||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -47,6 +47,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('right_ir_qos', default_value='default'),
|
DeclareLaunchArgument('right_ir_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
|
DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_accel', default_value='true'),
|
DeclareLaunchArgument('enable_accel', default_value='true'),
|
||||||
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
||||||
DeclareLaunchArgument('accel_range', default_value='4g'),
|
DeclareLaunchArgument('accel_range', default_value='4g'),
|
||||||
@@ -74,6 +75,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
||||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -56,6 +56,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('right_ir_qos', default_value='default'),
|
DeclareLaunchArgument('right_ir_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
|
DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
|
||||||
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||||
|
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
|
||||||
DeclareLaunchArgument('enable_accel', default_value='false'),
|
DeclareLaunchArgument('enable_accel', default_value='false'),
|
||||||
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
DeclareLaunchArgument('accel_rate', default_value='100hz'),
|
||||||
DeclareLaunchArgument('accel_range', default_value='4g'),
|
DeclareLaunchArgument('accel_range', default_value='4g'),
|
||||||
@@ -90,6 +91,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
||||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -61,6 +61,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -50,6 +50,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||||
|
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||||
]
|
]
|
||||||
|
|
||||||
# Node configuration
|
# Node configuration
|
||||||
|
|||||||
@@ -2,7 +2,7 @@
|
|||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
<package format="3">
|
<package format="3">
|
||||||
<name>orbbec_camera</name>
|
<name>orbbec_camera</name>
|
||||||
<version>1.4.0</version>
|
<version>1.4.3</version>
|
||||||
<description>Orbbec Camera package</description>
|
<description>Orbbec Camera package</description>
|
||||||
<maintainer email="[email protected]">Joe Dong</maintainer>
|
<maintainer email="[email protected]">Joe Dong</maintainer>
|
||||||
<license>TODO: License declaration</license>
|
<license>TODO: License declaration</license>
|
||||||
|
|||||||
@@ -92,6 +92,9 @@ SUBSYSTEM=="usb", ATTR{idProduct}=="069e", ATTR{idVendor}=="2bc5", MODE:="0666",
|
|||||||
SUBSYSTEM=="usb", ATTR{idProduct}=="0560", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_max_pro_rgb"
|
SUBSYSTEM=="usb", ATTR{idProduct}=="0560", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_max_pro_rgb"
|
||||||
SUBSYSTEM=="usb", ATTR{idProduct}=="06aa", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_gemini_uw"
|
SUBSYSTEM=="usb", ATTR{idProduct}=="06aa", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_gemini_uw"
|
||||||
SUBSYSTEM=="usb", ATTR{idProduct}=="05aa", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_gemini_uw_rgb"
|
SUBSYSTEM=="usb", ATTR{idProduct}=="05aa", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_gemini_uw_rgb"
|
||||||
|
SUBSYSTEM=="usb", ATTR{idProduct}=="06a6", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="gemini_ew"
|
||||||
|
SUBSYSTEM=="usb", ATTR{idProduct}=="05a6", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="gemini_ew_rgb"
|
||||||
|
SUBSYSTEM=="usb", ATTR{idProduct}=="06a7", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="gemini_ew_lite"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -133,6 +133,13 @@ void OBCameraNode::setupDevices() {
|
|||||||
auto info = device_->getDeviceInfo();
|
auto info = device_->getDeviceInfo();
|
||||||
if (enable_hardware_d2d_ && info->pid() == GEMINI2_PID) {
|
if (enable_hardware_d2d_ && info->pid() == GEMINI2_PID) {
|
||||||
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, true);
|
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, true);
|
||||||
|
bool isHWD2D = device_->getBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL);
|
||||||
|
if(isHWD2D == false) {
|
||||||
|
RCLCPP_INFO_STREAM(logger_, "Depth process is soft D2D.");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
RCLCPP_INFO_STREAM(logger_, "Depth process is HW D2D.");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
try {
|
try {
|
||||||
if (!depth_work_mode_.empty()) {
|
if (!depth_work_mode_.empty()) {
|
||||||
@@ -153,6 +160,9 @@ void OBCameraNode::setupDevices() {
|
|||||||
if (default_precision_level != depth_precision_) {
|
if (default_precision_level != depth_precision_) {
|
||||||
device_->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
|
device_->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int32_t precisionLevel = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
|
||||||
|
RCLCPP_INFO_STREAM(logger_, "Depth precision level:" << precisionLevel);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (const auto &stream_index : IMAGE_STREAMS) {
|
for (const auto &stream_index : IMAGE_STREAMS) {
|
||||||
@@ -178,18 +188,37 @@ void OBCameraNode::setupDevices() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
|
if (!depth_filter_config_.empty() && enable_depth_filter_) {
|
||||||
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
|
RCLCPP_INFO_STREAM(logger_, "Load depth filter config: " << depth_filter_config_);
|
||||||
device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
|
device_->loadDepthFilterConfig(depth_filter_config_.c_str());
|
||||||
auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
|
} else {
|
||||||
if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
|
if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
|
||||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
|
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
auto default_soft_filter_speckle_size =
|
|
||||||
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
|
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||||
if (soft_filter_speckle_size_ != -1 &&
|
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
|
||||||
default_soft_filter_speckle_size != soft_filter_speckle_size_) {
|
}
|
||||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
|
|
||||||
|
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||||
|
device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
|
||||||
|
auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
|
||||||
|
if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
|
||||||
|
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE)) {
|
||||||
|
auto default_soft_filter_speckle_size =
|
||||||
|
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
|
||||||
|
if (soft_filter_speckle_size_ != -1 &&
|
||||||
|
default_soft_filter_speckle_size != soft_filter_speckle_size_) {
|
||||||
|
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
} catch (const ob::Error &e) {
|
} catch (const ob::Error &e) {
|
||||||
RCLCPP_ERROR_STREAM(logger_, "Failed to setup devices: " << e.getMessage());
|
RCLCPP_ERROR_STREAM(logger_, "Failed to setup devices: " << e.getMessage());
|
||||||
@@ -286,7 +315,7 @@ void OBCameraNode::startStreams() {
|
|||||||
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline");
|
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline");
|
||||||
throw std::runtime_error("Failed to start pipeline");
|
throw std::runtime_error("Failed to start pipeline");
|
||||||
}
|
}
|
||||||
if (enable_stream_[COLOR]) {
|
if (enable_stream_[COLOR] && !colorFrameThread_) {
|
||||||
colorFrameThread_ = std::make_shared<std::thread>([this]() { onNewColorFrameCallback(); });
|
colorFrameThread_ = std::make_shared<std::thread>([this]() { onNewColorFrameCallback(); });
|
||||||
}
|
}
|
||||||
if (enable_frame_sync_) {
|
if (enable_frame_sync_) {
|
||||||
@@ -296,49 +325,94 @@ void OBCameraNode::startStreams() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void OBCameraNode::startIMU() {
|
void OBCameraNode::startIMU() {
|
||||||
for (const auto &stream_index : HID_STREAMS) {
|
if (enable_sync_output_accel_gyro_) {
|
||||||
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
|
if (imuPipeline_ != nullptr) {
|
||||||
CHECK(sensors_.count(stream_index));
|
imuPipeline_.reset();
|
||||||
auto profile_list = sensors_[stream_index]->getStreamProfileList();
|
}
|
||||||
for (size_t i = 0; i < profile_list->count(); i++) {
|
|
||||||
auto item = profile_list->getProfile(i);
|
imuPipeline_ = std::make_unique<ob::Pipeline>(device_);
|
||||||
if (stream_index == ACCEL) {
|
if (imu_sync_output_start_){
|
||||||
auto profile = item->as<ob::AccelStreamProfile>();
|
return;
|
||||||
auto accel_rate = sampleRateFromString(imu_rate_[stream_index]);
|
}
|
||||||
auto accel_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
|
|
||||||
if (profile->fullScaleRange() == accel_range && profile->sampleRate() == accel_rate) {
|
//ACCEL
|
||||||
sensors_[stream_index]->start(
|
auto accelProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_ACCEL);
|
||||||
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
|
auto accel_range = fullAccelScaleRangeFromString(imu_range_[ACCEL]);
|
||||||
onNewIMUFrameCallback(frame, stream_index);
|
auto accel_rate = sampleRateFromString(imu_rate_[ACCEL]);
|
||||||
});
|
auto accelProfile = accelProfiles->getAccelStreamProfile(accel_range, accel_rate);
|
||||||
imu_started_[stream_index] = true;
|
//GYRO
|
||||||
RCLCPP_INFO_STREAM(logger_, "start accel stream with "
|
auto gyroProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_GYRO);
|
||||||
<< magic_enum::enum_name(accel_range) << " range and "
|
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[GYRO]);
|
||||||
<< magic_enum::enum_name(accel_rate) << " rate");
|
auto gyro_rate = sampleRateFromString(imu_rate_[GYRO]);
|
||||||
}
|
auto gyroProfile = gyroProfiles->getGyroStreamProfile(gyro_range, gyro_rate);
|
||||||
} else if (stream_index == GYRO) {
|
std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
|
||||||
auto profile = item->as<ob::GyroStreamProfile>();
|
imuConfig->enableStream(accelProfile);
|
||||||
auto gyro_rate = sampleRateFromString(imu_rate_[stream_index]);
|
imuConfig->enableStream(gyroProfile);
|
||||||
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
|
imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame){
|
||||||
if (profile->fullScaleRange() == gyro_range && profile->sampleRate() == gyro_rate) {
|
auto frameSet = frame->as<ob::FrameSet>();
|
||||||
sensors_[stream_index]->start(
|
auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
|
||||||
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
|
auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
|
||||||
onNewIMUFrameCallback(frame, stream_index);
|
onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
|
||||||
});
|
});
|
||||||
RCLCPP_INFO_STREAM(logger_, "start gyro stream with "
|
|
||||||
<< magic_enum::enum_name(gyro_range) << " range and "
|
imu_sync_output_start_ = true;
|
||||||
<< magic_enum::enum_name(gyro_rate) << " rate");
|
if (!imu_sync_output_start_) {
|
||||||
imu_started_[stream_index] = true;
|
RCLCPP_ERROR_STREAM(
|
||||||
|
logger_,
|
||||||
|
"Failed to start IMU stream, please check the imu_rate and imu_range parameters.");
|
||||||
|
} else {
|
||||||
|
RCLCPP_INFO_STREAM(
|
||||||
|
logger_, "start accel stream with range: " << fullAccelScaleRangeToString(accel_range)
|
||||||
|
<< ",rate:" << sampleRateToString(accel_rate)
|
||||||
|
<< ", and start gyro stream with range:"
|
||||||
|
<< fullGyroScaleRangeToString(gyro_range)
|
||||||
|
<< ",rate:" << sampleRateToString(gyro_rate));
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
for (const auto &stream_index : HID_STREAMS) {
|
||||||
|
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
|
||||||
|
CHECK(sensors_.count(stream_index));
|
||||||
|
auto profile_list = sensors_[stream_index]->getStreamProfileList();
|
||||||
|
for (size_t i = 0; i < profile_list->count(); i++) {
|
||||||
|
auto item = profile_list->getProfile(i);
|
||||||
|
if (stream_index == ACCEL) {
|
||||||
|
auto profile = item->as<ob::AccelStreamProfile>();
|
||||||
|
auto accel_rate = sampleRateFromString(imu_rate_[stream_index]);
|
||||||
|
auto accel_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
|
||||||
|
if (profile->fullScaleRange() == accel_range && profile->sampleRate() == accel_rate) {
|
||||||
|
sensors_[stream_index]->start(
|
||||||
|
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
|
||||||
|
onNewIMUFrameCallback(frame, stream_index);
|
||||||
|
});
|
||||||
|
imu_started_[stream_index] = true;
|
||||||
|
RCLCPP_INFO_STREAM(logger_, "start accel stream with "
|
||||||
|
<< magic_enum::enum_name(accel_range) << " range and "
|
||||||
|
<< magic_enum::enum_name(accel_rate) << " rate");
|
||||||
|
}
|
||||||
|
} else if (stream_index == GYRO) {
|
||||||
|
auto profile = item->as<ob::GyroStreamProfile>();
|
||||||
|
auto gyro_rate = sampleRateFromString(imu_rate_[stream_index]);
|
||||||
|
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
|
||||||
|
if (profile->fullScaleRange() == gyro_range && profile->sampleRate() == gyro_rate) {
|
||||||
|
sensors_[stream_index]->start(
|
||||||
|
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
|
||||||
|
onNewIMUFrameCallback(frame, stream_index);
|
||||||
|
});
|
||||||
|
RCLCPP_INFO_STREAM(logger_, "start gyro stream with "
|
||||||
|
<< magic_enum::enum_name(gyro_range) << " range and "
|
||||||
|
<< magic_enum::enum_name(gyro_rate) << " rate");
|
||||||
|
imu_started_[stream_index] = true;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
for (const auto &stream_index : HID_STREAMS) {
|
||||||
for (const auto &stream_index : HID_STREAMS) {
|
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
|
||||||
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
|
RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU stream: "
|
||||||
RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU stream: "
|
<< magic_enum::enum_name(stream_index.first)
|
||||||
<< magic_enum::enum_name(stream_index.first)
|
<< ", please check the imu_rate and imu_range parameters");
|
||||||
<< ", please check the imu_rate and imu_range parameters");
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -355,12 +429,23 @@ void OBCameraNode::stopStreams() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void OBCameraNode::stopIMU() {
|
void OBCameraNode::stopIMU() {
|
||||||
for (const auto &stream_index : HID_STREAMS) {
|
if (enable_sync_output_accel_gyro_) {
|
||||||
if (imu_started_[stream_index]) {
|
if (!imu_sync_output_start_ || !imuPipeline_) {
|
||||||
CHECK(sensors_.count(stream_index));
|
return;
|
||||||
RCLCPP_INFO_STREAM(logger_, "stop " << stream_name_[stream_index] << " stream");
|
}
|
||||||
sensors_[stream_index]->stop();
|
try {
|
||||||
imu_started_[stream_index] = false;
|
imuPipeline_->stop();
|
||||||
|
} catch (const ob::Error &e) {
|
||||||
|
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline: " << e.getMessage());
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
for (const auto &stream_index : HID_STREAMS) {
|
||||||
|
if (imu_started_[stream_index]) {
|
||||||
|
CHECK(sensors_.count(stream_index));
|
||||||
|
RCLCPP_INFO_STREAM(logger_, "stop " << stream_name_[stream_index] << " stream");
|
||||||
|
sensors_[stream_index]->stop();
|
||||||
|
imu_started_[stream_index] = false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -446,11 +531,15 @@ void OBCameraNode::getParameters() {
|
|||||||
depth_aligned_frame_id_[stream_index] = optical_frame_id_[COLOR];
|
depth_aligned_frame_id_[stream_index] = optical_frame_id_[COLOR];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
setAndGetNodeParameter(enable_sync_output_accel_gyro_, "enable_sync_output_accel_gyro", false);
|
||||||
for (const auto &stream_index : HID_STREAMS) {
|
for (const auto &stream_index : HID_STREAMS) {
|
||||||
std::string param_name = stream_name_[stream_index] + "_qos";
|
std::string param_name = stream_name_[stream_index] + "_qos";
|
||||||
setAndGetNodeParameter<std::string>(imu_qos_[stream_index], param_name, "default");
|
setAndGetNodeParameter<std::string>(imu_qos_[stream_index], param_name, "default");
|
||||||
param_name = "enable_" + stream_name_[stream_index];
|
param_name = "enable_" + stream_name_[stream_index];
|
||||||
setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
|
setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
|
||||||
|
if(enable_sync_output_accel_gyro_) {
|
||||||
|
enable_stream_[stream_index] = true;
|
||||||
|
}
|
||||||
param_name = stream_name_[stream_index] + "_rate";
|
param_name = stream_name_[stream_index] + "_rate";
|
||||||
setAndGetNodeParameter<std::string>(imu_rate_[stream_index], param_name, "");
|
setAndGetNodeParameter<std::string>(imu_rate_[stream_index], param_name, "");
|
||||||
param_name = stream_name_[stream_index] + "_range";
|
param_name = stream_name_[stream_index] + "_range";
|
||||||
@@ -462,7 +551,7 @@ void OBCameraNode::getParameters() {
|
|||||||
camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
|
camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
|
||||||
param_name = stream_name_[stream_index] + "_optical_frame_id";
|
param_name = stream_name_[stream_index] + "_optical_frame_id";
|
||||||
setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
|
setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
|
||||||
depth_aligned_frame_id_[stream_index] = stream_name_[COLOR] + "_optical_frame";
|
depth_aligned_frame_id_[stream_index] = camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame";
|
||||||
}
|
}
|
||||||
|
|
||||||
setAndGetNodeParameter(publish_tf_, "publish_tf", true);
|
setAndGetNodeParameter(publish_tf_, "publish_tf", true);
|
||||||
@@ -477,6 +566,10 @@ void OBCameraNode::getParameters() {
|
|||||||
setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false);
|
setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false);
|
||||||
setAndGetNodeParameter(enable_hardware_d2d_, "enable_hardware_d2d", true);
|
setAndGetNodeParameter(enable_hardware_d2d_, "enable_hardware_d2d", true);
|
||||||
setAndGetNodeParameter(enable_soft_filter_, "enable_soft_filter", true);
|
setAndGetNodeParameter(enable_soft_filter_, "enable_soft_filter", true);
|
||||||
|
setAndGetNodeParameter<std::string>(depth_filter_config_, "depth_filter_config", "");
|
||||||
|
if (!depth_filter_config_.empty()) {
|
||||||
|
enable_depth_filter_ = true;
|
||||||
|
}
|
||||||
setAndGetNodeParameter(enable_frame_sync_, "enable_frame_sync", false);
|
setAndGetNodeParameter(enable_frame_sync_, "enable_frame_sync", false);
|
||||||
setAndGetNodeParameter(enable_color_auto_exposure_, "enable_color_auto_exposure", true);
|
setAndGetNodeParameter(enable_color_auto_exposure_, "enable_color_auto_exposure", true);
|
||||||
setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
|
setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
|
||||||
@@ -499,6 +592,7 @@ void OBCameraNode::getParameters() {
|
|||||||
setAndGetNodeParameter<int>(soft_filter_speckle_size_, "soft_filter_speckle_size", -1);
|
setAndGetNodeParameter<int>(soft_filter_speckle_size_, "soft_filter_speckle_size", -1);
|
||||||
setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
|
setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
|
||||||
setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
|
setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
|
||||||
|
setAndGetNodeParameter<bool>(ordered_pc_, "ordered_pc", false);
|
||||||
}
|
}
|
||||||
|
|
||||||
void OBCameraNode::setupTopics() {
|
void OBCameraNode::setupTopics() {
|
||||||
@@ -568,31 +662,35 @@ void OBCameraNode::setupPublishers() {
|
|||||||
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
|
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
|
||||||
camera_info_qos_profile));
|
camera_info_qos_profile));
|
||||||
}
|
}
|
||||||
for (const auto &stream_index : HID_STREAMS) {
|
|
||||||
if (!enable_stream_[stream_index]) {
|
if (enable_sync_output_accel_gyro_) {
|
||||||
continue;
|
std::string data_topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
|
||||||
}
|
auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]);
|
||||||
std::string data_topic_name = stream_name_[stream_index] + "/sample";
|
imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
|
||||||
auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
|
|
||||||
imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
|
|
||||||
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
|
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
|
||||||
|
} else {
|
||||||
|
for (const auto &stream_index : HID_STREAMS) {
|
||||||
|
if (!enable_stream_[stream_index]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
std::string data_topic_name = stream_name_[stream_index] + "/sample";
|
||||||
|
auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
|
||||||
|
imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
|
||||||
|
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set, bool isColorPointCloud) {
|
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
|
||||||
try {
|
try {
|
||||||
if (isColorPointCloud) {
|
if (depth_registration_ || enable_colored_point_cloud_) {
|
||||||
if (depth_registration_ || enable_colored_point_cloud_) {
|
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
|
||||||
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
|
publishColoredPointCloud(frame_set);
|
||||||
publishColoredPointCloud(frame_set);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!isColorPointCloud) {
|
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
|
||||||
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
|
publishDepthPointCloud(frame_set);
|
||||||
publishDepthPointCloud(frame_set);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
} catch (const ob::Error &e) {
|
} catch (const ob::Error &e) {
|
||||||
RCLCPP_ERROR_STREAM(logger_, e.getMessage());
|
RCLCPP_ERROR_STREAM(logger_, e.getMessage());
|
||||||
@@ -608,10 +706,8 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
|
|||||||
depth_cloud_pub_->get_subscription_count() == 0) {
|
depth_cloud_pub_->get_subscription_count() == 0) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (!camera_param_ && depth_registration_) {
|
if (!camera_param_) {
|
||||||
camera_param_ = pipeline_->getCameraParam();
|
camera_param_ = pipeline_->getCameraParam();
|
||||||
} else if (!camera_param_) {
|
|
||||||
camera_param_ = getDepthCameraParam();
|
|
||||||
}
|
}
|
||||||
if (!camera_param_) {
|
if (!camera_param_) {
|
||||||
RCLCPP_ERROR_STREAM(logger_, "camera_param_ is null");
|
RCLCPP_ERROR_STREAM(logger_, "camera_param_ is null");
|
||||||
@@ -642,6 +738,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
|
|||||||
modifier.resize(width * height);
|
modifier.resize(width * height);
|
||||||
point_cloud_msg_.width = depth_frame->width();
|
point_cloud_msg_.width = depth_frame->width();
|
||||||
point_cloud_msg_.height = depth_frame->height();
|
point_cloud_msg_.height = depth_frame->height();
|
||||||
|
point_cloud_msg_.is_dense = false;
|
||||||
point_cloud_msg_.row_step = point_cloud_msg_.width * 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);
|
point_cloud_msg_.data.resize(point_cloud_msg_.height * point_cloud_msg_.row_step);
|
||||||
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
|
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
|
||||||
@@ -655,29 +752,34 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
|
|||||||
const static float max_depth = MAX_DISTANCE / depth_scale;
|
const static float max_depth = MAX_DISTANCE / depth_scale;
|
||||||
for (uint32_t y = 0; y < height; y++) {
|
for (uint32_t y = 0; y < height; y++) {
|
||||||
for (uint32_t x = 0; x < width; x++) {
|
for (uint32_t x = 0; x < width; x++) {
|
||||||
|
bool vaild_point = true;
|
||||||
if (depth_data[y * width + x] < min_depth || depth_data[y * width + x] > max_depth) {
|
if (depth_data[y * width + x] < min_depth || depth_data[y * width + x] > max_depth) {
|
||||||
continue;
|
vaild_point = false;
|
||||||
|
}
|
||||||
|
if(vaild_point || ordered_pc_) {
|
||||||
|
float xf = (x - u0) * fdx;
|
||||||
|
float yf = (y - v0) * fdy;
|
||||||
|
float zf = depth_data[y * width + x] * depth_scale;
|
||||||
|
*iter_x = zf * xf / 1000.0;
|
||||||
|
*iter_y = zf * yf / 1000.0;
|
||||||
|
*iter_z = zf / 1000.0;
|
||||||
|
++iter_x, ++iter_y, ++iter_z;
|
||||||
|
valid_count++;
|
||||||
}
|
}
|
||||||
float xf = (x - u0) * fdx;
|
|
||||||
float yf = (y - v0) * fdy;
|
|
||||||
float zf = depth_data[y * width + x] * depth_scale;
|
|
||||||
*iter_x = zf * xf / 1000.0;
|
|
||||||
*iter_y = zf * yf / 1000.0;
|
|
||||||
*iter_z = zf / 1000.0;
|
|
||||||
++iter_x, ++iter_y, ++iter_z;
|
|
||||||
valid_count++;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
|
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
|
||||||
|
if(!ordered_pc_){
|
||||||
|
point_cloud_msg_.is_dense = true;
|
||||||
|
point_cloud_msg_.width = valid_count;
|
||||||
|
point_cloud_msg_.height = 1;
|
||||||
|
modifier.resize(valid_count);
|
||||||
|
}
|
||||||
|
|
||||||
std::string frame_id =
|
std::string frame_id =
|
||||||
depth_registration_ ? depth_aligned_frame_id_[COLOR] : optical_frame_id_[DEPTH];
|
depth_registration_ ? depth_aligned_frame_id_[COLOR] : optical_frame_id_[DEPTH];
|
||||||
point_cloud_msg_.header.stamp = timestamp;
|
point_cloud_msg_.header.stamp = timestamp;
|
||||||
point_cloud_msg_.header.frame_id = frame_id;
|
point_cloud_msg_.header.frame_id = frame_id;
|
||||||
point_cloud_msg_.is_dense = true;
|
|
||||||
point_cloud_msg_.width = valid_count;
|
|
||||||
point_cloud_msg_.height = 1;
|
|
||||||
modifier.resize(valid_count);
|
|
||||||
depth_cloud_pub_->publish(point_cloud_msg_);
|
depth_cloud_pub_->publish(point_cloud_msg_);
|
||||||
|
|
||||||
if (save_point_cloud_) {
|
if (save_point_cloud_) {
|
||||||
@@ -740,6 +842,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
|
|||||||
modifier.resize(color_width * color_height);
|
modifier.resize(color_width * color_height);
|
||||||
point_cloud_msg_.width = color_frame->width();
|
point_cloud_msg_.width = color_frame->width();
|
||||||
point_cloud_msg_.height = color_frame->height();
|
point_cloud_msg_.height = color_frame->height();
|
||||||
|
point_cloud_msg_.is_dense = false;
|
||||||
std::string format_str = "rgb";
|
std::string format_str = "rgb";
|
||||||
point_cloud_msg_.point_step =
|
point_cloud_msg_.point_step =
|
||||||
addPointField(point_cloud_msg_, format_str, 1, sensor_msgs::msg::PointField::FLOAT32,
|
addPointField(point_cloud_msg_, format_str, 1, sensor_msgs::msg::PointField::FLOAT32,
|
||||||
@@ -761,34 +864,39 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
|
|||||||
for (uint32_t y = 0; y < color_height; y++) {
|
for (uint32_t y = 0; y < color_height; y++) {
|
||||||
for (uint32_t x = 0; x < color_width; x++) {
|
for (uint32_t x = 0; x < color_width; x++) {
|
||||||
float depth = depth_data[y * depth_width + x];
|
float depth = depth_data[y * depth_width + x];
|
||||||
|
bool vaild_point = true;
|
||||||
if (depth < min_depth || depth > max_depth) {
|
if (depth < min_depth || depth > max_depth) {
|
||||||
continue;
|
vaild_point= false;
|
||||||
|
}
|
||||||
|
if(vaild_point || ordered_pc_) {
|
||||||
|
float xf = (x - u0) * fdx;
|
||||||
|
float yf = (y - v0) * fdy;
|
||||||
|
float zf = depth * depth_scale;
|
||||||
|
*iter_x = zf * xf / 1000.0;
|
||||||
|
*iter_y = zf * yf / 1000.0;
|
||||||
|
*iter_z = zf / 1000.0;
|
||||||
|
*iter_r = color_data[(y * color_width + x) * 3];
|
||||||
|
*iter_g = color_data[(y * color_width + x) * 3 + 1];
|
||||||
|
*iter_b = color_data[(y * color_width + x) * 3 + 2];
|
||||||
|
++iter_x;
|
||||||
|
++iter_y;
|
||||||
|
++iter_z;
|
||||||
|
++iter_r;
|
||||||
|
++iter_g;
|
||||||
|
++iter_b;
|
||||||
|
++valid_count;
|
||||||
}
|
}
|
||||||
float xf = (x - u0) * fdx;
|
|
||||||
float yf = (y - v0) * fdy;
|
|
||||||
float zf = depth * depth_scale;
|
|
||||||
*iter_x = zf * xf / 1000.0;
|
|
||||||
*iter_y = zf * yf / 1000.0;
|
|
||||||
*iter_z = zf / 1000.0;
|
|
||||||
*iter_r = color_data[(y * color_width + x) * 3];
|
|
||||||
*iter_g = color_data[(y * color_width + x) * 3 + 1];
|
|
||||||
*iter_b = color_data[(y * color_width + x) * 3 + 2];
|
|
||||||
++iter_x;
|
|
||||||
++iter_y;
|
|
||||||
++iter_z;
|
|
||||||
++iter_r;
|
|
||||||
++iter_g;
|
|
||||||
++iter_b;
|
|
||||||
++valid_count;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
|
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
|
||||||
|
if(!ordered_pc_){
|
||||||
|
point_cloud_msg_.is_dense = true;
|
||||||
|
point_cloud_msg_.width = valid_count;
|
||||||
|
point_cloud_msg_.height = 1;
|
||||||
|
modifier.resize(valid_count);
|
||||||
|
}
|
||||||
point_cloud_msg_.header.stamp = timestamp;
|
point_cloud_msg_.header.stamp = timestamp;
|
||||||
point_cloud_msg_.header.frame_id = optical_frame_id_[COLOR];
|
point_cloud_msg_.header.frame_id = optical_frame_id_[COLOR];
|
||||||
point_cloud_msg_.is_dense = true;
|
|
||||||
point_cloud_msg_.width = valid_count;
|
|
||||||
point_cloud_msg_.height = 1;
|
|
||||||
modifier.resize(valid_count);
|
|
||||||
depth_registration_cloud_pub_->publish(point_cloud_msg_);
|
depth_registration_cloud_pub_->publish(point_cloud_msg_);
|
||||||
if (save_colored_point_cloud_) {
|
if (save_colored_point_cloud_) {
|
||||||
save_colored_point_cloud_ = false;
|
save_colored_point_cloud_ = false;
|
||||||
@@ -826,8 +934,10 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet> &fr
|
|||||||
colorFrameQueue_.push(frame_set);
|
colorFrameQueue_.push(frame_set);
|
||||||
colorFrameCV_.notify_all();
|
colorFrameCV_.notify_all();
|
||||||
}
|
}
|
||||||
|
else {
|
||||||
|
publishPointCloud(frame_set);
|
||||||
|
}
|
||||||
|
|
||||||
publishPointCloud(frame_set, false);
|
|
||||||
for (const auto &stream_index : IMAGE_STREAMS) {
|
for (const auto &stream_index : IMAGE_STREAMS) {
|
||||||
if (enable_stream_[stream_index]) {
|
if (enable_stream_[stream_index]) {
|
||||||
auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first);
|
auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first);
|
||||||
@@ -869,7 +979,7 @@ void OBCameraNode::onNewColorFrameCallback() {
|
|||||||
|
|
||||||
std::shared_ptr<ob::FrameSet> frameSet = colorFrameQueue_.front();
|
std::shared_ptr<ob::FrameSet> frameSet = colorFrameQueue_.front();
|
||||||
is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_);
|
is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_);
|
||||||
publishPointCloud(frameSet, true);
|
publishPointCloud(frameSet);
|
||||||
onNewFrameCallback(frameSet->colorFrame(), IMAGE_STREAMS.at(2));
|
onNewFrameCallback(frameSet->colorFrame(), IMAGE_STREAMS.at(2));
|
||||||
colorFrameQueue_.pop();
|
colorFrameQueue_.pop();
|
||||||
}
|
}
|
||||||
@@ -1009,12 +1119,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
|||||||
int height = static_cast<int>(video_frame->height());
|
int height = static_cast<int>(video_frame->height());
|
||||||
|
|
||||||
auto timestamp = frameTimeStampToROSTime(video_frame->systemTimeStamp());
|
auto timestamp = frameTimeStampToROSTime(video_frame->systemTimeStamp());
|
||||||
if (!camera_param_ && depth_registration_) {
|
if(!camera_param_) {
|
||||||
camera_param_ = pipeline_->getCameraParam();
|
camera_param_ = pipeline_->getCameraParam();
|
||||||
} else if (!camera_param_ && stream_index == COLOR) {
|
|
||||||
camera_param_ = getColorCameraParam();
|
|
||||||
} else if (!camera_param_ && (stream_index == DEPTH || stream_index == INFRA0)) {
|
|
||||||
camera_param_ = getDepthCameraParam();
|
|
||||||
}
|
}
|
||||||
auto &intrinsic =
|
auto &intrinsic =
|
||||||
stream_index == COLOR ? camera_param_->rgbIntrinsic : camera_param_->depthIntrinsic;
|
stream_index == COLOR ? camera_param_->rgbIntrinsic : camera_param_->depthIntrinsic;
|
||||||
@@ -1095,6 +1201,38 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame> &accelframe,
|
||||||
|
const std::shared_ptr<ob::Frame> &gryoframe) {
|
||||||
|
if (!imu_gyro_accel_publisher_) {
|
||||||
|
RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
auto subscriber_count = imu_gyro_accel_publisher_->get_subscription_count();
|
||||||
|
if (subscriber_count == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto imu_msg = sensor_msgs::msg::Imu();
|
||||||
|
setDefaultIMUMessage(imu_msg);
|
||||||
|
|
||||||
|
std::string imu_optical_frame_id ="camera_gyro_accel_optical_frame";
|
||||||
|
imu_msg.header.frame_id = imu_optical_frame_id;
|
||||||
|
auto timestamp = frameTimeStampToROSTime(accelframe->systemTimeStamp());
|
||||||
|
imu_msg.header.stamp = timestamp;
|
||||||
|
auto gyro_frame = gryoframe->as<ob::GyroFrame>();
|
||||||
|
auto gyroData = gyro_frame->value();
|
||||||
|
imu_msg.angular_velocity.x = gyroData.x;
|
||||||
|
imu_msg.angular_velocity.y = gyroData.y;
|
||||||
|
imu_msg.angular_velocity.z = gyroData.z;
|
||||||
|
auto accel_frame = accelframe->as<ob::AccelFrame>();
|
||||||
|
auto accelData = accel_frame->value();
|
||||||
|
imu_msg.linear_acceleration.x = accelData.x;
|
||||||
|
imu_msg.linear_acceleration.y = accelData.y;
|
||||||
|
imu_msg.linear_acceleration.z = accelData.z;
|
||||||
|
imu_gyro_accel_publisher_->publish(imu_msg);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||||
const stream_index_pair &stream_index) {
|
const stream_index_pair &stream_index) {
|
||||||
if (!imu_publishers_.count(stream_index)) {
|
if (!imu_publishers_.count(stream_index)) {
|
||||||
@@ -1260,8 +1398,14 @@ void OBCameraNode::calcAndPublishStaticTransform() {
|
|||||||
Q = transform.getRotation();
|
Q = transform.getRotation();
|
||||||
trans = transform.getOrigin();
|
trans = transform.getOrigin();
|
||||||
rclcpp::Time tf_timestamp = node_->now();
|
rclcpp::Time tf_timestamp = node_->now();
|
||||||
|
auto device_info = device_->getDeviceInfo();
|
||||||
|
auto pid = device_info->pid();
|
||||||
if (enable_stream_[COLOR]) {
|
if (enable_stream_[COLOR]) {
|
||||||
publishStaticTF(tf_timestamp, trans, Q, camera_link_frame_id_, frame_id_[COLOR]);
|
if(pid != FEMTO_BOLT_PID){
|
||||||
|
publishStaticTF(tf_timestamp, trans, Q, camera_link_frame_id_, frame_id_[COLOR]);
|
||||||
|
} else {
|
||||||
|
publishStaticTF(tf_timestamp, trans, zero_rot, camera_link_frame_id_, frame_id_[COLOR]);
|
||||||
|
}
|
||||||
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[COLOR],
|
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[COLOR],
|
||||||
optical_frame_id_[COLOR]);
|
optical_frame_id_[COLOR]);
|
||||||
}
|
}
|
||||||
@@ -1269,8 +1413,13 @@ void OBCameraNode::calcAndPublishStaticTransform() {
|
|||||||
if (stream_index == COLOR || !enable_stream_[stream_index]) {
|
if (stream_index == COLOR || !enable_stream_[stream_index]) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
if(pid != FEMTO_BOLT_PID){
|
||||||
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_,
|
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_,
|
||||||
frame_id_[stream_index]);
|
frame_id_[stream_index]);
|
||||||
|
} else {
|
||||||
|
publishStaticTF(tf_timestamp, zero_trans, Q, camera_link_frame_id_,
|
||||||
|
frame_id_[stream_index]);
|
||||||
|
}
|
||||||
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
|
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
|
||||||
optical_frame_id_[stream_index]);
|
optical_frame_id_[stream_index]);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -22,15 +22,19 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
|
|||||||
OBCameraDistortion distortion, int width) {
|
OBCameraDistortion distortion, int width) {
|
||||||
(void)width;
|
(void)width;
|
||||||
sensor_msgs::msg::CameraInfo info;
|
sensor_msgs::msg::CameraInfo info;
|
||||||
info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
|
info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
|
||||||
info.width = intrinsic.width;
|
info.width = intrinsic.width;
|
||||||
info.height = intrinsic.height;
|
info.height = intrinsic.height;
|
||||||
info.d.resize(5, 0.0);
|
info.d.resize(8, 0.0);
|
||||||
info.d[0] = distortion.k1;
|
info.d[0] = distortion.k1;
|
||||||
info.d[1] = distortion.k2;
|
info.d[1] = distortion.k2;
|
||||||
info.d[2] = distortion.k3;
|
info.d[2] = distortion.p1;
|
||||||
info.d[3] = distortion.k4;
|
info.d[3] = distortion.p2;
|
||||||
info.d[4] = distortion.k5;
|
info.d[4] = distortion.k3;
|
||||||
|
info.d[5] = distortion.k4;
|
||||||
|
info.d[6] = distortion.k5;
|
||||||
|
info.d[7] = distortion.k6;
|
||||||
|
|
||||||
|
|
||||||
info.k.fill(0.0);
|
info.k.fill(0.0);
|
||||||
info.k[0] = intrinsic.fx;
|
info.k[0] = intrinsic.fx;
|
||||||
|
|||||||
Reference in New Issue
Block a user