Merge pull request #939 from stereolabs/release_4.1

Release 4.1 compatibility
stereolabs · Apr 5, 2024 · 37d0c02 · 37d0c02
2 parents 78391c6 + aa913d2
commit 37d0c02
Show file tree

Hide file tree

Showing 5 changed files with 47 additions and 56 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -1,6 +1,14 @@
 CHANGELOG
 =========
 
+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
 ------
 

diff --git a/README.md b/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

diff --git a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp b/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;

diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp
@@ -957,19 +957,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());
 
@@ -979,8 +979,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)
     {
@@ -1023,9 +1023,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;
   }
 }
 
@@ -1490,7 +1487,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;
@@ -1568,7 +1565,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;
@@ -1634,8 +1631,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
@@ -1658,6 +1654,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());
@@ -1685,9 +1682,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
@@ -1709,6 +1704,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());
@@ -1737,8 +1733,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
@@ -1760,6 +1755,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());
@@ -2011,7 +2007,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;
@@ -2332,11 +2327,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
@@ -2628,11 +2618,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;
 
@@ -2655,13 +2644,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);
 
@@ -2770,7 +2755,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
@@ -2922,7 +2906,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");
@@ -2960,7 +2944,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
@@ -3033,8 +3017,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
@@ -4054,7 +4038,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)
                 {
@@ -4205,20 +4189,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 "
@@ -4290,7 +4274,7 @@ void ZEDWrapperNodelet::processPointcloud(ros::Time ts)
 
 void ZEDWrapperNodelet::processCameraSettings()
 {
-  int value;
+
   sl::VIDEO_SETTINGS setting;
   sl::ERROR_CODE err;
 
@@ -4299,6 +4283,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)
@@ -4540,8 +4525,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
@@ -4573,8 +4558,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
@@ -4908,7 +4893,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);
@@ -5644,7 +5630,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;
@@ -5660,8 +5645,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();

diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml
@@ -40,13 +40,13 @@ general:
 #video:
 
 depth:
-    depth_mode:                 'ULTRA'                         # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL'
+    depth_mode:                 'ULTRA'                         # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', `NEURAL_PLUS`
     depth_stabilization:        1                               # [0-100] - 0: Disabled
     openni_depth_mode:          false                           # 'false': 32bit float meter units, 'true': 16bit uchar millimeter units
 
 pos_tracking:
     pos_tracking_enabled:       true                            # True to enable positional tracking from start
-    pos_tracking_mode:          'STANDARD'                      # Matches the ZED SDK setting: 'QUALITY', 'STANDARD' -> Use 'QUALITY' with 'ULTRA' depth mode for improved outdoors performance
+    pos_tracking_mode:          'GEN_2'                          # Matches the ZED SDK setting: 'GEN_1', 'GEN_2'
     set_gravity_as_origin:      true                            # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose.
     imu_fusion:                 true                            # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
     publish_tf:                 true                            # publish `odom -> base_link` TF
@@ -75,7 +75,7 @@ mapping:
 
 sensors:
     sensors_timestamp_sync:     false                           # Synchronize Sensors messages timestamp with latest received frame
-    max_pub_rate:               400.                            # max frequency of publishing of sensors data. MAX: 400. - MIN: grab rate
+    max_pub_rate:               200.                            # max frequency of publishing of sensors data. MAX: 400. - MIN: grab rate
     publish_imu_tf:             true                            # publish `IMU -> <cam_name>_left_camera_frame` TF
 
 object_detection: