From acb2750ce52e969726b4ae1f1c659155e00a866c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 12:35:43 +0200 Subject: [PATCH 1/5] [WIP] Update OD module --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 33 ++++++++++++++----- zed_wrapper/params/common.yaml | 3 +- zed_wrapper/params/zed.yaml | 2 +- zed_wrapper/params/zed2.yaml | 2 +- zed_wrapper/params/zed2i.yaml | 2 +- zed_wrapper/params/zedm.yaml | 2 +- zed_wrapper/params/zedx.yaml | 2 +- zed_wrapper/params/zedxm.yaml | 2 +- 8 files changed, 32 insertions(+), 16 deletions(-) diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index 90da3015..bf8b21d6 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1086,16 +1086,33 @@ void ZEDWrapperNodelet::readObjDetParams() } NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - int model; - mNhNs.getParam("object_detection/model", model); - if (model < 0 || model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) + std::string model_str; + mNhNs.getParam("object_detection/model", model_str); + + NODELET_DEBUG_STREAM(" 'object_detection.model': " << model_str.c_str()); + + bool matched = false; + for (int idx = static_cast(sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST); + idx < static_cast(sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS); idx++) + { + sl::OBJECT_DETECTION_MODEL test_model = static_cast(idx); + std::string test_model_str = sl::toString(test_model).c_str(); + std::replace(test_model_str.begin(), test_model_str.end(), ' ', + '_'); // Replace spaces with underscores to match the YAML setting + // NODELETDEBUG(" Comparing '%s' to '%s'", test_model_str.c_str(), model_str.c_str()); + if (model_str == test_model_str) + { + mObjDetModel = test_model; + matched = true; + break; + } + } + if (!matched) { - NODELET_WARN("Detection model not valid. Forced to the default value"); - model = static_cast(mObjDetModel); + NODELET_WARN_STREAM( "The value of the parameter 'object_detection.model' is not valid: '" + << model_str << "'. Using the default value."); } - mObjDetModel = static_cast(model); - - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + NODELET_INFO_STREAM(" * Object Det. model:\t" << sl::toString(mObjDetModel).c_str()); mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 0982426e..9ad141a9 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -80,8 +80,7 @@ sensors: object_detection: od_enabled: false # True to enable Object Detection [not available for ZED] - model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] + model: 'MULTI_CLASS_BOX_ACCURATE' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models diff --git a/zed_wrapper/params/zed.yaml b/zed_wrapper/params/zed.yaml index 0dada5cd..15212700 100644 --- a/zed_wrapper/params/zed.yaml +++ b/zed_wrapper/params/zed.yaml @@ -9,4 +9,4 @@ general: depth: min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 6f3cfeda..e6f18f53 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -9,5 +9,5 @@ general: depth: min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 20.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 \ No newline at end of file diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index 08510fa4..2742f370 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -9,4 +9,4 @@ general: depth: min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index ca3f66c2..f328af8e 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -9,4 +9,4 @@ general: depth: min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.3 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 20.0 + max_depth: 15.0 # Max: 20.0 diff --git a/zed_wrapper/params/zedx.yaml b/zed_wrapper/params/zedx.yaml index 7d364165..9778857c 100644 --- a/zed_wrapper/params/zedx.yaml +++ b/zed_wrapper/params/zedx.yaml @@ -9,5 +9,5 @@ general: depth: min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 diff --git a/zed_wrapper/params/zedxm.yaml b/zed_wrapper/params/zedxm.yaml index dd931d84..f3e91fac 100644 --- a/zed_wrapper/params/zedxm.yaml +++ b/zed_wrapper/params/zedxm.yaml @@ -9,4 +9,4 @@ general: depth: min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 From 25ab3e9a5cae71ad62f4c34f264b32f9beacea1c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 14:54:52 +0200 Subject: [PATCH 2/5] Rework and fix OD module --- CHANGELOG.rst | 8 + README.md | 3 +- zed_nodelets/CMakeLists.txt | 1 + .../include/zed_wrapper_nodelet.hpp | 21 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 350 +++++++++--------- zed_wrapper/params/common.yaml | 25 +- 6 files changed, 202 insertions(+), 206 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 68fc1b60..d7fd4390 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,14 @@ CHANGELOG ========= +2023-09-15 +---------- +- Remove start_object_detection and stop_object_detection services +- Add enable_object_detection service that takes a bool as parameter to enable/disable the OD module (same behavior as in ROS 2 Wrapper) +- Add parameter 'object_detection/allow_reduced_precision_inference' +- Add parameter 'object_detection/prediction_timeout' + + 2023-09-15 ---------- - Add pose and odometry status publishers diff --git a/README.md b/README.md index 16fcbc35..c89700f6 100644 --- a/README.md +++ b/README.md @@ -30,6 +30,7 @@ The zed_ros_wrapper is a catkin package. It depends on the following ROS package - sensor_msgs - stereo_msgs - std_msgs +- std_srvs - message_filters - tf2_ros - nodelet @@ -106,7 +107,7 @@ The SDK v3.0 introduces the Object Detection and Tracking module. **The Object D The Object Detection can be enabled *automatically* when the node start setting the parameter `object_detection/od_enabled` to `true` in the file `common.yaml`. -The Object Detection can be enabled/disabled *manually* calling the services `start_object_detection` and `stop_object_detection`. +The Object Detection can be enabled/disabled *manually* calling the service `enable_object_detection`. ### Body Tracking The Body Tracking module is not available for the ZED ROS Wrapper. Please consider migrating to the [ZED ROS2 Wrapper](https://github.com/stereolabs/zed-ros2-wrapper) if you need it. diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index d779d65b..811a7aaf 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -48,6 +48,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs stereo_msgs std_msgs + std_srvs message_filters tf2_ros nodelet diff --git a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp index 2768026b..8d1cb5af 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -34,6 +34,7 @@ #include #include #include +#include #include @@ -50,11 +51,9 @@ #include #include #include -#include #include #include #include -#include #include #include #include @@ -395,15 +394,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ bool on_save_3d_map(zed_interfaces::save_3d_map::Request& req, zed_interfaces::save_3d_map::Response& res); - /*! \brief Service callback to start_object_detection service + /*! \brief Service callback to enable_object_detection service */ - bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res); - - /*! \brief Service callback to stop_object_detection service - */ - bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res); + bool on_enable_object_detection(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); /*! \brief Service callback to save_area_memory service */ @@ -563,8 +556,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet ros::ServiceServer mSrvStartMapping; ros::ServiceServer mSrvStopMapping; ros::ServiceServer mSrvSave3dMap; - ros::ServiceServer mSrvStartObjDet; - ros::ServiceServer mSrvStopObjDet; + ros::ServiceServer mSrvEnableObjDet; ros::ServiceServer mSrvSaveAreaMemory; ros::ServiceServer mSrvSetRoi; ros::ServiceServer mSrvResetRoi; @@ -807,9 +799,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mObjDetEnabled = false; bool mObjDetRunning = false; bool mObjDetTracking = true; + bool mObjDetReducedPrecision = false; bool mObjDetBodyFitting = true; float mObjDetConfidence = 50.f; float mObjDetMaxRange = 10.f; + double mObjDetPredTimeout = 0.5; std::vector mObjDetFilter; bool mObjDetPeopleEnable = true; bool mObjDetVehiclesEnable = true; @@ -817,9 +811,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mObjDetAnimalsEnable = true; bool mObjDetElectronicsEnable = true; bool mObjDetFruitsEnable = true; - bool mObjDetSportsEnable = true; + bool mObjDetSportEnable = true; sl::OBJECT_DETECTION_MODEL mObjDetModel = sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM; + sl::OBJECT_FILTERING_MODE mObjFilterMode = sl::OBJECT_FILTERING_MODE::NMS3D; ros::Publisher mPubObjDet; }; // class ZEDROSWrapperNodelet diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index bf8b21d6..6f8387d6 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -53,7 +53,7 @@ ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() ZEDWrapperNodelet::~ZEDWrapperNodelet() { - //std::cerr << "Destroying " << getName() << std::endl; + // std::cerr << "Destroying " << getName() << std::endl; if (mDevicePollThread.joinable()) { @@ -77,7 +77,7 @@ ZEDWrapperNodelet::~ZEDWrapperNodelet() std::cerr << "ZED " << mZedSerialNumber << " closed." << std::endl; - //std::cerr << "ZED Nodelet '" << getName().c_str() << "' correctly stopped" << std::endl; + // std::cerr << "ZED Nodelet '" << getName().c_str() << "' correctly stopped" << std::endl; } void ZEDWrapperNodelet::onInit() @@ -688,12 +688,9 @@ void ZEDWrapperNodelet::initServices() mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); NODELET_INFO_STREAM(" * Advertised on service " << mSrvSave3dMap.getService().c_str()); - mSrvStartObjDet = - mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvStartObjDet.getService().c_str()); - mSrvStopObjDet = - mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvStopObjDet.getService().c_str()); + mSrvEnableObjDet = + mNhNs.advertiseService("enable_object_detection", &ZEDWrapperNodelet::on_enable_object_detection, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvEnableObjDet.getService().c_str()); } mSrvSvoStartRecording = @@ -1070,65 +1067,66 @@ void ZEDWrapperNodelet::readObjDetParams() NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); mNhNs.getParam("object_detection/od_enabled", mObjDetEnabled); - - if (mObjDetEnabled) - { - NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); - mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); - NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); - mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); - if (mObjDetMaxRange > mCamMaxDepth) - { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; - } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - - std::string model_str; - mNhNs.getParam("object_detection/model", model_str); - - NODELET_DEBUG_STREAM(" 'object_detection.model': " << model_str.c_str()); - - bool matched = false; - for (int idx = static_cast(sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST); - idx < static_cast(sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS); idx++) + NODELET_INFO_STREAM(" * Object Detection\t\t-> " << (mObjDetEnabled ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); + NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); + mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); + if (mObjDetMaxRange > mCamMaxDepth) + { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; + } + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); + + mNhNs.getParam("object_detection/allow_reduced_precision_inference", mObjDetReducedPrecision); + NODELET_INFO_STREAM(" * Allow reduced precision\t-> " << (mObjDetReducedPrecision ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/prediction_timeout", mObjDetPredTimeout); + NODELET_INFO_STREAM(" * Prediction Timeout\t\t-> " << mObjDetPredTimeout); + + std::string model_str; + mNhNs.getParam("object_detection/model", model_str); + + NODELET_DEBUG_STREAM(" 'object_detection.model': " << model_str.c_str()); + + bool matched = false; + for (int idx = static_cast(sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST); + idx < static_cast(sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS); idx++) + { + sl::OBJECT_DETECTION_MODEL test_model = static_cast(idx); + std::string test_model_str = sl::toString(test_model).c_str(); + std::replace(test_model_str.begin(), test_model_str.end(), ' ', + '_'); // Replace spaces with underscores to match the YAML setting + // NODELETDEBUG(" Comparing '%s' to '%s'", test_model_str.c_str(), model_str.c_str()); + if (model_str == test_model_str) { - sl::OBJECT_DETECTION_MODEL test_model = static_cast(idx); - std::string test_model_str = sl::toString(test_model).c_str(); - std::replace(test_model_str.begin(), test_model_str.end(), ' ', - '_'); // Replace spaces with underscores to match the YAML setting - // NODELETDEBUG(" Comparing '%s' to '%s'", test_model_str.c_str(), model_str.c_str()); - if (model_str == test_model_str) - { - mObjDetModel = test_model; - matched = true; - break; - } - } - if (!matched) - { - NODELET_WARN_STREAM( "The value of the parameter 'object_detection.model' is not valid: '" - << model_str << "'. Using the default value."); + mObjDetModel = test_model; + matched = true; + break; } - NODELET_INFO_STREAM(" * Object Det. model:\t" << sl::toString(mObjDetModel).c_str()); - - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); } + if (!matched) + { + NODELET_WARN_STREAM("The value of the parameter 'object_detection.model' is not valid: '" + << model_str << "'. Using the default value."); + } + NODELET_INFO_STREAM(" * Object Det. model\t\t->" << sl::toString(mObjDetModel).c_str()); + + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_sport", mObjDetSportEnable); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportEnable ? "ENABLED" : "DISABLED")); } } @@ -2001,6 +1999,12 @@ bool ZEDWrapperNodelet::start_obj_detect() return false; } + if (mDepthDisabled) + { + NODELET_WARN("Cannot start Object Detection if `depth/depth_mode` is set to `NONE`"); + return false; + } + if (!mObjDetEnabled) { return false; @@ -2015,13 +2019,44 @@ bool ZEDWrapperNodelet::start_obj_detect() NODELET_INFO_STREAM("*** Starting Object Detection ***"); sl::ObjectDetectionParameters od_p; - od_p.allow_reduced_precision_inference = false; od_p.enable_segmentation = false; od_p.enable_tracking = mObjDetTracking; - od_p.image_sync = false; // Asynchronous object detection + od_p.image_sync = true; od_p.detection_model = mObjDetModel; + od_p.filtering_mode = mObjFilterMode; + od_p.prediction_timeout_s = mObjDetPredTimeout; + od_p.allow_reduced_precision_inference = mObjDetReducedPrecision; od_p.max_range = mObjDetMaxRange; - od_p.instance_module_id = 0; + + mObjDetFilter.clear(); + if (mObjDetPeopleEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); + } + if (mObjDetVehiclesEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); + } + if (mObjDetBagsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); + } + if (mObjDetAnimalsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); + } + if (mObjDetElectronicsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); + } + 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); @@ -2042,42 +2077,6 @@ bool ZEDWrapperNodelet::start_obj_detect() NODELET_INFO_STREAM(" * Advertised on topic " << mPubObjDet.getTopic()); } - mObjDetFilter.clear(); - - if (mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE || - mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM || - mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST) - { - if (mObjDetPeopleEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); - } - if (mObjDetVehiclesEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); - } - if (mObjDetBagsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); - } - if (mObjDetAnimalsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); - } - if (mObjDetElectronicsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); - } - if (mObjDetFruitsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); - } - if (mObjDetSportsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); - } - } - mObjDetRunning = true; return false; } @@ -2086,10 +2085,24 @@ void ZEDWrapperNodelet::stop_obj_detect() { if (mObjDetRunning) { - NODELET_INFO_STREAM("*** Stopping Object Detection ***"); + NODELET_INFO("*** Stopping Object Detection ***"); mObjDetRunning = false; mObjDetEnabled = false; mZed.disableObjectDetection(); + + // ----> Send an empty message to indicate that no more objects are tracked + // (e.g clean Rviz2) + zed_interfaces::ObjectsStampedPtr objMsg = boost::make_shared(); + + objMsg->header.stamp = mFrameTimestamp; + objMsg->header.frame_id = mLeftCamFrameId; + + objMsg->objects.clear(); + + NODELET_DEBUG_STREAM("Publishing EMPTY OBJ message " << mPubObjDet.getTopic().c_str()); + mPubObjDet.publish(objMsg); + // <---- Send an empty message to indicate that no more objects are tracked + // (e.g clean Rviz2) } } @@ -3973,12 +3986,15 @@ void ZEDWrapperNodelet::device_poll_thread_func() mMappingMutex.unlock(); // Start the object detection? - mObjDetMutex.lock(); - if (mObjDetEnabled && !mObjDetRunning) + if (!mDepthDisabled) { - start_obj_detect(); + mObjDetMutex.lock(); + if (mObjDetEnabled && !mObjDetRunning) + { + start_obj_detect(); + } + mObjDetMutex.unlock(); } - mObjDetMutex.unlock(); // Detect if one of the subscriber need to have the depth information mComputeDepth = !mDepthDisabled && @@ -4656,7 +4672,7 @@ bool ZEDWrapperNodelet::on_start_svo_recording(zed_interfaces::start_svo_recordi if (mRecording) { res.result = false; - res.info = "Recording was just active"; + res.info = "Recording was already active"; return false; } @@ -4971,7 +4987,7 @@ bool ZEDWrapperNodelet::on_start_3d_mapping(zed_interfaces::start_3d_mapping::Re { if (mMappingEnabled && mMappingRunning) { - NODELET_WARN_STREAM("Spatial mapping was just running"); + NODELET_WARN_STREAM("Spatial mapping was already running"); res.done = false; return res.done; @@ -5053,10 +5069,11 @@ bool ZEDWrapperNodelet::on_save_3d_map(zed_interfaces::save_3d_map::Request& req return true; } -bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res) +bool ZEDWrapperNodelet::on_enable_object_detection(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { - NODELET_INFO("Called 'start_object_detection' service"); + NODELET_INFO("Called 'enable_object_detection' service"); + + std::lock_guard lock(mObjDetMutex); if (mZedRealCamModel == sl::MODEL::ZED) { @@ -5064,83 +5081,56 @@ bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_d mObjDetRunning = false; NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); - res.done = false; - return res.done; + res.message = "OObject detection not started. OD is not available for ZED camera model"; + res.success = false; + return res.success; } - if (mObjDetEnabled && mObjDetRunning) + if (req.data) { - NODELET_WARN_STREAM("Object Detection was just running"); - - res.done = false; - return res.done; - } + NODELET_INFO("Starting Object Detection"); + // Start + if (mObjDetEnabled && mObjDetRunning) + { + NODELET_WARN("Object Detection is already running"); + res.message = "Object Detection is already running"; + res.success = false; + return res.success; + } - mObjDetRunning = false; + mObjDetEnabled = true; - mObjDetConfidence = req.confidence; - mObjDetTracking = req.tracking; - if (req.model < 0 || req.model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) - { - NODELET_ERROR_STREAM("Object Detection model not valid."); - res.done = false; - return res.done; + if (start_obj_detect()) + { + res.message = "Object Detection started"; + res.success = true; + return res.success; + } + else + { + res.message = "Error occurred starting Object Detection. See log for more info"; + res.success = false; + return res.success; + } } - mObjDetModel = static_cast(req.model); - - mObjDetMaxRange = req.max_range; - if (mObjDetMaxRange > mCamMaxDepth) - { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; - } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - - NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); - - mObjDetRunning = false; - mObjDetEnabled = true; - res.done = true; - - return res.done; -} - -/*! \brief Service callback to stop_object_detection service - */ -bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res) -{ - if (mObjDetEnabled) + else { - mObjDetMutex.lock(); + NODELET_INFO("Stopping Object Detection"); + // Stop + if (!mObjDetEnabled || !mObjDetRunning) + { + NODELET_WARN("Object Detection was not running"); + res.message = "Object Detection was not running"; + res.success = false; + return res.success; + } + stop_obj_detect(); - mObjDetMutex.unlock(); - res.done = true; + res.message = "Object Detection stopped"; + res.success = true; + return res.success; } - else - { - res.done = false; - } - - return res.done; } void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 9ad141a9..f2c752bb 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -79,15 +79,16 @@ sensors: publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: - od_enabled: false # True to enable Object Detection [not available for ZED] - model: 'MULTI_CLASS_BOX_ACCURATE' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' - max_range: 15. # Maximum detection range - object_tracking_enabled: true # Enable/disable the tracking of the detected objects - mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models - mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models - mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models - mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models - mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models - mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models - mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models - + od_enabled: false # True to enable Object Detection [not available for ZED] + model: 'MULTI_CLASS_BOX_ACCURATE' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' + max_range: 15. # Maximum detection range + allow_reduced_precision_inference: true # Allow inference to run at a lower precision to improve runtime and memory usage + prediction_timeout: 0.5 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + object_tracking_enabled: true # Enable/disable the tracking of the detected objects + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models + mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models From c3f68db67cfc03509fa5b82d3990f0e2eeea3b2d Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 15:00:50 +0200 Subject: [PATCH 3/5] Update Changelog --- CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d7fd4390..e38d5683 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,7 +7,7 @@ CHANGELOG - Add enable_object_detection service that takes a bool as parameter to enable/disable the OD module (same behavior as in ROS 2 Wrapper) - Add parameter 'object_detection/allow_reduced_precision_inference' - Add parameter 'object_detection/prediction_timeout' - +- The parameter 'object_detection/model' is no more an integer, but a string with the full name of the supported OD model 2023-09-15 ---------- From bfd23df6e7a6572fa1e24bf6b1f4aea5f7c9aba5 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 17:45:55 +0200 Subject: [PATCH 4/5] Update zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp Co-authored-by: Matt Rousseau <32394882+mattrouss@users.noreply.github.com> --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index 6f8387d6..445d6fab 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -5081,7 +5081,7 @@ bool ZEDWrapperNodelet::on_enable_object_detection(std_srvs::SetBool::Request& r mObjDetRunning = false; NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); - res.message = "OObject detection not started. OD is not available for ZED camera model"; + res.message = "Object detection not started. OD is not available for ZED camera model"; res.success = false; return res.success; } From ce0d2c3da27fa8f5ebdc21ff8d04e341b3ba5484 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 18:34:11 +0200 Subject: [PATCH 5/5] Fix enable_object_detection service behavior --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index 6f8387d6..de8e1c9e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2078,7 +2078,7 @@ bool ZEDWrapperNodelet::start_obj_detect() } mObjDetRunning = true; - return false; + return true; } void ZEDWrapperNodelet::stop_obj_detect()