diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 68fc1b60..e38d5683 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' +- 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 ---------- - 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 90da3015..a291297d 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,48 +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_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) { - 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); - - int model; - mNhNs.getParam("object_detection/model", model); - if (model < 0 || model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) - { - NODELET_WARN("Detection model not valid. Forced to the default value"); - model = static_cast(mObjDetModel); + mObjDetModel = test_model; + matched = true; + break; } - mObjDetModel = static_cast(model); - - 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")); } + 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")); } } @@ -1984,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; @@ -1998,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); @@ -2025,54 +2077,32 @@ 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; + return true; } 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) } } @@ -3956,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 && @@ -4639,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; } @@ -4954,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; @@ -5036,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) { @@ -5047,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 = "Object 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 0982426e..f2c752bb 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -79,16 +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: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] - 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 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