diff --git a/README.MD b/README.MD index 80fa2bd0..912c6d8d 100644 --- a/README.MD +++ b/README.MD @@ -1,5 +1,5 @@ # orbbec_camera -[![stable](http://badges.github.io/stability-badges/dist/stable.svg)](http://github.com/badges/stability-badges) ![version](https://img.shields.io/badge/version-1.3.6-green) +[![stable](http://badges.github.io/stability-badges/dist/stable.svg)](http://github.com/badges/stability-badges) ![version](https://img.shields.io/badge/version-1.4.2-green) --- 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. @@ -195,6 +195,9 @@ to `true` in the stream that corresponds to the argument of the launch file. is `true`. - `/camera/ir/camera_info`: The IR camera info. - `/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 @@ -337,6 +340,7 @@ The following are the launch parameters available: camera. - `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. +- `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 - 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 Sparse Default # Binned Sparse Default + # Obstacle Avoidance 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 | |----------------------------------------------|-------------------------| | 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 | | astra2 | astra2.launch.py | | astra stereo s | stereo_s_u3.launch.py | +| astra pro2 | astra_pro2.launch.py | | dabai | dabai.launch.py | | dabai d1 | dabai_d1.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 -| **SDK version** | **products list** | **firmware version** | -|-----------------|-------------------|-------------------------------------------| -| v1.8.1 | Gemini 2 XL | Obox: V1.2.5 VL:1.4.54 | -| | Astra 2 | 2.8.20 | -| | Gemini 2 L | 1.4.32 | -| | Gemini 2 | 1.4.60 /1.4.76 | -| | Femto Mega | 1.1.7 (window10、ubuntu20.04、ubuntu22.04) | -| | Astra+ | 1.0.22/1.0.21/1.0.20/1.0.19 | -| | Femto | 1.6.7 | -| | Femto W | 1.1.8 | -| | Femto Bolt | 1.0.6 (unsupported ARNM32) | -| | DaBai | 2436 | -| | DaBai DCW | 2460 | -| | DaBai DW | 2606 | -| | Astra Mini Pro | 1007 | -| | Gemini E | 3460 | -| | Gemini E Lite | 3606 | -| | Gemini | 3.0.18 | -| | Astra Mini S Pro | 1.0.05 | -| | DaBai Max Pro | 1.0.06 | -| | Gemini UW | 1.0.060 | +| **products list** | **firmware version** | +|-------------------|-------------------------------------------| +| Gemini 2 XL | Obox: V1.2.5 VL:1.4.54 | +| Astra 2 | 2.8.20 | +| Gemini 2 L | 1.4.32 | +| Gemini 2 | 1.4.60 /1.4.76 | +| Femto Mega | 1.1.7 (window10、ubuntu20.04、ubuntu22.04) | +| Astra+ | 1.0.22/1.0.21/1.0.20/1.0.19 | +| Femto | 1.6.7 | +| Femto W | 1.1.8 | +| Femto Bolt | 1.0.6 (unsupported ARM32) | +| DaBai | 2436 | +| DaBai DCW | 2460 | +| DaBai DW | 2606 | +| Astra Mini Pro | 1007 | +| Gemini E | 3460 | +| Gemini E Lite | 3606 | +| Gemini | 3.0.18 | +| Astra Mini S Pro | 1.0.05 | ## DDS Tuning diff --git a/README_CN.MD b/README_CN.MD index fc68d9fe..c67993ce 100644 --- a/README_CN.MD +++ b/README_CN.MD @@ -196,6 +196,9 @@ ros2 service call /camera/save_point_cloud std_srvs/srv/Empty "{}" 都设置为 `true`才可用。 - `/camera/ir/camera_info`: IR相机信息。 - `/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设备支持。 当通过网络方式访问以上设备时，需提前配置好设备的IP地址，使能开关需要配置成true。 +- `depth_filter_config` : 配置深度滤波配置文件加载路径，默认深度滤波配置文件在/config/depthfilter目录下,仅gemini2支持。 ## 深度模式切换： - 启动相机前,可通过配置对应相继的xxx.launch.py的深度模式(depth_work_mode)支持。 @@ -336,6 +340,7 @@ ros2 launch orbbec_camera multi_camera.launch.py # Unbinned Dense Default # Unbinned Sparse Default # Binned Sparse Default + # Obstacle Avoidance DeclareLaunchArgument('depth_work_mode', default_value='') ``` @@ -365,10 +370,11 @@ IR的分辨率和帧率必须和深度保持一致。不同模式和分辨率对 | product serials | launch file | |----------------------------------------------|-------------------------| | 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 | | astra2 | astra2.launch.py | | astra stereo s | stereo_s_u3.launch.py | +| astra pro2 | astra_pro2.launch.py | | dabai | dabai.launch.py | | dabai d1 | dabai_d1.launch.py | | dabai dcw | dabai_dcw.launch.py | @@ -390,27 +396,25 @@ XML launch 文件将会在后续的版本中删除。 ## 已经支持的硬件产品列表 -| **SDK version** | **products list** | **firmware version** | -|-----------------|-------------------|-------------------------------------------| -| v1.8.1 | Gemini 2 XL | Obox: V1.2.5 VL:1.4.54 | -| | Astra 2 | 2.8.20 | -| | Gemini 2 L | 1.4.32 | -| | Gemini 2 | 1.4.60 /1.4.76 | -| | Femto Mega | 1.1.7 (window10、ubuntu20.04、ubuntu22.04) | -| | Astra+ | 1.0.22/1.0.21/1.0.20/1.0.19 | -| | Femto | 1.6.7 | -| | Femto W | 1.1.8 | -| | Femto Bolt | 1.0.6 (unsupported ARNM32) | -| | DaBai | 2436 | -| | DaBai DCW | 2460 | -| | DaBai DW | 2606 | -| | Astra Mini Pro | 1007 | -| | Gemini E | 3460 | -| | Gemini E Lite | 3606 | -| | Gemini | 3.0.18 | -| | Astra Mini S Pro | 1.0.05 | -| | DaBai Max Pro | 1.0.06 | -| | Gemini UW | 1.0.060 | +| **products list** | **firmware version** | +|-------------------|-------------------------------------------| +| Gemini 2 XL | Obox: V1.2.5 VL:1.4.54 | +| Astra 2 | 2.8.20 | +| Gemini 2 L | 1.4.32 | +| Gemini 2 | 1.4.60 /1.4.76 | +| Femto Mega | 1.1.7 (window10、ubuntu20.04、ubuntu22.04) | +| Astra+ | 1.0.22/1.0.21/1.0.20/1.0.19 | +| Femto | 1.6.7 | +| Femto W | 1.1.8 | +| Femto Bolt | 1.0.6 (unsupported ARM32) | +| DaBai | 2436 | +| DaBai DCW | 2460 | +| DaBai DW | 2606 | +| Astra Mini Pro | 1007 | +| Gemini E | 3460 | +| Gemini E Lite | 3606 | +| Gemini | 3.0.18 | +| Astra Mini S Pro | 1.0.05 | ## DDS Tuning diff --git a/orbbec_camera/SDK/include/libobsensor/h/Device.h b/orbbec_camera/SDK/include/libobsensor/h/Device.h index 9529b0b9..62808509 100644 --- a/orbbec_camera/SDK/include/libobsensor/h/Device.h +++ b/orbbec_camera/SDK/include/libobsensor/h/Device.h @@ -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. * - * @param[in] info Device Information + * @param[in] list Device list object * @param[in] index Device index * @param[out] error Log error messages * @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); +/** + * @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 } #endif diff --git a/orbbec_camera/SDK/include/libobsensor/h/Frame.h b/orbbec_camera/SDK/include/libobsensor/h/Frame.h index d532cddc..b589f081 100644 --- a/orbbec_camera/SDK/include/libobsensor/h/Frame.h +++ b/orbbec_camera/SDK/include/libobsensor/h/Frame.h @@ -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); +/** + * @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 * diff --git a/orbbec_camera/SDK/include/libobsensor/h/ObTypes.h b/orbbec_camera/SDK/include/libobsensor/h/ObTypes.h index 41b0b272..73e0d260 100644 --- a/orbbec_camera/SDK/include/libobsensor/h/ObTypes.h +++ b/orbbec_camera/SDK/include/libobsensor/h/ObTypes.h @@ -161,6 +161,7 @@ typedef enum { OB_SENSOR_IR_LEFT = 6, /**< left IR */ OB_SENSOR_IR_RIGHT = 7, /**< Right IR */ OB_SENSOR_RAW_PHASE = 8, /**< Raw Phase */ + OB_SENSOR_COUNT, } OBSensorType, ob_sensor_type; @@ -367,7 +368,7 @@ typedef struct { typedef struct { float rot[9]; ///< Rotation matrix float trans[3]; ///< Transformation matrix -} OBD2CTransform, ob_d2c_transform; +} OBD2CTransform, ob_d2c_transform, OBTransform, ob_transform; /** * @brief Structure for camera parameters @@ -380,6 +381,7 @@ typedef struct { OBD2CTransform transform; ///< Rotation/transformation matrix bool isMirrored; ///< Whether the image frame corresponding to this group of parameters is mirrored } OBCameraParam, ob_camera_param; + /** * @brief Camera parameters */ @@ -392,6 +394,15 @@ typedef struct { OBD2CTransform transform; ///< Rotation/transformation matrix } 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 */ @@ -456,6 +467,7 @@ typedef enum { FORMAT_MJPG_TO_BGRA, /**< MJPG to BGRA */ FORMAT_UYVY_TO_RGB888, /**< UYVY to RGB888 */ FORMAT_BGR_TO_RGB, /**< BGR to RGB */ + FORMAT_MJPG_TO_NV12, /**< MJPG to NV12 */ } OBConvertFormat, ob_convert_format; @@ -632,7 +644,15 @@ typedef struct { float x; ///< X coordinate float y; ///< Y 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 diff --git a/orbbec_camera/SDK/include/libobsensor/h/Pipeline.h b/orbbec_camera/SDK/include/libobsensor/h/Pipeline.h index 54a148a8..fad24d12 100644 --- a/orbbec_camera/SDK/include/libobsensor/h/Pipeline.h +++ b/orbbec_camera/SDK/include/libobsensor/h/Pipeline.h @@ -169,7 +169,6 @@ ob_camera_param ob_pipeline_get_camera_param_with_profile(ob_pipeline *pipeline, uint32_t depthHeight, ob_error **error); /** - * \if English * @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 * @@ -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); +/** + * @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 * @@ -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. * * @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 */ void ob_config_set_frame_aggregate_output_mode(ob_config *config, ob_frame_aggregate_output_mode mode, ob_error **error); diff --git a/orbbec_camera/SDK/include/libobsensor/h/Property.h b/orbbec_camera/SDK/include/libobsensor/h/Property.h index 923c7e81..913ef406 100644 --- a/orbbec_camera/SDK/include/libobsensor/h/Property.h +++ b/orbbec_camera/SDK/include/libobsensor/h/Property.h @@ -617,11 +617,6 @@ typedef enum { */ 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) */ diff --git a/orbbec_camera/SDK/include/libobsensor/h/Utils.h b/orbbec_camera/SDK/include/libobsensor/h/Utils.h new file mode 100644 index 00000000..f7283e73 --- /dev/null +++ b/orbbec_camera/SDK/include/libobsensor/h/Utils.h @@ -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 \ No newline at end of file diff --git a/orbbec_camera/SDK/include/libobsensor/hpp/Device.hpp b/orbbec_camera/SDK/include/libobsensor/hpp/Device.hpp index 869cf670..50d2348a 100644 --- a/orbbec_camera/SDK/include/libobsensor/hpp/Device.hpp +++ b/orbbec_camera/SDK/include/libobsensor/hpp/Device.hpp @@ -26,9 +26,11 @@ class CameraParamList; class OBDepthWorkModeList; class OB_EXTENSION_API Device { -private: +protected: std::unique_ptr impl_; + Device(Device &&device); + public: /** * @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); + /** + * @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 * @@ -530,8 +539,20 @@ public: */ 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 Recorder; + friend class CoordinateTransformHelper; }; /** diff --git a/orbbec_camera/SDK/include/libobsensor/hpp/Frame.hpp b/orbbec_camera/SDK/include/libobsensor/hpp/Frame.hpp index 61dc97da..28604f38 100644 --- a/orbbec_camera/SDK/include/libobsensor/hpp/Frame.hpp +++ b/orbbec_camera/SDK/include/libobsensor/hpp/Frame.hpp @@ -108,6 +108,18 @@ public: */ 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. * @@ -134,6 +146,7 @@ private: friend class Filter; friend class Recorder; friend class FrameHelper; + friend class CoordinateTransformHelper; }; class OB_EXTENSION_API VideoFrame : public Frame { diff --git a/orbbec_camera/SDK/include/libobsensor/hpp/Pipeline.hpp b/orbbec_camera/SDK/include/libobsensor/hpp/Pipeline.hpp index 93009c0c..70a6671c 100644 --- a/orbbec_camera/SDK/include/libobsensor/hpp/Pipeline.hpp +++ b/orbbec_camera/SDK/include/libobsensor/hpp/Pipeline.hpp @@ -145,6 +145,15 @@ public: */ 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); + /** * @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); + /** + * @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; }; diff --git a/orbbec_camera/SDK/include/libobsensor/hpp/Utils.hpp b/orbbec_camera/SDK/include/libobsensor/hpp/Utils.hpp new file mode 100644 index 00000000..0ac6c1a8 --- /dev/null +++ b/orbbec_camera/SDK/include/libobsensor/hpp/Utils.hpp @@ -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 Transformed depth frame + */ + static std::shared_ptr transformationDepthFrameToColorCamera(std::shared_ptr device, std::shared_ptr depthFrame, + uint32_t targetColorCameraWidth, uint32_t targetColorCameraHeight); +}; +} // namespace ob diff --git a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so b/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so index 253ea98d..cbc0ec35 120000 --- a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so +++ b/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so @@ -1 +1 @@ -libOrbbecSDK.so.1.8 \ No newline at end of file +libOrbbecSDK.so.1.9 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.8 b/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.8 deleted file mode 120000 index 65e38893..00000000 --- a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.8 +++ /dev/null @@ -1 +0,0 @@ -libOrbbecSDK.so.1.8.1 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.9 b/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.9 new file mode 120000 index 00000000..15d77e01 --- /dev/null +++ b/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.9 @@ -0,0 +1 @@ +libOrbbecSDK.so.1.9.1 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.8.1 b/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.9.1 similarity index 60% rename from orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.8.1 rename to orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.9.1 index facd3c9c..12446bf0 100644 Binary files a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.8.1 and b/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.9.1 differ diff --git a/orbbec_camera/SDK/lib/arm32/libudev.so b/orbbec_camera/SDK/lib/arm32/libudev.so deleted file mode 120000 index d38ce683..00000000 --- a/orbbec_camera/SDK/lib/arm32/libudev.so +++ /dev/null @@ -1 +0,0 @@ -libudev.so.1.6.3 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm32/libudev.so.1 b/orbbec_camera/SDK/lib/arm32/libudev.so.1 deleted file mode 120000 index d38ce683..00000000 --- a/orbbec_camera/SDK/lib/arm32/libudev.so.1 +++ /dev/null @@ -1 +0,0 @@ -libudev.so.1.6.3 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm32/libudev.so.1.6.3 b/orbbec_camera/SDK/lib/arm32/libudev.so.1.6.3 deleted file mode 100644 index 355db088..00000000 Binary files a/orbbec_camera/SDK/lib/arm32/libudev.so.1.6.3 and /dev/null differ diff --git a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so b/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so index 253ea98d..cbc0ec35 120000 --- a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so +++ b/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so @@ -1 +1 @@ -libOrbbecSDK.so.1.8 \ No newline at end of file +libOrbbecSDK.so.1.9 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.8 b/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.8 deleted file mode 120000 index 65e38893..00000000 --- a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.8 +++ /dev/null @@ -1 +0,0 @@ -libOrbbecSDK.so.1.8.1 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.9 b/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.9 new file mode 120000 index 00000000..15d77e01 --- /dev/null +++ b/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.9 @@ -0,0 +1 @@ +libOrbbecSDK.so.1.9.1 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.8.1 b/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.9.1 similarity index 56% rename from orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.8.1 rename to orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.9.1 index c601da3d..3134414d 100644 Binary files a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.8.1 and b/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.9.1 differ diff --git a/orbbec_camera/SDK/lib/arm64/libudev.so b/orbbec_camera/SDK/lib/arm64/libudev.so deleted file mode 120000 index d38ce683..00000000 --- a/orbbec_camera/SDK/lib/arm64/libudev.so +++ /dev/null @@ -1 +0,0 @@ -libudev.so.1.6.3 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm64/libudev.so.1 b/orbbec_camera/SDK/lib/arm64/libudev.so.1 deleted file mode 120000 index d38ce683..00000000 --- a/orbbec_camera/SDK/lib/arm64/libudev.so.1 +++ /dev/null @@ -1 +0,0 @@ -libudev.so.1.6.3 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/arm64/libudev.so.1.6.3 b/orbbec_camera/SDK/lib/arm64/libudev.so.1.6.3 deleted file mode 100644 index c1577c84..00000000 Binary files a/orbbec_camera/SDK/lib/arm64/libudev.so.1.6.3 and /dev/null differ diff --git a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so b/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so index 253ea98d..cbc0ec35 120000 --- a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so +++ b/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so @@ -1 +1 @@ -libOrbbecSDK.so.1.8 \ No newline at end of file +libOrbbecSDK.so.1.9 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.8 b/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.8 deleted file mode 120000 index 65e38893..00000000 --- a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.8 +++ /dev/null @@ -1 +0,0 @@ -libOrbbecSDK.so.1.8.1 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.9 b/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.9 new file mode 120000 index 00000000..15d77e01 --- /dev/null +++ b/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.9 @@ -0,0 +1 @@ +libOrbbecSDK.so.1.9.1 \ No newline at end of file diff --git a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.8.1 b/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.9.1 similarity index 57% rename from orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.8.1 rename to orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.9.1 index effb390c..14bdd176 100644 Binary files a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.8.1 and b/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.9.1 differ diff --git a/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml b/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml index 6f49a57c..39cf17a9 100644 --- a/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml +++ b/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml @@ -33,6 +33,14 @@ 10 + + false + + 1000 + + 10 + + @@ -461,6 +469,10 @@ + + 640 @@ -498,6 +510,92 @@ 0 + + + + 640 + + 400 + + 15 + + Y11 + + 0 + + + 30 + + + + + + 640 + + 480 + + 15 + + MJPG + + 0 + + + + 640 + + 400 + + 15 + + Y10 + + 0 + + + 30 + + + + + + + + 640 + + 320 + + 10 + + Y12 + + 0 + + + + 640 + + 480 + + 25 + + MJPG + + 0 + + + + 640 + + 400 + + 10 + + Y10 + + 0 + + @@ -627,7 +725,42 @@ 0 - + + + + 640 + + 400 + + 15 + + Y11 + + 0 + + + 30 + + + + + + 640 + + 400 + + 15 + + Y10 + + 0 + + + 30 + + + + @@ -1229,6 +1362,11 @@ 0 1 + + + @@ -1294,7 +1432,7 @@ - + @@ -1316,6 +1454,18 @@ Y8 + + + + + 640 + + 400 + + 15 + RLE + + @@ -2076,7 +2226,7 @@ 5 5000 - + diff --git a/orbbec_camera/config/depthfilter/Gemini2_v1.7.json b/orbbec_camera/config/depthfilter/Gemini2_v1.7.json new file mode 100644 index 00000000..124d485f --- /dev/null +++ b/orbbec_camera/config/depthfilter/Gemini2_v1.7.json @@ -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 + } + } + ] +} diff --git a/orbbec_camera/config/depthfilter/Openni_device.json b/orbbec_camera/config/depthfilter/Openni_device.json new file mode 100644 index 00000000..eded4a5e --- /dev/null +++ b/orbbec_camera/config/depthfilter/Openni_device.json @@ -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 + } + } + ] +} \ No newline at end of file diff --git a/orbbec_camera/include/orbbec_camera/constants.h b/orbbec_camera/include/orbbec_camera/constants.h index 7386a0a2..676c0299 100644 --- a/orbbec_camera/include/orbbec_camera/constants.h +++ b/orbbec_camera/include/orbbec_camera/constants.h @@ -23,7 +23,7 @@ #define OB_ROS_MAJOR_VERSION 1 #define OB_ROS_MINOR_VERSION 4 -#define OB_ROS_PATCH_VERSION 0 +#define OB_ROS_PATCH_VERSION 3 #ifndef STRINGIFY #define STRINGIFY(arg) #arg diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 69a69819..3eb8ef9a 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -262,7 +262,7 @@ class OBCameraNode { void switchIRCameraCallback(const std::shared_ptr& request, std::shared_ptr& response); - void publishPointCloud(const std::shared_ptr& frame_set, bool isColorPointCloud); + void publishPointCloud(const std::shared_ptr& frame_set); void publishDepthPointCloud(const std::shared_ptr& frame_set); @@ -284,6 +284,9 @@ class OBCameraNode { void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image, const sensor_msgs::msg::Image::SharedPtr& image_msg); + void onNewIMUFrameSyncOutputCallback(const std::shared_ptr& accelframe, + const std::shared_ptr& gryoframe); + void onNewIMUFrameCallback(const std::shared_ptr& frame, const stream_index_pair& stream_index); @@ -305,6 +308,7 @@ class OBCameraNode { rclcpp::Logger logger_; std::atomic_bool is_running_{false}; std::unique_ptr pipeline_ = nullptr; + std::unique_ptr imuPipeline_ = nullptr; std::atomic_bool pipeline_started_{false}; std::string camera_name_ = "camera"; std::shared_ptr pipeline_config_ = nullptr; @@ -364,6 +368,7 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_fan_work_mode_srv_; rclcpp::Service::SharedPtr toggle_sensors_srv_; + bool enable_sync_output_accel_gyro_ = false; bool publish_tf_ = false; bool tf_published_ = false; std::shared_ptr static_tf_broadcaster_ = nullptr; @@ -393,6 +398,8 @@ class OBCameraNode { std::atomic_bool save_colored_point_cloud_{false}; rclcpp::Service::SharedPtr save_images_srv_; rclcpp::Service::SharedPtr save_point_cloud_srv_; + std::string depth_filter_config_; + bool enable_depth_filter_ = false; bool enable_soft_filter_ = true; bool enable_color_auto_exposure_ = true; bool enable_ir_auto_exposure_ = true; @@ -414,6 +421,8 @@ class OBCameraNode { OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8; // IMU std::map::SharedPtr> imu_publishers_; + rclcpp::Publisher::SharedPtr imu_gyro_accel_publisher_; + bool imu_sync_output_start_ = false; std::map imu_rate_; std::map imu_range_; std::map imu_qos_; @@ -433,5 +442,6 @@ class OBCameraNode { std::mutex colorFrameMtx_; std::condition_variable colorFrameCV_; + bool ordered_pc_ = false; }; } // namespace orbbec_camera diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index c0caed16..6267f961 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/orbbec_camera/launch/astra.launch.py @@ -61,6 +61,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index 65fe0ee7..21924c06 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/orbbec_camera/launch/astra2.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/astra_adv.launch.py b/orbbec_camera/launch/astra_adv.launch.py index 5dc068f7..1f636c39 100644 --- a/orbbec_camera/launch/astra_adv.launch.py +++ b/orbbec_camera/launch/astra_adv.launch.py @@ -61,6 +61,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/astra_embedded_s.launch.py b/orbbec_camera/launch/astra_embedded_s.launch.py index 32958030..1b83d4db 100644 --- a/orbbec_camera/launch/astra_embedded_s.launch.py +++ b/orbbec_camera/launch/astra_embedded_s.launch.py @@ -61,6 +61,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/astra_pro2.launch.py b/orbbec_camera/launch/astra_pro2.launch.py new file mode 100644 index 00000000..ad8091d4 --- /dev/null +++ b/orbbec_camera/launch/astra_pro2.launch.py @@ -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 diff --git a/orbbec_camera/launch/astra_stereo_u3.launch.py b/orbbec_camera/launch/astra_stereo_u3.launch.py index a22f45ee..1383a5cb 100644 --- a/orbbec_camera/launch/astra_stereo_u3.launch.py +++ b/orbbec_camera/launch/astra_stereo_u3.launch.py @@ -61,6 +61,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/dabai.launch.py b/orbbec_camera/launch/dabai.launch.py index a22f45ee..1383a5cb 100644 --- a/orbbec_camera/launch/dabai.launch.py +++ b/orbbec_camera/launch/dabai.launch.py @@ -61,6 +61,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/dabai_d1.launch.py b/orbbec_camera/launch/dabai_d1.launch.py index a0d27a29..69b8a7dd 100644 --- a/orbbec_camera/launch/dabai_d1.launch.py +++ b/orbbec_camera/launch/dabai_d1.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/dabai_dcw.launch.py b/orbbec_camera/launch/dabai_dcw.launch.py index 91790181..4f9ffef3 100644 --- a/orbbec_camera/launch/dabai_dcw.launch.py +++ b/orbbec_camera/launch/dabai_dcw.launch.py @@ -60,7 +60,8 @@ def generate_launch_description(): 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('soft_filter_speckle_size', default_value='-1'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py index 65d3e8b9..2da3c545 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/orbbec_camera/launch/dabai_dcw2.launch.py @@ -60,7 +60,8 @@ def generate_launch_description(): 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('soft_filter_speckle_size', default_value='-1'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/dabai_dw.launch.py b/orbbec_camera/launch/dabai_dw.launch.py index 07f93c39..50fafa50 100644 --- a/orbbec_camera/launch/dabai_dw.launch.py +++ b/orbbec_camera/launch/dabai_dw.launch.py @@ -49,8 +49,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/dabai_max.launch.py b/orbbec_camera/launch/dabai_max.launch.py index 7664e58c..7c7e3506 100644 --- a/orbbec_camera/launch/dabai_max.launch.py +++ b/orbbec_camera/launch/dabai_max.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/dabai_pro.launch.py b/orbbec_camera/launch/dabai_pro.launch.py index a0d27a29..69b8a7dd 100644 --- a/orbbec_camera/launch/dabai_pro.launch.py +++ b/orbbec_camera/launch/dabai_pro.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/deeya.launch.py b/orbbec_camera/launch/deeya.launch.py index af81347b..6765c15a 100644 --- a/orbbec_camera/launch/deeya.launch.py +++ b/orbbec_camera/launch/deeya.launch.py @@ -60,6 +60,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 5c60b8be..cc3405f8 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/orbbec_camera/launch/femto.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): DeclareLaunchArgument('ir_qos', default_value='default'), DeclareLaunchArgument('ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('accel_rate', default_value='100hz'), 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_speckle_size', default_value='-1'), DeclareLaunchArgument('enable_frame_sync', default_value='false'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index 8d1bd0b9..6808db45 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/orbbec_camera/launch/femto_bolt.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): DeclareLaunchArgument('ir_qos', default_value='default'), DeclareLaunchArgument('ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('accel_rate', default_value='100hz'), DeclareLaunchArgument('accel_range', default_value='4g'), @@ -75,6 +76,7 @@ def generate_launch_description(): DeclareLaunchArgument('trigger2image_delay_us', default_value='0'), DeclareLaunchArgument('trigger_out_delay_us', default_value='0'), DeclareLaunchArgument('trigger_out_enabled', default_value='false'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index e83054e0..722b1d18 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/orbbec_camera/launch/femto_mega.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): DeclareLaunchArgument('ir_qos', default_value='default'), DeclareLaunchArgument('ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('accel_rate', default_value='100hz'), 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_enabled', default_value='false'), DeclareLaunchArgument('enable_frame_sync', default_value='true'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index 6cc42491..9d06b8c8 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): DeclareLaunchArgument('ir_qos', default_value='default'), DeclareLaunchArgument('ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('accel_rate', default_value='100hz'), DeclareLaunchArgument('accel_range', default_value='4g'), @@ -64,15 +65,14 @@ def generate_launch_description(): DeclareLaunchArgument('log_level', default_value='none'), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_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'), + # Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json + DeclareLaunchArgument('depth_filter_config', default_value=''), # Depth work mode support is as follows: # Unbinned Dense Default # Unbinned Sparse Default # Binned Sparse Default + # Obstacle Avoidance DeclareLaunchArgument('depth_work_mode', default_value=''), DeclareLaunchArgument('sync_mode', default_value='free_run'), 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_enabled', default_value='false'), DeclareLaunchArgument('enable_frame_sync', default_value='true'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index 6637171b..0f705a8a 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): DeclareLaunchArgument('ir_qos', default_value='default'), DeclareLaunchArgument('ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('accel_rate', default_value='100hz'), 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_enabled', default_value='false'), DeclareLaunchArgument('enable_frame_sync', default_value='true'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/gemini2VL.launch.py b/orbbec_camera/launch/gemini2VL.launch.py index f1bcefdc..d6dfbfda 100644 --- a/orbbec_camera/launch/gemini2VL.launch.py +++ b/orbbec_camera/launch/gemini2VL.launch.py @@ -47,6 +47,7 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_qos', default_value='default'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'), DeclareLaunchArgument('enable_accel', default_value='true'), DeclareLaunchArgument('accel_rate', default_value='100hz'), 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_enabled', default_value='false'), DeclareLaunchArgument('enable_frame_sync', default_value='true'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/gemini2XL.launch.py b/orbbec_camera/launch/gemini2XL.launch.py index 6c79fab7..903edf79 100644 --- a/orbbec_camera/launch/gemini2XL.launch.py +++ b/orbbec_camera/launch/gemini2XL.launch.py @@ -56,6 +56,7 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_qos', default_value='default'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'), DeclareLaunchArgument('enable_accel', default_value='false'), DeclareLaunchArgument('accel_rate', default_value='100hz'), 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_enabled', default_value='false'), DeclareLaunchArgument('enable_frame_sync', default_value='true'), + DeclareLaunchArgument('ordered_pc', default_value='false'), ] # Node configuration diff --git a/orbbec_camera/launch/gemini_e.launch.py b/orbbec_camera/launch/gemini_e.launch.py index f25462dc..8ff13393 100644 --- a/orbbec_camera/launch/gemini_e.launch.py +++ b/orbbec_camera/launch/gemini_e.launch.py @@ -61,6 +61,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/launch/gemini_e_lite.launch.py b/orbbec_camera/launch/gemini_e_lite.launch.py index 8c010f57..9514f795 100644 --- a/orbbec_camera/launch/gemini_e_lite.launch.py +++ b/orbbec_camera/launch/gemini_e_lite.launch.py @@ -50,6 +50,7 @@ def generate_launch_description(): 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 diff --git a/orbbec_camera/package.xml b/orbbec_camera/package.xml index 216f29fc..c942af5b 100644 --- a/orbbec_camera/package.xml +++ b/orbbec_camera/package.xml @@ -2,7 +2,7 @@ orbbec_camera - 1.4.0 + 1.4.3 Orbbec Camera package Joe Dong TODO: License declaration diff --git a/orbbec_camera/scripts/99-obsensor-libusb.rules b/orbbec_camera/scripts/99-obsensor-libusb.rules index 24ad10c1..d6a565ee 100644 --- a/orbbec_camera/scripts/99-obsensor-libusb.rules +++ b/orbbec_camera/scripts/99-obsensor-libusb.rules @@ -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}=="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}=="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" diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 1e481c86..7ed7fd50 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -133,6 +133,13 @@ void OBCameraNode::setupDevices() { auto info = device_->getDeviceInfo(); if (enable_hardware_d2d_ && info->pid() == GEMINI2_PID) { 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 { if (!depth_work_mode_.empty()) { @@ -153,6 +160,9 @@ void OBCameraNode::setupDevices() { if (default_precision_level != 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) { @@ -178,18 +188,37 @@ void OBCameraNode::setupDevices() { } } - device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_); - device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_); - device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_); - 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 (!depth_filter_config_.empty() && enable_depth_filter_) { + RCLCPP_INFO_STREAM(logger_, "Load depth filter config: " << depth_filter_config_); + device_->loadDepthFilterConfig(depth_filter_config_.c_str()); + } else { + if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) { + device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_); + } } - 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_); + + if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) { + device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_); + } + + 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) { 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"); throw std::runtime_error("Failed to start pipeline"); } - if (enable_stream_[COLOR]) { + if (enable_stream_[COLOR] && !colorFrameThread_) { colorFrameThread_ = std::make_shared([this]() { onNewColorFrameCallback(); }); } if (enable_frame_sync_) { @@ -296,49 +325,94 @@ void OBCameraNode::startStreams() { } void OBCameraNode::startIMU() { - 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(); - 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 &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(); - 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 &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; + if (enable_sync_output_accel_gyro_) { + if (imuPipeline_ != nullptr) { + imuPipeline_.reset(); + } + + imuPipeline_ = std::make_unique(device_); + if (imu_sync_output_start_){ + return; + } + + //ACCEL + auto accelProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_ACCEL); + auto accel_range = fullAccelScaleRangeFromString(imu_range_[ACCEL]); + auto accel_rate = sampleRateFromString(imu_rate_[ACCEL]); + auto accelProfile = accelProfiles->getAccelStreamProfile(accel_range, accel_rate); + //GYRO + auto gyroProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_GYRO); + auto gyro_range = fullGyroScaleRangeFromString(imu_range_[GYRO]); + auto gyro_rate = sampleRateFromString(imu_rate_[GYRO]); + auto gyroProfile = gyroProfiles->getGyroStreamProfile(gyro_range, gyro_rate); + std::shared_ptr imuConfig = std::make_shared(); + imuConfig->enableStream(accelProfile); + imuConfig->enableStream(gyroProfile); + imuPipeline_->start(imuConfig, [&](std::shared_ptr frame){ + auto frameSet = frame->as(); + auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL); + auto gFrame = frameSet->getFrame(OB_FRAME_GYRO); + onNewIMUFrameSyncOutputCallback(aFrame, gFrame); + }); + + imu_sync_output_start_ = true; + if (!imu_sync_output_start_) { + 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(); + 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 &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(); + 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 &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) { - if (enable_stream_[stream_index] && !imu_started_[stream_index]) { - RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU stream: " - << magic_enum::enum_name(stream_index.first) - << ", please check the imu_rate and imu_range parameters"); + for (const auto &stream_index : HID_STREAMS) { + if (enable_stream_[stream_index] && !imu_started_[stream_index]) { + RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU stream: " + << magic_enum::enum_name(stream_index.first) + << ", please check the imu_rate and imu_range parameters"); + } } } } @@ -355,12 +429,23 @@ void OBCameraNode::stopStreams() { } void OBCameraNode::stopIMU() { - 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; + if (enable_sync_output_accel_gyro_) { + if (!imu_sync_output_start_ || !imuPipeline_) { + return; + } + try { + 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]; } + setAndGetNodeParameter(enable_sync_output_accel_gyro_, "enable_sync_output_accel_gyro", false); for (const auto &stream_index : HID_STREAMS) { std::string param_name = stream_name_[stream_index] + "_qos"; setAndGetNodeParameter(imu_qos_[stream_index], param_name, "default"); param_name = "enable_" + stream_name_[stream_index]; 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"; setAndGetNodeParameter(imu_rate_[stream_index], param_name, ""); param_name = stream_name_[stream_index] + "_range"; @@ -462,7 +551,7 @@ void OBCameraNode::getParameters() { camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame"; param_name = stream_name_[stream_index] + "_optical_frame_id"; setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id); - depth_aligned_frame_id_[stream_index] = stream_name_[COLOR] + "_optical_frame"; + depth_aligned_frame_id_[stream_index] = camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame"; } setAndGetNodeParameter(publish_tf_, "publish_tf", true); @@ -477,6 +566,10 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false); setAndGetNodeParameter(enable_hardware_d2d_, "enable_hardware_d2d", true); setAndGetNodeParameter(enable_soft_filter_, "enable_soft_filter", true); + setAndGetNodeParameter(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_color_auto_exposure_, "enable_color_auto_exposure", true); setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true); @@ -499,6 +592,7 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(soft_filter_speckle_size_, "soft_filter_speckle_size", -1); setAndGetNodeParameter(liner_accel_cov_, "linear_accel_cov", 0.0003); setAndGetNodeParameter(angular_vel_cov_, "angular_vel_cov", 0.02); + setAndGetNodeParameter(ordered_pc_, "ordered_pc", false); } void OBCameraNode::setupTopics() { @@ -568,31 +662,35 @@ void OBCameraNode::setupPublishers() { topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile), camera_info_qos_profile)); } - 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( + + if (enable_sync_output_accel_gyro_) { + std::string data_topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample"; + auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]); + imu_gyro_accel_publisher_ = node_->create_publisher( 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( + data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos)); + } } } -void OBCameraNode::publishPointCloud(const std::shared_ptr &frame_set, bool isColorPointCloud) { +void OBCameraNode::publishPointCloud(const std::shared_ptr &frame_set) { try { - if (isColorPointCloud) { - if (depth_registration_ || enable_colored_point_cloud_) { - if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) { - publishColoredPointCloud(frame_set); - } + if (depth_registration_ || enable_colored_point_cloud_) { + if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) { + publishColoredPointCloud(frame_set); } } - if (!isColorPointCloud) { - if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) { - publishDepthPointCloud(frame_set); - } + if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) { + publishDepthPointCloud(frame_set); } } catch (const ob::Error &e) { RCLCPP_ERROR_STREAM(logger_, e.getMessage()); @@ -608,10 +706,8 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr &f depth_cloud_pub_->get_subscription_count() == 0) { return; } - if (!camera_param_ && depth_registration_) { + if (!camera_param_) { camera_param_ = pipeline_->getCameraParam(); - } else if (!camera_param_) { - camera_param_ = getDepthCameraParam(); } if (!camera_param_) { RCLCPP_ERROR_STREAM(logger_, "camera_param_ is null"); @@ -642,6 +738,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr &f modifier.resize(width * height); point_cloud_msg_.width = depth_frame->width(); 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_.data.resize(point_cloud_msg_.height * point_cloud_msg_.row_step); sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); @@ -655,29 +752,34 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr &f const static float max_depth = MAX_DISTANCE / depth_scale; for (uint32_t y = 0; y < height; y++) { 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) { - 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()); + 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 = depth_registration_ ? depth_aligned_frame_id_[COLOR] : optical_frame_id_[DEPTH]; point_cloud_msg_.header.stamp = timestamp; 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_); if (save_point_cloud_) { @@ -740,6 +842,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr modifier.resize(color_width * color_height); point_cloud_msg_.width = color_frame->width(); point_cloud_msg_.height = color_frame->height(); + point_cloud_msg_.is_dense = false; std::string format_str = "rgb"; point_cloud_msg_.point_step = addPointField(point_cloud_msg_, format_str, 1, sensor_msgs::msg::PointField::FLOAT32, @@ -761,34 +864,39 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr for (uint32_t y = 0; y < color_height; y++) { for (uint32_t x = 0; x < color_width; x++) { float depth = depth_data[y * depth_width + x]; + bool vaild_point = true; 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()); + 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.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_); if (save_colored_point_cloud_) { save_colored_point_cloud_ = false; @@ -826,8 +934,10 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr &fr colorFrameQueue_.push(frame_set); colorFrameCV_.notify_all(); } + else { + publishPointCloud(frame_set); + } - publishPointCloud(frame_set, false); for (const auto &stream_index : IMAGE_STREAMS) { if (enable_stream_[stream_index]) { auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first); @@ -869,7 +979,7 @@ void OBCameraNode::onNewColorFrameCallback() { std::shared_ptr frameSet = colorFrameQueue_.front(); is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_); - publishPointCloud(frameSet, true); + publishPointCloud(frameSet); onNewFrameCallback(frameSet->colorFrame(), IMAGE_STREAMS.at(2)); colorFrameQueue_.pop(); } @@ -1009,12 +1119,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, int height = static_cast(video_frame->height()); auto timestamp = frameTimeStampToROSTime(video_frame->systemTimeStamp()); - if (!camera_param_ && depth_registration_) { + if(!camera_param_) { 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 = 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 &accelframe, + const std::shared_ptr &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(); + 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(); + 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 &frame, const stream_index_pair &stream_index) { if (!imu_publishers_.count(stream_index)) { @@ -1260,8 +1398,14 @@ void OBCameraNode::calcAndPublishStaticTransform() { Q = transform.getRotation(); trans = transform.getOrigin(); rclcpp::Time tf_timestamp = node_->now(); + auto device_info = device_->getDeviceInfo(); + auto pid = device_info->pid(); 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], optical_frame_id_[COLOR]); } @@ -1269,8 +1413,13 @@ void OBCameraNode::calcAndPublishStaticTransform() { if (stream_index == COLOR || !enable_stream_[stream_index]) { continue; } + if(pid != FEMTO_BOLT_PID){ publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, 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], optical_frame_id_[stream_index]); } diff --git a/orbbec_camera/src/utils.cpp b/orbbec_camera/src/utils.cpp index c7fd7f6d..f0d395c0 100644 --- a/orbbec_camera/src/utils.cpp +++ b/orbbec_camera/src/utils.cpp @@ -22,15 +22,19 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic, OBCameraDistortion distortion, int width) { (void)width; 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.height = intrinsic.height; - info.d.resize(5, 0.0); + info.d.resize(8, 0.0); info.d[0] = distortion.k1; info.d[1] = distortion.k2; - info.d[2] = distortion.k3; - info.d[3] = distortion.k4; - info.d[4] = distortion.k5; + info.d[2] = distortion.p1; + info.d[3] = distortion.p2; + 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[0] = intrinsic.fx;