From 345f95321269a83db1be5dc3b7ac6a671b397c06 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 24 Feb 2022 12:21:27 +0100 Subject: [PATCH 1/5] Add support for sport-related OD objects --- .../include/zed_camera_component.hpp | 1 + .../zed_camera/src/zed_camera_component.cpp | 23 +++++++++++++++---- zed_wrapper/config/common.yaml | 10 ++++---- zed_wrapper/config/zed2.yaml | 18 +++++++-------- zed_wrapper/config/zed2i.yaml | 17 +++++++------- zed_wrapper/config/zedm.yaml | 15 ++++++------ 6 files changed, 50 insertions(+), 34 deletions(-) diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp index ab5b99c1..15a4f984 100644 --- a/zed_components/src/zed_camera/include/zed_camera_component.hpp +++ b/zed_components/src/zed_camera/include/zed_camera_component.hpp @@ -320,6 +320,7 @@ class ZedCamera : public rclcpp::Node bool mObjDetAnimalsEnable = true; bool mObjDetElectronicsEnable = true; bool mObjDetFruitsEnable = true; + bool mObjDetSportEnable = true; bool mObjDetBodyFitting = false; sl::BODY_FORMAT mObjDetBodyFmt = sl::BODY_FORMAT::POSE_34; sl::DETECTION_MODEL mObjDetModel = sl::DETECTION_MODEL::HUMAN_BODY_FAST; diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp index 1ba2d532..d76e9c00 100644 --- a/zed_components/src/zed_camera/src/zed_camera_component.cpp +++ b/zed_components/src/zed_camera/src/zed_camera_component.cpp @@ -270,7 +270,7 @@ void ZedCamera::getDebugParams() std::string paramName; RCLCPP_INFO(get_logger(), "*** DEBUG parameters ***"); - + getParam("general.debug_mode", mDebugMode, mDebugMode); RCLCPP_INFO(get_logger(), " * Debug mode: %s", mDebugMode ? "TRUE" : "FALSE"); if (mDebugMode) { @@ -328,7 +328,7 @@ void ZedCamera::getGeneralParams() getParam("general.sdk_verbose", mVerbose, mVerbose, " * SDK Verbose: "); getParam("general.svo_file", std::string(), mSvoFilepath, " * SVO: "); - if (mSvoFilepath.compare("live")==0) // Patch for launch file not allowing empty strings as default parameters + if (mSvoFilepath.compare("live") == 0) // Patch for launch file not allowing empty strings as default parameters { mSvoFilepath = ""; } @@ -1165,6 +1165,12 @@ void ZedCamera::getOdParams() RCLCPP_INFO_STREAM(get_logger(), " * MultiClassBox fruits and vegetables: " << (mObjDetFruitsEnable ? "TRUE" : "FALSE")); + getParam("object_detection.mc_sport", + mObjDetSportEnable, + mObjDetSportEnable); + RCLCPP_INFO_STREAM(get_logger(), + " * MultiClassBox sport-related objects: " + << (mObjDetSportEnable ? "TRUE" : "FALSE")); getParam("object_detection.body_fitting", mObjDetBodyFitting, mObjDetBodyFitting); RCLCPP_INFO_STREAM( get_logger(), " * Skeleton fitting: " << (mObjDetBodyFitting ? "TRUE" : "FALSE")); @@ -1252,7 +1258,7 @@ void ZedCamera::getOdParams() rcl_interfaces::msg::SetParametersResult ZedCamera::callback_paramChange(std::vector parameters) { - RCLCPP_INFO(get_logger(), "Parameter change callback"); + //RCLCPP_INFO(get_logger(), "Parameter change callback"); rcl_interfaces::msg::SetParametersResult result; result.successful = false; @@ -2250,7 +2256,7 @@ bool ZedCamera::startCamera() // <---- TF2 Transform // ----> ZED configuration - if (!mSvoFilepath.empty()) { + if (!mSvoFilepath.empty()) { RCLCPP_INFO(get_logger(), "*** SVO OPENING ***"); mInitParams.input.setFromSVOFile(mSvoFilepath.c_str()); @@ -2933,8 +2939,12 @@ bool ZedCamera::startObjDetect() if (mObjDetFruitsEnable) { mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); } + if (mObjDetSportEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); + } - sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); + sl::ERROR_CODE objDetError + = mZed.enableObjectDetection(od_p); if (objDetError != sl::ERROR_CODE::SUCCESS) { RCLCPP_ERROR_STREAM( @@ -4862,6 +4872,9 @@ void ZedCamera::processDetectedObjects(rclcpp::Time t) if (mObjDetFruitsEnable) { mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); } + if (mObjDetSportEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); + } objectTracker_parameters_rt.object_class_filter = mObjDetFilter; // <---- Process realtime dynamic parameters diff --git a/zed_wrapper/config/common.yaml b/zed_wrapper/config/common.yaml index fc1f4834..d0d29d16 100644 --- a/zed_wrapper/config/common.yaml +++ b/zed_wrapper/config/common.yaml @@ -8,7 +8,7 @@ ros__parameters: general: svo_file: '' - svo_loop: true # Enable loop mode when using an SVO as input source + svo_loop: false # Enable loop mode when using an SVO as input source svo_realtime: false # if true SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting camera_timeout_sec: 5 camera_max_reconnect: 5 @@ -23,7 +23,7 @@ video: extrinsic_in_camera_frame: false # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] - img_downsample_factor: 0.5 # Resample factor for image data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices + img_downsample_factor: 1.0 # Resample factor for image data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices brightness: 4 # [DYNAMIC] contrast: 4 # [DYNAMIC] hue: 0 # [DYNAMIC] @@ -41,13 +41,13 @@ qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE depth: - quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA - Note: if '0' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + quality: 4 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA - '4': NEURAL - Note: if '0' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) sensing_mode: 0 # '0': STANDARD, '1': FILL depth_stabilization: true # Forces positional tracking to start if 'true' openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] - depth_downsample_factor: 0.5 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) + depth_downsample_factor: 1.0 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) point_cloud_freq: 10.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) - depth_confidence: 50 # [DYNAMIC] + depth_confidence: 30 # [DYNAMIC] depth_texture_conf: 100 # [DYNAMIC] qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL qos_depth: 1 # Queue size if using KEEP_LAST diff --git a/zed_wrapper/config/zed2.yaml b/zed_wrapper/config/zed2.yaml index 5875756d..a607b84f 100644 --- a/zed_wrapper/config/zed2.yaml +++ b/zed_wrapper/config/zed2.yaml @@ -28,16 +28,16 @@ object_detection: od_enabled: false # True to enable Object Detection [only ZED 2] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] - model: 3 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX - mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + model: 6 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models + mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models + mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models + mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_X' models + mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models + mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models + mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models - body_format: 1 # '0': POSE_18 - '1': POSE_34 [Only if `HUMAN_BODY_*` model is selected] qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL qos_depth: 1 # Queue size if using KEEP_LAST qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - - qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE \ No newline at end of file diff --git a/zed_wrapper/config/zed2i.yaml b/zed_wrapper/config/zed2i.yaml index 7da3927d..c76f801d 100644 --- a/zed_wrapper/config/zed2i.yaml +++ b/zed_wrapper/config/zed2i.yaml @@ -26,15 +26,16 @@ qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE object_detection: - od_enabled: false # True to enable Object Detection [only ZED 2] + od_enabled: false # True to enable Object Detection [not available for ZED] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] - model: 2 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX - mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + model: 6 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models + mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models + mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models + mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_X' models + mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models + mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models + mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL qos_depth: 1 # Queue size if using KEEP_LAST diff --git a/zed_wrapper/config/zedm.yaml b/zed_wrapper/config/zedm.yaml index d3026be1..8af3974b 100644 --- a/zed_wrapper/config/zedm.yaml +++ b/zed_wrapper/config/zedm.yaml @@ -26,15 +26,16 @@ qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE object_detection: - od_enabled: false # True to enable Object Detection [only ZED 2] + od_enabled: false # True to enable Object Detection [not available for ZED] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] model: 6 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX - mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models + mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models + mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models + mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_X' models + mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models + mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models + mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL qos_depth: 1 # Queue size if using KEEP_LAST From 93e3ebfaffa9646727a7db1380d7a2d25514ad18 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 24 Feb 2022 18:58:03 +0100 Subject: [PATCH 2/5] add depth.remove_saturated_areas dynamic parameter --- README.md | 2 +- last_changes.md | 4 ++ .../include/zed_camera_component.hpp | 1 + .../zed_camera/src/zed_camera_component.cpp | 47 +++++++++++++++++-- zed_wrapper/config/common.yaml | 3 +- zed_wrapper/config/zed2.yaml | 7 +-- zed_wrapper/config/zed2i.yaml | 9 ++-- zed_wrapper/config/zedm.yaml | 9 ++-- 8 files changed, 66 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index 9caed0bc..cf61a4f5 100644 --- a/README.md +++ b/README.md @@ -48,7 +48,7 @@ $ sudo apt remove ros-foxy-image-transport-plugins ros-foxy-compressed-depth-ima ### Prerequisites - [Ubuntu 20.04 (Focal Fossa)](https://releases.ubuntu.com/focal/) -- [ZED SDK](https://www.stereolabs.com/developers/release/latest/) v3.6 +- [ZED SDK](https://www.stereolabs.com/developers/release/latest/) v3.7 - [CUDA](https://developer.nvidia.com/cuda-downloads) dependency - ROS2 Foxy Fitxroy: - [Ubuntu 20.04](https://docs.ros.org/en/foxy/Installation/Linux-Install-Debians.html) diff --git a/last_changes.md b/last_changes.md index 49ff1af1..c2fe00fd 100644 --- a/last_changes.md +++ b/last_changes.md @@ -1,5 +1,9 @@ LATEST CHANGES ============== +2022-02-24 +---------- +- Add support for sport-related OD objects +- Add `remove_saturated_areas` dynamic parameter to disable depth filtering when luminance >=255 2022-01-04 ---------- diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp index 15a4f984..996c490f 100644 --- a/zed_components/src/zed_camera/include/zed_camera_component.hpp +++ b/zed_components/src/zed_camera/include/zed_camera_component.hpp @@ -356,6 +356,7 @@ class ZedCamera : public rclcpp::Node double mDepthPubRate = 15.0; double mPcPubRate = 15.0; double mFusedPcPubRate = 1.0; + bool mRemoveSatAreas = true; // <---- Dynamic params // ----> Frame IDs diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp index d76e9c00..880e70df 100644 --- a/zed_components/src/zed_camera/src/zed_camera_component.cpp +++ b/zed_components/src/zed_camera/src/zed_camera_component.cpp @@ -616,7 +616,10 @@ void ZedCamera::getDepthParams() mDepthTextConf, mDepthTextConf, " * [DYN] Depth Texture Confidence: "); - + getParam("depth.remove_saturated_areas", mRemoveSatAreas, mRemoveSatAreas); + RCLCPP_INFO(get_logger(), + " * [DYN] Remove saturated areas: %s", + mRemoveSatAreas ? "TRUE" : "FALSE"); // ------------------------------------------ paramName = "depth.qos_history"; @@ -1627,6 +1630,24 @@ ZedCamera::callback_paramChange(std::vector parameters) result.successful = true; result.reason = param.get_name() + " correctly set."; return result; + } else if (param.get_name() == "depth.remove_saturated_areas") { + rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_BOOL; + if (param.get_type() != correctType) { + result.successful = false; + result.reason = param.get_name() + " must be a " + rclcpp::to_string(correctType); + RCLCPP_WARN_STREAM(get_logger(), result.reason); + return result; + } + + mRemoveSatAreas = param.as_bool(); + + RCLCPP_INFO_STREAM(get_logger(), + "Parameter '" + << param.get_name() << "' correctly set to " + << (mRemoveSatAreas ? "TRUE" : "FALSE")); + result.successful = true; + result.reason = param.get_name() + " correctly set."; + return result; } else if (param.get_name() == "mapping.fused_pointcloud_freq") { rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; if (param.get_type() != correctType) { @@ -1788,6 +1809,24 @@ ZedCamera::callback_paramChange(std::vector parameters) result.successful = true; result.reason = param.get_name() + " correctly set."; return result; + } else if (param.get_name() == "object_detection.mc_sport") { + rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_BOOL; + if (param.get_type() != correctType) { + result.successful = false; + result.reason = param.get_name() + " must be a " + rclcpp::to_string(correctType); + RCLCPP_WARN_STREAM(get_logger(), result.reason); + return result; + } + + mObjDetSportEnable = param.as_bool(); + + RCLCPP_INFO_STREAM(get_logger(), + "Parameter '" + << param.get_name() << "' correctly set to " + << (mObjDetSportEnable ? "TRUE" : "FALSE")); + result.successful = true; + result.reason = param.get_name() + " correctly set."; + return result; } else { result.reason = param.get_name() + " is not a dynamic parameter"; } @@ -2943,7 +2982,7 @@ bool ZedCamera::startObjDetect() mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); } - sl::ERROR_CODE objDetError + sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); if (objDetError != sl::ERROR_CODE::SUCCESS) { @@ -3399,6 +3438,7 @@ void ZedCamera::threadFunc_zedGrab() mRunParams.sensing_mode = static_cast(mDepthSensingMode); mRunParams.enable_depth = false; mRunParams.measure3D_reference_frame = sl::REFERENCE_FRAME::CAMERA; + mRunParams.remove_saturated_areas = mRemoveSatAreas; // <---- Grab Runtime parameters // Infinite grab thread @@ -4313,7 +4353,7 @@ bool ZedCamera::publishVideoDepth(rclcpp::Time& out_pub_ts) grab_ts = mat_conf.timestamp; } } else { - RCLCPP_INFO(get_logger(), "Lock timeout"); + RCLCPP_DEBUG(get_logger(), "Lock timeout"); } // <---- Retrieve all required data @@ -5018,6 +5058,7 @@ void ZedCamera::applyDepthSettings() mDynParMutex.lock(); mRunParams.confidence_threshold = mDepthConf; // Update depth confidence if changed mRunParams.texture_confidence_threshold = mDepthTextConf; // Update depth texture confidence if changed + mRunParams.remove_saturated_areas = mRemoveSatAreas; mDynParMutex.unlock(); mRunParams.enable_depth = true; diff --git a/zed_wrapper/config/common.yaml b/zed_wrapper/config/common.yaml index d0d29d16..e4670711 100644 --- a/zed_wrapper/config/common.yaml +++ b/zed_wrapper/config/common.yaml @@ -17,7 +17,7 @@ serial_number: 0 resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA sdk_verbose: true - grab_frame_rate: 30 # ZED SDK internal grabbing rate + grab_frame_rate: 15 # ZED SDK internal grabbing rate pub_frame_rate: 15.0 # [DYNAMIC] - frequency of publishing of visual images and depth images gpu_id: -1 @@ -49,6 +49,7 @@ point_cloud_freq: 10.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) depth_confidence: 30 # [DYNAMIC] depth_texture_conf: 100 # [DYNAMIC] + remove_saturated_areas: true # [DYNAMIC] qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL qos_depth: 1 # Queue size if using KEEP_LAST qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - diff --git a/zed_wrapper/config/zed2.yaml b/zed_wrapper/config/zed2.yaml index a607b84f..fbc61337 100644 --- a/zed_wrapper/config/zed2.yaml +++ b/zed_wrapper/config/zed2.yaml @@ -28,7 +28,7 @@ object_detection: od_enabled: false # True to enable Object Detection [only ZED 2] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] - model: 6 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models @@ -36,8 +36,9 @@ mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models - body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + body_fitting: true # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + body_format: 1 # '0': POSE_18 - '1': POSE_34 [Only if `HUMAN_BODY_*` model is selected] qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL qos_depth: 1 # Queue size if using KEEP_LAST - qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE \ No newline at end of file diff --git a/zed_wrapper/config/zed2i.yaml b/zed_wrapper/config/zed2i.yaml index c76f801d..caedf487 100644 --- a/zed_wrapper/config/zed2i.yaml +++ b/zed_wrapper/config/zed2i.yaml @@ -26,9 +26,9 @@ qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE object_detection: - od_enabled: false # True to enable Object Detection [not available for ZED] + od_enabled: false # True to enable Object Detection [only ZED 2] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] - model: 6 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models @@ -36,8 +36,9 @@ mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models - body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + body_fitting: true # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + body_format: 1 # '0': POSE_18 - '1': POSE_34 [Only if `HUMAN_BODY_*` model is selected] qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL qos_depth: 1 # Queue size if using KEEP_LAST - qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE diff --git a/zed_wrapper/config/zedm.yaml b/zed_wrapper/config/zedm.yaml index 8af3974b..92292350 100644 --- a/zed_wrapper/config/zedm.yaml +++ b/zed_wrapper/config/zedm.yaml @@ -26,9 +26,9 @@ qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE object_detection: - od_enabled: false # True to enable Object Detection [not available for ZED] + od_enabled: false # True to enable Object Detection [only ZED 2] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] - model: 6 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models @@ -36,8 +36,9 @@ mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models - body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + body_fitting: true # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + body_format: 1 # '0': POSE_18 - '1': POSE_34 [Only if `HUMAN_BODY_*` model is selected] qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL qos_depth: 1 # Queue size if using KEEP_LAST - qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE From 9f2c9cef80649003aecf099bb09dcf3c33ec797a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 25 Feb 2022 12:55:29 +0100 Subject: [PATCH 3/5] Add parameter --- last_changes.md | 2 ++ .../include/zed_camera_component.hpp | 1 + .../zed_camera/src/zed_camera_component.cpp | 26 ++++++++++++++----- zed_wrapper/config/zed2.yaml | 1 + zed_wrapper/config/zed2i.yaml | 1 + zed_wrapper/config/zedm.yaml | 1 + 6 files changed, 26 insertions(+), 6 deletions(-) diff --git a/last_changes.md b/last_changes.md index c2fe00fd..f50a2b5d 100644 --- a/last_changes.md +++ b/last_changes.md @@ -4,6 +4,8 @@ LATEST CHANGES ---------- - Add support for sport-related OD objects - Add `remove_saturated_areas` dynamic parameter to disable depth filtering when luminance >=255 +- Add `sl::ObjectDetectionParameters::filtering_mode` parameter +- Add `depth_info` topic with current min/max depth information 2022-01-04 ---------- diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp index 996c490f..6e3465a2 100644 --- a/zed_components/src/zed_camera/include/zed_camera_component.hpp +++ b/zed_components/src/zed_camera/include/zed_camera_component.hpp @@ -324,6 +324,7 @@ class ZedCamera : public rclcpp::Node bool mObjDetBodyFitting = false; sl::BODY_FORMAT mObjDetBodyFmt = sl::BODY_FORMAT::POSE_34; sl::DETECTION_MODEL mObjDetModel = sl::DETECTION_MODEL::HUMAN_BODY_FAST; + sl::OBJECT_FILTERING_MODE mObjFilterMode = sl::OBJECT_FILTERING_MODE::NMS3D; // QoS parameters // https://github.com/ros2/ros2/wiki/About-Quality-of-Service-Settings rclcpp::QoS mVideoQos; diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp index 880e70df..1e50cd29 100644 --- a/zed_components/src/zed_camera/src/zed_camera_component.cpp +++ b/zed_components/src/zed_camera/src/zed_camera_component.cpp @@ -1136,6 +1136,12 @@ void ZedCamera::getOdParams() RCLCPP_INFO_STREAM(get_logger(), " * Object Detection model: " << model << " - " << sl::toString(mObjDetModel).c_str()); + int filtering_mode = static_cast(mObjFilterMode); + getParam("object_detection.filtering_mode", filtering_mode, filtering_mode); + mObjFilterMode = static_cast(filtering_mode); + RCLCPP_INFO_STREAM(get_logger(), + " * Object Filtering mode: " << filtering_mode << " - " + << sl::toString(mObjFilterMode).c_str()); getParam( "object_detection.mc_people", mObjDetPeopleEnable, mObjDetPeopleEnable); RCLCPP_INFO_STREAM( @@ -1173,15 +1179,22 @@ void ZedCamera::getOdParams() mObjDetSportEnable); RCLCPP_INFO_STREAM(get_logger(), " * MultiClassBox sport-related objects: " - << (mObjDetSportEnable ? "TRUE" : "FALSE")); - getParam("object_detection.body_fitting", mObjDetBodyFitting, mObjDetBodyFitting); - RCLCPP_INFO_STREAM( - get_logger(), " * Skeleton fitting: " << (mObjDetBodyFitting ? "TRUE" : "FALSE")); + << (mObjDetSportEnable ? "TRUE" : "FALSE")); int bodyFormat = 0; getParam("object_detection.body_format", bodyFormat, bodyFormat); mObjDetBodyFmt = static_cast(bodyFormat); - //RCLCPP_INFO_STREAM(get_logger(), " * Body format: " << bodyFormat << " - " << sl::toString(mObjDetBodyFmt).c_str()); - RCLCPP_INFO_STREAM(get_logger(), " * Body format: " << bodyFormat); + RCLCPP_INFO_STREAM(get_logger(), " * Body format: " << bodyFormat << " - " << sl::toString(mObjDetBodyFmt).c_str()); + if (mObjDetBodyFmt == sl::BODY_FORMAT::POSE_34) { + RCLCPP_INFO_STREAM( + get_logger(), " * Skeleton fitting: TRUE (forced by `object_detection.body_format`)"); + mObjDetBodyFitting = true; + } + else + { + getParam("object_detection.body_fitting", mObjDetBodyFitting, mObjDetBodyFitting); + RCLCPP_INFO_STREAM( + get_logger(), " * Skeleton fitting: " << (mObjDetBodyFitting ? "TRUE" : "FALSE")); + } // ------------------------------------------ paramName = "object_detection.qos_history"; @@ -2956,6 +2969,7 @@ bool ZedCamera::startObjDetect() od_p.enable_tracking = mObjDetTracking; od_p.image_sync = true; od_p.detection_model = mObjDetModel; + od_p.filtering_mode = mObjFilterMode; od_p.enable_body_fitting = mObjDetBodyFitting; od_p.body_format = mObjDetBodyFmt; diff --git a/zed_wrapper/config/zed2.yaml b/zed_wrapper/config/zed2.yaml index fbc61337..5e2e703e 100644 --- a/zed_wrapper/config/zed2.yaml +++ b/zed_wrapper/config/zed2.yaml @@ -29,6 +29,7 @@ od_enabled: false # True to enable Object Detection [only ZED 2] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + filtering_mode: 1 # '0': NONE - '1': NMS3D - '2': NMS3D_PER_CLASS mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models diff --git a/zed_wrapper/config/zed2i.yaml b/zed_wrapper/config/zed2i.yaml index caedf487..132a1db4 100644 --- a/zed_wrapper/config/zed2i.yaml +++ b/zed_wrapper/config/zed2i.yaml @@ -29,6 +29,7 @@ od_enabled: false # True to enable Object Detection [only ZED 2] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + filtering_mode: 1 # '0': NONE - '1': NMS3D - '2': NMS3D_PER_CLASS mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models diff --git a/zed_wrapper/config/zedm.yaml b/zed_wrapper/config/zedm.yaml index 92292350..5750d9d8 100644 --- a/zed_wrapper/config/zedm.yaml +++ b/zed_wrapper/config/zedm.yaml @@ -29,6 +29,7 @@ od_enabled: false # True to enable Object Detection [only ZED 2] confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100] model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + filtering_mode: 1 # '0': NONE - '1': NMS3D - '2': NMS3D_PER_CLASS mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models From c9443a2b565101dfc9b49fddc5719ee5782b8333 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 25 Feb 2022 15:45:24 +0100 Subject: [PATCH 4/5] Publish depth_info topic with min/max depth info --- last_changes.md | 2 +- .../include/zed_camera_component.hpp | 5 ++ .../zed_camera/src/zed_camera_component.cpp | 87 ++++++++++++++----- 3 files changed, 70 insertions(+), 24 deletions(-) diff --git a/last_changes.md b/last_changes.md index f50a2b5d..6ab7f5e5 100644 --- a/last_changes.md +++ b/last_changes.md @@ -5,7 +5,7 @@ LATEST CHANGES - Add support for sport-related OD objects - Add `remove_saturated_areas` dynamic parameter to disable depth filtering when luminance >=255 - Add `sl::ObjectDetectionParameters::filtering_mode` parameter -- Add `depth_info` topic with current min/max depth information +- Publish `depth_info` topic with current min/max depth information 2022-01-04 ---------- diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp index 6e3465a2..ad0f9bbb 100644 --- a/zed_components/src/zed_camera/include/zed_camera_component.hpp +++ b/zed_components/src/zed_camera/include/zed_camera_component.hpp @@ -50,6 +50,7 @@ #include #include #include + #include #include #include @@ -61,6 +62,7 @@ #include #include +#include #include #include @@ -88,6 +90,7 @@ typedef std::shared_ptr> odomPub; typedef std::shared_ptr> pathPub; typedef std::shared_ptr> objPub; +typedef std::shared_ptr> depthInfoPub; typedef std::unique_ptr imageMsgPtr; typedef std::shared_ptr camInfoMsgPtr; @@ -105,6 +108,7 @@ typedef std::unique_ptr odomMsgPtr; typedef std::unique_ptr pathMsgPtr; typedef std::unique_ptr objDetMsgPtr; +typedef std::unique_ptr depthInfoMsgPtr; typedef rclcpp::Service::SharedPtr resetOdomSrvPtr; typedef rclcpp::Service::SharedPtr resetPosTrkSrvPtr; @@ -477,6 +481,7 @@ class ZedCamera : public rclcpp::Node tempPub mPubTempR; // transfPub mPubCamImuTransf; //; objPub mPubObjDet; + depthInfoPub mPubDepthInfo; // <---- Publishers // ----> Threads and Timers diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp index 1e50cd29..13f4cd12 100644 --- a/zed_components/src/zed_camera/src/zed_camera_component.cpp +++ b/zed_components/src/zed_camera/src/zed_camera_component.cpp @@ -1179,7 +1179,7 @@ void ZedCamera::getOdParams() mObjDetSportEnable); RCLCPP_INFO_STREAM(get_logger(), " * MultiClassBox sport-related objects: " - << (mObjDetSportEnable ? "TRUE" : "FALSE")); + << (mObjDetSportEnable ? "TRUE" : "FALSE")); int bodyFormat = 0; getParam("object_detection.body_format", bodyFormat, bodyFormat); mObjDetBodyFmt = static_cast(bodyFormat); @@ -1188,12 +1188,10 @@ void ZedCamera::getOdParams() RCLCPP_INFO_STREAM( get_logger(), " * Skeleton fitting: TRUE (forced by `object_detection.body_format`)"); mObjDetBodyFitting = true; - } - else - { + } else { getParam("object_detection.body_fitting", mObjDetBodyFitting, mObjDetBodyFitting); - RCLCPP_INFO_STREAM( - get_logger(), " * Skeleton fitting: " << (mObjDetBodyFitting ? "TRUE" : "FALSE")); + RCLCPP_INFO_STREAM( + get_logger(), " * Skeleton fitting: " << (mObjDetBodyFitting ? "TRUE" : "FALSE")); } // ------------------------------------------ @@ -2079,6 +2077,7 @@ void ZedCamera::initPublishers() "Openni depth mode activated -> Units: mm, Encoding: MONO16"); } std::string depth_topic = mTopicRoot + depth_topic_root + "/depth_registered"; + std::string depth_info_topic = mTopicRoot + depth_topic_root + "/depth_info"; std::string pointcloud_topic = mTopicRoot + "point_cloud/cloud_registered"; mPointcloudFusedTopic = mTopicRoot + "mapping/fused_cloud"; @@ -2187,6 +2186,10 @@ void ZedCamera::initPublishers() "Advertised on topic: " << mPubDepth.getTopic()); RCLCPP_INFO_STREAM(get_logger(), "Advertised on topic: " << mPubDepth.getInfoTopic()); + mPubDepthInfo = create_publisher( + depth_info_topic, mDepthQos); + RCLCPP_INFO_STREAM(get_logger(), + "Advertised on topic: " << mPubDepthInfo->get_topic_name()); } mPubStereo = image_transport::create_publisher( @@ -4270,6 +4273,7 @@ bool ZedCamera::publishVideoDepth(rclcpp::Time& out_pub_ts) static size_t depthSubnumber = 0; static size_t confMapSubnumber = 0; static size_t disparitySubnumber = 0; + static size_t depthInfoSubnumber = 0; try { rgbSubnumber = mPubRgb.getNumSubscribers(); @@ -4287,6 +4291,7 @@ bool ZedCamera::publishVideoDepth(rclcpp::Time& out_pub_ts) stereoSubnumber = mPubStereo.getNumSubscribers(); stereoRawSubnumber = mPubRawStereo.getNumSubscribers(); depthSubnumber = mPubDepth.getNumSubscribers(); + depthInfoSubnumber = count_subscribers(mPubDepthInfo->get_topic_name()); confMapSubnumber = count_subscribers(mPubConfMap->get_topic_name()); disparitySubnumber = count_subscribers(mPubDisparity->get_topic_name()); } catch (...) { @@ -4308,45 +4313,47 @@ bool ZedCamera::publishVideoDepth(rclcpp::Time& out_pub_ts) static sl::Timestamp ts_depth = 0; // used to check RGB/Depth sync static sl::Timestamp grab_ts = 0; + float min_depth = 0.0f, max_depth = 0.0f; + // ----> Retrieve all required data std::unique_lock lock(mCamDataMutex, std::defer_lock); if (lock.try_lock_for(std::chrono::milliseconds(1000 / mCamGrabFrameRate))) { if (rgbSubnumber + leftSubnumber + stereoSubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); ts_rgb = mat_left.timestamp; grab_ts = mat_left.timestamp; } if (rgbRawSubnumber + leftRawSubnumber + stereoRawSubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); grab_ts = mat_left_raw.timestamp; } if (rightSubnumber + stereoSubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); grab_ts = mat_right.timestamp; } if (rightRawSubnumber + stereoRawSubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); grab_ts = mat_right_raw.timestamp; } if (rgbGraySubnumber + leftGraySubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); grab_ts = mat_left_gray.timestamp; } if (rgbGrayRawSubnumber + leftGrayRawSubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); grab_ts = mat_left_raw_gray.timestamp; } if (rightGraySubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); grab_ts = mat_right_gray.timestamp; } if (rightGrayRawSubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); grab_ts = mat_right_raw_gray.timestamp; } - if (depthSubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + if (depthSubnumber > 0 || depthInfoSubnumber > 0) { + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); grab_ts = mat_depth.timestamp; ts_depth = mat_depth.timestamp; @@ -4359,13 +4366,20 @@ bool ZedCamera::publishVideoDepth(rclcpp::Time& out_pub_ts) } } if (disparitySubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); grab_ts = mat_disp.timestamp; } if (confMapSubnumber > 0) { - retrieved = sl::ERROR_CODE::SUCCESS == mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); grab_ts = mat_conf.timestamp; } + if (depthInfoSubnumber > 0) { + retrieved |= sl::ERROR_CODE::SUCCESS == mZed.getCurrentMinMaxDepth(min_depth, max_depth); +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION == 7 && ZED_SDK_PATCH_VERSION == 0 // Units bug workaround + min_depth *= 0.001f; + max_depth *= 0.001f; +#endif + } } else { RCLCPP_DEBUG(get_logger(), "Lock timeout"); } @@ -4547,6 +4561,18 @@ bool ZedCamera::publishVideoDepth(rclcpp::Time& out_pub_ts) } // <---- Publish the disparity image if someone has subscribed to + // ----> Publish the depth info if someone has subscribed to + if (depthInfoSubnumber > 0) { + depthInfoMsgPtr depthInfoMsg = std::make_unique(); + depthInfoMsg->header.stamp = timeStamp; + depthInfoMsg->header.frame_id = mDepthOptFrameId; + depthInfoMsg->min_depth = min_depth; + depthInfoMsg->max_depth = max_depth; + + mPubDepthInfo->publish(std::move(depthInfoMsg)); + } + // <---- Publish the depth info if someone has subscribed to + // Diagnostic statistic mVideoDepthElabMean_sec->addValue(elabTimer.toc()); @@ -5053,17 +5079,32 @@ bool ZedCamera::isDepthRequired() return false; } - size_t topics_sub = 0; - try { - topics_sub = count_subscribers(mPubDepth.getTopic()) + count_subscribers(mPubConfMap->get_topic_name()) + count_subscribers(mPubDisparity->get_topic_name()) + count_subscribers(mPubCloud->get_topic_name()); - } catch (...) { + size_t tot_sub = 0; + size_t depthSub = 0; + size_t confMapSub = 0; + size_t dispSub = 0; + size_t pcSub = 0; + size_t depthInfoSub = 0; + + try + { + depthSub = mPubDepth.getNumSubscribers(); + confMapSub = count_subscribers(mPubConfMap->get_topic_name()); + dispSub = count_subscribers(mPubDisparity->get_topic_name()); + pcSub = count_subscribers(mPubCloud->get_topic_name()); + depthInfoSub = count_subscribers(mPubDepthInfo->get_topic_name()); + + tot_sub = depthSub + confMapSub + dispSub + pcSub + depthInfoSub; + } + catch (...) + { rcutils_reset_error(); RCLCPP_DEBUG(get_logger(), "isDepthRequired: Exception while counting subscribers"); return false; } - return topics_sub > 0 || isPosTrackingRequired(); + return tot_sub > 0 || isPosTrackingRequired(); } void ZedCamera::applyDepthSettings() From 278e9df7e78ec3fbec05ecdfae6aa1b43a981fa5 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 25 Feb 2022 15:56:32 +0100 Subject: [PATCH 5/5] Update changelog --- last_changes.md | 9 ++------- zed-ros2-interfaces | 2 +- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/last_changes.md b/last_changes.md index 6ab7f5e5..63b06cc9 100644 --- a/last_changes.md +++ b/last_changes.md @@ -1,18 +1,13 @@ LATEST CHANGES ============== -2022-02-24 + +v3.7.x ---------- - Add support for sport-related OD objects - Add `remove_saturated_areas` dynamic parameter to disable depth filtering when luminance >=255 - Add `sl::ObjectDetectionParameters::filtering_mode` parameter - Publish `depth_info` topic with current min/max depth information - -2022-01-04 ----------- - Fix parameter override problem (Issue #71). Thx @kevinanschau - -2021-12-16 ----------- - Add default xacro path value in `zed_camera.launch.py`. Thx @sttobia - Fix `zed-ros2-interfaces` sub-module url, changing from `ssh` to `https`. diff --git a/zed-ros2-interfaces b/zed-ros2-interfaces index 5fab23c6..efba19c4 160000 --- a/zed-ros2-interfaces +++ b/zed-ros2-interfaces @@ -1 +1 @@ -Subproject commit 5fab23c62ae6199021c35e6802d2c87ac87276a9 +Subproject commit efba19c464d6731ab8843eac49af3a20c4b2db34