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 f536fae5..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 */ @@ -405,6 +416,21 @@ typedef struct { bool enable_direction; ///< Set to true for horizontal and vertical, false for horizontal only } ob_margin_filter_config, OBMarginFilterConfig; +/** + * @brief Configuration for mgc filter +*/ +typedef struct{ + uint32_t width; + uint32_t height; + int max_width_left; + int max_width_right; + int max_radius; + int margin_x_th; + int margin_y_th; + int limit_x_th; + int limit_y_th; +}OBMGCFilterConfig,ob_mgc_filter_config; + /** * @brief Alignment mode */ @@ -441,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; @@ -617,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/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 be444a75..00000000 --- a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.8 +++ /dev/null @@ -1 +0,0 @@ -libOrbbecSDK.so.1.8.3 \ 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.3 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.3 rename to orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.9.1 index 608a6a62..12446bf0 100644 Binary files a/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.8.3 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 be444a75..00000000 --- a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.8 +++ /dev/null @@ -1 +0,0 @@ -libOrbbecSDK.so.1.8.3 \ 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.3 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.3 rename to orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.9.1 index dd38fe08..3134414d 100644 Binary files a/orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.8.3 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 be444a75..00000000 --- a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.8 +++ /dev/null @@ -1 +0,0 @@ -libOrbbecSDK.so.1.8.3 \ 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.3 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.3 rename to orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.9.1 index 53966cea..14bdd176 100644 Binary files a/orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.8.3 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 589597b9..12c71c37 100644 --- a/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml +++ b/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml @@ -33,6 +33,13 @@ 10 + + + 1000 + + 10 + + @@ -461,6 +468,10 @@ + + 640 @@ -498,7 +509,168 @@ 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 + + + + + + 640 + + 400 + + 10 + + Y12 + + 0 + + + + 640 + + 480 + + 25 + + MJPG + + 0 + + + + 640 + + 400 + + 10 + + Y10 + + 0 + + + + + + 640 + + 400 + + 10 + + Y12 + + 0 + + + + 640 + + 480 + + 25 + + MJPG + + 0 + + + + 640 + + 400 + + 10 + + Y10 + + 0 + + @@ -552,7 +724,42 @@ 0 - + + + + 640 + + 400 + + 15 + + Y11 + + 0 + + + 30 + + + + + + 640 + + 400 + + 15 + + Y10 + + 0 + + + 30 + + + + @@ -1154,6 +1361,11 @@ 0 1 + + + @@ -1219,7 +1431,7 @@ - + @@ -1241,6 +1453,18 @@ Y8 + + + + + 640 + + 400 + + 15 + RLE + + @@ -1450,8 +1674,6 @@ 0 - - 0 @@ -1717,407 +1939,386 @@ - + 0 - - - - - 0 - - 5000 - - 0 - - 2000 - - 1280 - - 800 - - 30 - RLE - - + 1280 - + 800 - + 30 MJPG - + 0 - - - 1280 - - 800 - - 30 - Y8 - - 0 - - + 1280 - + 800 - + 30 Y8 - + 0 - + 1280 - + 800 - + 30 Y8 - + 0 - - - - - 640 - - 400 - - 30 - RLE - - 0 - - - - 640 - - 400 - - 30 - Y8 - - 0 - - - + + - - 1920 - - 1080 - + + 1280 + + 800 + 30 MJPG - + + 0 + + + + 640 + + 400 + + 30 + Y8 + + 0 + + + + 640 + + 400 + + 30 + Y8 + + 0 + + + + + + 1920 + + 1080 + + 30 + MJPG + 0 - + 1280 - + 800 - + 30 Y10 - + 0 - + 1280 - + 800 - + 30 Y10 - + 0 - + 1920 - + 1080 - + 30 MJPG - + 0 - + 1280 - + 800 - + 30 Y8 - + 0 - + 1280 - + 800 - + 30 Y8 - + 0 - + 0 - - - + 0 - + 5000 - + 5 - - 2000 - + + 5000 + 1280 - + 800 - + 10 RVL - + 1280 - + 800 - + 10 MJPG - + 0 - + 5 - - 2000 + + 5000 - + 1280 - + 800 - + 10 Y8 - + 0 - + 5 - - 2000 + + 5000 - + 1280 - + 800 - + 10 Y8 - + 0 - + 5 - - 2000 + + 5000 - + 5 - - 2000 + + 5000 - + 5 - - 2000 + + 5000 - + - + 1280 - + 800 - + 20 MJPG - + 0 - + 5 - - 2000 + + 5000 - + 640 - + 400 - + 20 RVL - + 0 - + 5 - - 2000 + + 5000 - + 640 - + 400 - + 20 Y8 - + 0 - + 5 - - 2000 + + 5000 - + 640 - + 400 - + 20 Y8 - + 0 - + 5 - - 2000 - + + 5000 + - + 1280 - + 800 - + 10 MJPG - + 0 - + 5 - - 2000 + + 5000 - + 1280 - + 800 - + 10 Y10 - + 0 - + 5 - - 2000 + + 5000 - + 1280 - + 800 - + 10 Y10 - + 0 - + 5 - - 2000 + + 5000 - + 1280 - + 800 - + 10 MJPG - + 0 - + 5 - - 2000 + + 5000 - + 1280 - + 800 - + 10 Y8 - + 0 - + 5 - - 2000 + + 5000 - + 1280 - + 800 - + 10 Y8 - + 0 - + 5 - - 2000 + + 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/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index cf1e3d76..65d110a7 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -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_; diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 2f5ce04b..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'), diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index 4a02a1f2..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'), diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 721c8510..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'), diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index 59b98527..5e806a02 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'), @@ -66,9 +67,8 @@ def generate_launch_description(): 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/Openni_device.json + DeclareLaunchArgument('depth_filter_config', default_value=''), # Depth work mode support is as follows: # Unbinned Dense Default # Unbinned Sparse Default diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index 32ac1779..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'), diff --git a/orbbec_camera/launch/gemini2VL.launch.py b/orbbec_camera/launch/gemini2VL.launch.py index caa9300b..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'), diff --git a/orbbec_camera/launch/gemini2XL.launch.py b/orbbec_camera/launch/gemini2XL.launch.py index 64289270..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'), diff --git a/orbbec_camera/scripts/99-obsensor-libusb.rules b/orbbec_camera/scripts/99-obsensor-libusb.rules index fd43dea1..d6a565ee 100644 --- a/orbbec_camera/scripts/99-obsensor-libusb.rules +++ b/orbbec_camera/scripts/99-obsensor-libusb.rules @@ -86,9 +86,15 @@ SUBSYSTEM=="usb", ATTR{idProduct}=="065e", ATTR{idVendor}=="2bc5", MODE:="0666", SUBSYSTEM=="usb", ATTR{idProduct}=="0698", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="j1" SUBSYSTEM=="usb", ATTR{idProduct}=="069c", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="TB2201" SUBSYSTEM=="usb", ATTR{idProduct}=="06a0", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_dcw2" +SUBSYSTEM=="usb", ATTR{idProduct}=="0561", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_dcw2_rgb" SUBSYSTEM=="usb", ATTR{idProduct}=="069f", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_dw2" -SUBSYSTEM=="usb", ATTR{idProduct}=="0561", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_dw2_rgb" - +SUBSYSTEM=="usb", ATTR{idProduct}=="069e", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="dabai_max_pro" +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 5939db7f..5786761d 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -178,18 +178,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()); } - 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_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) { + device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_); + } + + 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()); @@ -296,49 +315,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 +419,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 +521,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"; @@ -477,6 +556,11 @@ 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_soft_filter_ = false; + 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); @@ -569,14 +653,22 @@ 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)); + } } } @@ -1102,6 +1194,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)) {