Merge branch 'master' of https://github.com/stereolabs/zed-ros-wrapper

Author: jdcast

CHANGELOG.rst

@@ -1,55 +1,44 @@
 CHANGELOG
 =========
 
-2023-09-15
+05-04-2024
 ----------
+- Fix compatibility with ZED SDK v4.1
+- Add support to Positional Tracking Gen 2
+- The parameter `pos_tracking_mode` can accept the new values `GEN_1` and `GEN_2`, instead of `STANDARD` and `QUALITY`
+- Add support to `NEURAL+` depth mode. Value `NEURAL_PLUS` for the parameter `depth_mode`
+- The annoying warning `Elaboration takes longer...` is now emitted only in DEBUG mode
+
+v4.0.8
+------
+
 - 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
 - Improve odometry and pose publishing and TF broadcasting
-
-2023-09-13
-----------
 - Add ROS '.clang-format'
 - Code refactoring
 - Add 'set_roi' and 'reset_roi' services
 - Add parameter 'pos_tracking/depth_min_range'
 - Add parameter 'pos_tracking/set_as_static'
-
-2023-09-12
-----------
 - Change the parameter 'general/resolution' to 'general/grab_resolution' and replace numeric values with strings
 - Add 'general.svo_realtime' parameter
 - Change 'general/verbose' to 'general/sdk_verbose'
 - Add 'general/region_of_interest' parameter
-
-2023-09-10
-----------
 - Change the parameter 'depth/quality' to 'depth/depth_mode' and replace numeric values with strings
 - Improve the behavior of the "NO DEPTH" mode
 - Remove the parameters 'depth_downsample_factor' and 'img_downsample_factor'
 - Add the parameter 'pub_resolution' and 'pub_downscale_factor'
-
-2023-09-08
-----------
 - Add 'pos_tracking/set_gravity_as_origin' parameter. If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose.
 - Add 'pos_tracking/pos_tracking_mode' parameter. Matches the ZED SDK setting: 'QUALITY', 'STANDARD'
 - Fix the warning 'Elaboration takes longer [...]'
-
-2023-09-07
-----------
 - 'pub_frame_rate' now controls the 'InitParameters::grab_compute_capping_fps' parameter of the ZED SDK instead of controlling the frequency of a parallel thread. It's not a Dynamic parameter anymore.
 - Change 'general/camera_flip' parameter to string: 'AUTO', 'ON', 'OFF'
 - Change 'general/verbose' from bool to integer
 
-
-
 v4.0.5
 ------
 - Support for ZED SDK v4.0

README.md

@@ -17,7 +17,7 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera
 ### Prerequisites
 
 - Ubuntu 20.04
-- [ZED SDK **≥ 4.0.6**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads)
+- [ZED SDK **v4.1**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads)
 - [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu)
 
 ### Build the repository

zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp

@@ -636,7 +636,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet
 
   // Positional tracking
   bool mPosTrackingEnabled = false;
-  sl::POSITIONAL_TRACKING_MODE mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY;
+  sl::POSITIONAL_TRACKING_MODE mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::GEN_2;
   bool mPosTrackingReady = false;
   bool mPosTrackingStarted = false;
   bool mPosTrackingRequired = false;

zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp

@@ -962,19 +962,19 @@ void ZEDWrapperNodelet::readPosTrkParams()
 
     std::string pos_trk_mode;
     mNhNs.getParam("pos_tracking/pos_tracking_mode", pos_trk_mode);
-    if (pos_trk_mode == "QUALITY")
+    if (pos_trk_mode == "GEN_1")
     {
-      mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY;
+      mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::GEN_1;
     }
-    else if (pos_trk_mode == "STANDARD")
+    else if (pos_trk_mode == "GEN_2")
     {
-      mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::STANDARD;
+      mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::GEN_2;
     }
     else
     {
       NODELET_WARN_STREAM("'pos_tracking/pos_tracking_mode' not valid ('" << pos_trk_mode
                                                                           << "'). Using default value.");
-      mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY;
+      mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::GEN_2;
     }
     NODELET_INFO_STREAM(" * Positional tracking mode\t-> " << sl::toString(mPosTrkMode).c_str());
 
@@ -984,8 +984,8 @@ void ZEDWrapperNodelet::readPosTrkParams()
     mNhNs.getParam("pos_tracking/path_pub_rate", mPathPubRate);
     NODELET_INFO_STREAM(" * Path rate\t\t\t-> " << mPathPubRate << " Hz");
     mNhNs.getParam("pos_tracking/path_max_count", mPathMaxCount);
-    NODELET_INFO_STREAM(" * Path history size\t\t-> " << (mPathMaxCount == -1) ? std::string("INFINITE") :
-                                                                                 std::to_string(mPathMaxCount));
+    NODELET_INFO_STREAM(" * Path history size\t\t-> " << ((mPathMaxCount == -1) ? std::string("INFINITE") :
+                                                                                 std::to_string(mPathMaxCount)));
 
     if (mPathMaxCount < 2 && mPathMaxCount != -1)
     {
@@ -1028,9 +1028,6 @@ void ZEDWrapperNodelet::readPosTrkParams()
     NODELET_INFO_STREAM(" * Camera is static\t\t-> " << (mIsStatic ? "ENABLED" : "DISABLED"));
     mNhNs.getParam("pos_tracking/depth_min_range", mPosTrkMinDepth);
     NODELET_INFO_STREAM(" * Depth minimum range\t\t-> " << mPosTrkMinDepth);
-
-    bool mIsStatic = false;
-    double mPosTrkMinDepth = 0.0;
   }
 }
 
@@ -1495,7 +1492,7 @@ void ZEDWrapperNodelet::checkResolFps()
           NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 15 FPS.");
           mCamFrameRate = 15;
         }
-        else if (mCamFrameRate >= 23)
+        else
         {
           NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 30 FPS.");
           mCamFrameRate = 30;
@@ -1573,7 +1570,7 @@ void ZEDWrapperNodelet::checkResolFps()
         NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Forced to 30 FPS.");
         mCamFrameRate = 30;
       }
-      else if (mCamFrameRate >= 45)
+      else
       {
         NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Forced to 60 FPS.");
         mCamFrameRate = 60;
@@ -1639,8 +1636,7 @@ bool ZEDWrapperNodelet::getCamera2BaseTransform()
 {
   NODELET_DEBUG("Getting static TF from '%s' to '%s'", mCameraFrameId.c_str(), mBaseFrameId.c_str());
 
-  mCamera2BaseTransfValid = false;
-  static bool first_error = true;
+  mCamera2BaseTransfValid = false;  
 
   // ----> Static transforms
   // Sensor to Base link
@@ -1663,6 +1659,7 @@ bool ZEDWrapperNodelet::getCamera2BaseTransform()
   }
   catch (tf2::TransformException& ex)
   {
+    static bool first_error = true;
     if (!first_error)
     {
       NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what());
@@ -1690,9 +1687,7 @@ bool ZEDWrapperNodelet::getSens2CameraTransform()
 {
   NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mCameraFrameId.c_str());
 
-  mSensor2CameraTransfValid = false;
-
-  static bool first_error = true;
+  mSensor2CameraTransfValid = false;  
 
   // ----> Static transforms
   // Sensor to Camera Center
@@ -1714,6 +1709,7 @@ bool ZEDWrapperNodelet::getSens2CameraTransform()
   }
   catch (tf2::TransformException& ex)
   {
+    static bool first_error = true;
     if (!first_error)
     {
       NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what());
@@ -1742,8 +1738,7 @@ bool ZEDWrapperNodelet::getSens2BaseTransform()
 {
   NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mBaseFrameId.c_str());
 
-  mSensor2BaseTransfValid = false;
-  static bool first_error = true;
+  mSensor2BaseTransfValid = false; 
 
   // ----> Static transforms
   // Sensor to Base link
@@ -1765,6 +1760,7 @@ bool ZEDWrapperNodelet::getSens2BaseTransform()
   }
   catch (tf2::TransformException& ex)
   {
+    static bool first_error = true;
     if (!first_error)
     {
       NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what());
@@ -2016,7 +2012,6 @@ bool ZEDWrapperNodelet::start_obj_detect()
   sl::ObjectDetectionParameters od_p;
   od_p.enable_segmentation = false;
   od_p.enable_tracking = mObjDetTracking;
-  od_p.image_sync = true;
   od_p.detection_model = mObjDetModel;
   od_p.filtering_mode = mObjFilterMode;
   od_p.prediction_timeout_s = mObjDetPredTimeout;
@@ -2337,11 +2332,6 @@ void ZEDWrapperNodelet::publishPose()
   size_t poseSub = mPubPose.getNumSubscribers();
   size_t poseCovSub = mPubPoseCov.getNumSubscribers();
 
-  tf2::Transform base_pose;
-  base_pose.setIdentity();
-
-  base_pose = mMap2BaseTransf;
-
   std_msgs::Header header;
   header.stamp = mFrameTimestamp;
   header.frame_id = mMapFrameId;  // frame
@@ -2633,11 +2623,10 @@ void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e)
     resized = true;
   }
 
-  std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
+  //std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
 
   // NODELET_INFO_STREAM("Chunks: " << mFusedPC.chunks.size());
 
-  int index = 0;
   float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]);
   int updated = 0;
 
@@ -2660,13 +2649,9 @@ void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e)
         pointcloudFusedMsg->header.stamp = sl_tools::slTime2Ros(mFusedPC.chunks[c].timestamp);
       }
     }
-    else
-    {
-      index += mFusedPC.chunks[c].vertices.size();
-    }
   }
 
-  std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
+  //std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
 
   // NODELET_INFO_STREAM("Updated: " << updated);
 
@@ -2775,7 +2760,6 @@ void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInf
 
   zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters;
 
-  float baseline = zedParam.getCameraBaseline();
   depth_info_msg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
   depth_info_msg->D.resize(5);
   depth_info_msg->D[0] = zedParam.left_cam.disto[0];  // k1
@@ -2927,7 +2911,7 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config,
       if (config.auto_exposure_gain != mCamAutoExposure)
       {
         mCamAutoExposure = config.auto_exposure_gain;
-        NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED" : "DISABLED");
+        NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << (mCamAutoExposure ? "ENABLED" : "DISABLED"));
       }
       mDynParMutex.unlock();
       // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK");
@@ -2965,7 +2949,7 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config,
       if (config.auto_whitebalance != mCamAutoWB)
       {
         mCamAutoWB = config.auto_whitebalance;
-        NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED");
+        NODELET_INFO_STREAM("Reconfigure auto white balance: " << (mCamAutoWB ? "ENABLED" : "DISABLED"));
         mTriggerAutoWB = true;
       }
       else
@@ -3038,8 +3022,8 @@ void ZEDWrapperNodelet::pubVideoDepth()
   sl::Mat mat_right_gray, mat_right_raw_gray;
   sl::Mat mat_depth, mat_disp, mat_conf;
 
-  sl::Timestamp ts_rgb = 0;    // used to check RGB/Depth sync
-  sl::Timestamp ts_depth = 0;  // used to check RGB/Depth sync
+  sl::Timestamp ts_rgb = 0;      // used to check RGB/Depth sync
+  sl::Timestamp ts_depth;    // used to check RGB/Depth sync
   sl::Timestamp grab_ts = 0;
 
   // ----> Retrieve all required image data
@@ -4059,7 +4043,7 @@ void ZEDWrapperNodelet::device_poll_thread_func()
               {
                 mStopNode = true;
 
-                std::lock_guard<std::mutex> lock(mCloseZedMutex);
+                std::lock_guard<std::mutex> stop_lock(mCloseZedMutex);
                 NODELET_INFO_STREAM("Closing ZED " << mZedSerialNumber << "...");
                 if (mRecording)
                 {
@@ -4210,20 +4194,20 @@ void ZEDWrapperNodelet::device_poll_thread_func()
 
       double elab_usec = std::chrono::duration_cast<std::chrono::microseconds>(end_elab - start_elab).count();
 
-      double mean_elab_sec = mElabPeriodMean_sec->addValue(elab_usec / 1000000.);
-
-      static int count_warn = 0;
+      double mean_elab_sec = mElabPeriodMean_sec->addValue(elab_usec / 1000000.);      
 
       if (!loop_rate.sleep())
       {
+        static int count_warn = 0;
         if (mean_elab_sec > (1. / mPubFrameRate))
         {
+          
           if (++count_warn > 10)
           {
             NODELET_DEBUG_THROTTLE(1.0, "Working thread is not synchronized with the Camera grab rate");
             NODELET_DEBUG_STREAM_THROTTLE(1.0, "Expected cycle time: " << loop_rate.expectedCycleTime()
                                                                        << " - Real cycle time: " << mean_elab_sec);
-            NODELET_WARN_STREAM_THROTTLE(10.0, "Elaboration takes longer ("
+            NODELET_DEBUG_STREAM_THROTTLE(10.0, "Elaboration takes longer ("
                                                    << mean_elab_sec << " sec) than requested by the FPS rate ("
                                                    << loop_rate.expectedCycleTime()
                                                    << " sec). Please consider to "
@@ -4295,7 +4279,7 @@ void ZEDWrapperNodelet::processPointcloud(ros::Time ts)
 
 void ZEDWrapperNodelet::processCameraSettings()
 {
-  int value;
+  
   sl::VIDEO_SETTINGS setting;
   sl::ERROR_CODE err;
 
@@ -4304,6 +4288,7 @@ void ZEDWrapperNodelet::processCameraSettings()
     // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK");
     mDynParMutex.lock();
 
+    int value;
     setting = sl::VIDEO_SETTINGS::BRIGHTNESS;
     err = mZed.getCameraSettings(setting, value);
     if (err == sl::ERROR_CODE::SUCCESS && value != mCamBrightness)
@@ -4545,8 +4530,8 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic
 
         if (mPcPublishing)
         {
-          double freq = 1000000. / mPcPeriodMean_usec->getMean();
-          double freq_perc = 100. * freq / mPointCloudFreq;
+          freq = 1000000. / mPcPeriodMean_usec->getMean();
+          freq_perc = 100. * freq / mPointCloudFreq;
           stat.addf("Point Cloud", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc);
         }
         else
@@ -4578,8 +4563,8 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic
 
         if (mObjDetRunning)
         {
-          double freq = 1000. / mObjDetPeriodMean_msec->getMean();
-          double freq_perc = 100. * freq / mPubFrameRate;
+          freq = 1000. / mObjDetPeriodMean_msec->getMean();
+          freq_perc = 100. * freq / mPubFrameRate;
           stat.addf("Object detection", "Mean Frequency: %.3f Hz  (%.1f%%)", freq, freq_perc);
         }
         else
@@ -4913,7 +4898,8 @@ bool ZEDWrapperNodelet::on_set_roi(zed_interfaces::set_roi::Request& req, zed_in
   // Create mask
   NODELET_INFO(" * Setting ROI");
   std::vector<sl::float2> sl_poly;
-  std::string log_msg = parseRoiPoly(parsed_poly, sl_poly);
+  //std::string log_msg = 
+  parseRoiPoly(parsed_poly, sl_poly);
   // NODELET_INFO_STREAM(" * Parsed ROI: " << log_msg.c_str());
   sl::Resolution resol(mCamWidth, mCamHeight);
   sl::Mat roi_mask(resol, sl::MAT_TYPE::U8_C1, sl::MEM::CPU);
@@ -5271,7 +5257,7 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms
   float c_x = zedParam.left_cam.cx;
   float c_y = zedParam.left_cam.cy;
 
-  float out_scale_factor = mMatResol.width / mCamWidth;
+  float out_scale_factor = static_cast<float>(mMatResol.width) / mCamWidth;
 
   float u = ((camX / camZ) * f_x + c_x) / out_scale_factor;
   float v = ((camY / camZ) * f_y + c_y) / out_scale_factor;
@@ -5649,7 +5635,6 @@ void ZEDWrapperNodelet::processPose()
       tr_2d.setZ(mFixedZValue);
       mMap2BaseTransf.setOrigin(tr_2d);
 
-      double roll, pitch, yaw;
       tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw);
 
       tf2::Quaternion quat_2d;
@@ -5665,8 +5650,6 @@ void ZEDWrapperNodelet::processPose()
                   mBaseFrameId.c_str(), mMap2BaseTransf.getOrigin().x(), mMap2BaseTransf.getOrigin().y(),
                   mMap2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG);
 
-    bool initOdom = false;
-
     // Transformation from map to odometry frame
     mOdomMutex.lock();  //
     mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse();