Merge pull request #727 from stereolabs/dev_noetic

Dev noetic

Author: Myzhar

README.md

@@ -1,6 +1,6 @@
 ![](./images/Picto+STEREOLABS_Black.jpg)
 
-# Stereolabs ZED Camera - ROS Integration
+# Stereolabs ZED Camera - ROS Noetic Ninjemis Integration
 
 This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, pose information and supports the use of multiple ZED cameras.
 
@@ -14,11 +14,13 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera
 
 ### Prerequisites
 
-- Ubuntu 16.04 or newer (Ubuntu 18 recommended)
-- [ZED SDK **≥ 3.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads)
-- [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) or [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu)
+- Ubuntu 20.04
+- [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads)
+- [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu)
 
-*Note:* an older version of the wrapper compatible with the **SDK v2.8.x** is available [here](https://github.com/stereolabs/zed-ros-wrapper/releases/tag/v2.x)
+- Ubuntu 18.04
+- [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads)
+- [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu)
 
 ### Build the program
 

latest_changes.md

@@ -1,6 +1,15 @@
 LATEST CHANGES
 ==============
 
+v3.5.x
+---------
+- Add support for ROS Noetic
+- Add support for SDK v3.5
+- Add support for the new ZED 2i
+- Add new parameter `pos_tracking/pos_tracking_enabled` to enable positional tracking from start even if not required by any subscribed topic. This is useful, for example, to keep the TF always updated.
+- Add new example to start multiple ZED Nodelets inside the same nodelet manager
+- Fixed issue #690
+
 v3.4.x
 ---------
 - Add support for new DEPTH16_MM data type for depth (OPENNI MODE)

zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp

@@ -532,9 +532,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet
   double mCamMinDepth;
   double mCamMaxDepth;
 
-  bool mTrackingActivated;
-
-  bool mTrackingReady;
+  // Positional tracking
+  bool mPosTrackingEnabled=false;
+  bool mPosTrackingActivated=false;
+  bool mPosTrackingReady=false;
   bool mTwoDMode = false;
   double mFixedZValue = 0.0;
   bool mFloorAlignment = false;
@@ -608,7 +609,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet
   double mCamImageResizeFactor = 1.0;
   double mCamDepthResizeFactor = 1.0;
 
-  // flags
+  // flags  
   bool mTriggerAutoExposure = true;
   bool mTriggerAutoWB = true;
   bool mComputeDepth;

zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp

@@ -669,6 +669,11 @@ void ZEDWrapperNodelet::readParameters()
     mZedUserCamModel = sl::MODEL::ZED2;
     NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model);
   }
+  else if (camera_model == "zed2i")
+  {
+    mZedUserCamModel = sl::MODEL::ZED2i;
+    NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model);
+  }
   else
   {
     NODELET_ERROR_STREAM("Camera model not valid: " << camera_model);
@@ -712,6 +717,8 @@ void ZEDWrapperNodelet::readParameters()
   NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***");
 
   // ----> Tracking
+  mNhNs.param<bool>("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true);
+  NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED"));
   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);
@@ -1322,7 +1329,7 @@ bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_
   std::lock_guard<std::mutex> lock(mPosTrkMutex);
 
   // Disable tracking
-  mTrackingActivated = false;
+  mPosTrackingActivated = false;
   mZed.disablePositionalTracking();
 
   // Restart tracking
@@ -1335,7 +1342,7 @@ bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_
 bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Request& req,
                                           zed_interfaces::reset_tracking::Response& res)
 {
-  if (!mTrackingActivated)
+  if (!mPosTrackingActivated)
   {
     res.reset_done = false;
     return false;
@@ -1351,7 +1358,7 @@ bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Reques
   std::lock_guard<std::mutex> lock(mPosTrkMutex);
 
   // Disable tracking
-  mTrackingActivated = false;
+  mPosTrackingActivated = false;
   mZed.disablePositionalTracking();
 
   // Restart tracking
@@ -1627,11 +1634,11 @@ void ZEDWrapperNodelet::start_pos_tracking()
 
   if (err == sl::ERROR_CODE::SUCCESS)
   {
-    mTrackingActivated = true;
+    mPosTrackingActivated = true;
   }
   else
   {
-    mTrackingActivated = false;
+    mPosTrackingActivated = false;
 
     NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str());
   }
@@ -1789,6 +1796,16 @@ void ZEDWrapperNodelet::publishStaticImuFrame()
 
 void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t)
 {
+  // ----> Avoid duplicated TF publishing
+  static ros::Time last_stamp;
+
+  if( t==last_stamp )
+  {
+    return;
+  }
+  last_stamp = t;
+  // <---- Avoid duplicated TF publishing
+
   if (!mSensor2BaseTransfValid)
   {
     getSens2BaseTransform();
@@ -1813,11 +1830,21 @@ void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t)
   // Publish transformation
   mTransformOdomBroadcaster.sendTransform(transformStamped);
 
-  // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t );
+  //NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t );
 }
 
 void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t)
 {
+  // ----> Avoid duplicated TF publishing
+  static ros::Time last_stamp;
+
+  if( t==last_stamp )
+  {
+    return;
+  }
+  last_stamp = t;
+  // <---- Avoid duplicated TF publishing
+
   if (!mSensor2BaseTransfValid)
   {
     getSens2BaseTransform();
@@ -2999,8 +3026,18 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t)
     ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp);
   }
 
+  bool new_imu_data = ts_imu != lastTs_imu;
+  bool new_baro_data = ts_baro != lastTs_baro;
+  bool new_mag_data = ts_mag != lastT_mag;
+
+  if (!new_imu_data && !new_baro_data && !new_mag_data)
+  {
+    NODELET_DEBUG("No updated sensors data");
+    return;
+  }
+
   // ----> Publish odometry tf only if enabled
-  if (mPublishTf && mTrackingReady)
+  if (mPublishTf && mPosTrackingReady && new_imu_data)
   {
     NODELET_DEBUG("Publishing TF");
 
@@ -3018,25 +3055,17 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t)
   }
   // <---- Publish odometry tf only if enabled
 
-  bool new_imu_data = ts_imu != lastTs_imu;
-  bool new_baro_data = ts_baro != lastTs_baro;
-  bool new_mag_data = ts_mag != lastT_mag;
-
-  if (!new_imu_data && !new_baro_data && !new_mag_data)
-  {
-    NODELET_DEBUG("No updated sensors data");
-    return;
-  }
-
   if (mZedRealCamModel == sl::MODEL::ZED2)
   {
     // Update temperatures for Diagnostic
     sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft);
     sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight);
   }
 
-  if (imu_TempSubNumber > 0)
+  if (imu_TempSubNumber > 0 && new_imu_data)
   {
+    lastTs_imu = ts_imu;
+
     sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared<sensor_msgs::Temperature>();
 
     imuTempMsg->header.stamp = ts_imu;
@@ -3262,6 +3291,8 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t)
 
   if (imu_RawSubNumber > 0 && new_imu_data)
   {
+    lastTs_imu = ts_imu;
+
     sensor_msgs::ImuPtr imuRawMsg = boost::make_shared<sensor_msgs::Imu>();
 
     imuRawMsg->header.stamp = ts_imu;
@@ -3347,7 +3378,7 @@ void ZEDWrapperNodelet::device_poll_thread_func()
 
   mPrevFrameTimestamp = mFrameTimestamp;
 
-  mTrackingActivated = false;
+  mPosTrackingActivated = false;
   mMappingRunning = false;
   mRecording = false;
 
@@ -3417,7 +3448,7 @@ void ZEDWrapperNodelet::device_poll_thread_func()
     }
 
     mGrabActive =
-        mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mTrackingActivated ||
+        mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingActivated ||
         ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber +
           rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber +
           rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber +
@@ -3430,11 +3461,11 @@ void ZEDWrapperNodelet::device_poll_thread_func()
       std::lock_guard<std::mutex> lock(mPosTrkMutex);
 
       // Note: ones tracking is started it is never stopped anymore to not lose tracking information
-      bool computeTracking = (mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) ||
+      bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) ||
                               poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0);
 
       // Start the tracking?
-      if ((computeTracking) && !mTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE))
+      if ((computeTracking) && !mPosTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE))
       {
         start_pos_tracking();
       }
@@ -3457,8 +3488,8 @@ void ZEDWrapperNodelet::device_poll_thread_func()
 
       // Detect if one of the subscriber need to have the depth information
       mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE &&
-                      ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber +
-                        poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0);
+                      (computeTracking || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber +
+                        poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0));
 
       if (mComputeDepth)
       {
@@ -3537,9 +3568,9 @@ void ZEDWrapperNodelet::device_poll_thread_func()
             std::this_thread::sleep_for(std::chrono::milliseconds(2000));
           }
 
-          mTrackingActivated = false;
+          mPosTrackingActivated = false;
 
-          computeTracking = mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0;
+          computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0;
 
           if (computeTracking)
           {  // Start the tracking
@@ -3829,7 +3860,7 @@ void ZEDWrapperNodelet::device_poll_thread_func()
               publishOdom(mOdom2BaseTransf, deltaOdom, stamp);
             }
 
-            mTrackingReady = true;
+            mPosTrackingReady = true;
           }
         }
         else if (mFloorAlignment)
@@ -3956,7 +3987,7 @@ void ZEDWrapperNodelet::device_poll_thread_func()
             publishPose(stamp);
           }
 
-          mTrackingReady = true;
+          mPosTrackingReady = true;
         }
 
         oldStatus = mPosTrackingStatus;
@@ -4162,7 +4193,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic
           }
         }
 
-        if (mTrackingActivated)
+        if (mPosTrackingActivated)
         {
           stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str());
         }

zed_wrapper/launch/zed2i.launch

@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2020, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+    <arg name="svo_file"             default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
+    <arg name="stream"               default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->
+
+    <arg name="node_name"            default="zed_node" />
+    <arg name="camera_model"         default="zed2i" />
+    <arg name="publish_urdf"         default="true" />
+
+    <arg name="camera_name"          default="zed2i" />
+
+    <arg name="base_frame"           default="base_link" />
+
+    <arg name="cam_pos_x"             default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
+    <arg name="cam_pos_y"             default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
+    <arg name="cam_pos_z"             default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
+    <arg name="cam_roll"              default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
+    <arg name="cam_pitch"             default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
+    <arg name="cam_yaw"               default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
+
+    <group ns="$(arg camera_name)">
+        <include file="$(find zed_wrapper)/launch/include/zed_camera.launch.xml">
+            <arg name="camera_name"         value="$(arg camera_name)" />
+            <arg name="svo_file"            value="$(arg svo_file)" />
+            <arg name="stream"              value="$(arg stream)" />
+            <arg name="node_name"           value="$(arg node_name)" />
+            <arg name="camera_model"        value="$(arg camera_model)" />
+            <arg name="base_frame"          value="$(arg base_frame)" />
+            <arg name="publish_urdf"        value="$(arg publish_urdf)" />
+            <arg name="cam_pos_x"           value="$(arg cam_pos_x)" />
+            <arg name="cam_pos_y"           value="$(arg cam_pos_y)" />
+            <arg name="cam_pos_z"           value="$(arg cam_pos_z)" />
+            <arg name="cam_roll"            value="$(arg cam_roll)" />
+            <arg name="cam_pitch"           value="$(arg cam_pitch)" />
+            <arg name="cam_yaw"             value="$(arg cam_yaw)" />
+        </include>
+    </group>
+</launch>

zed_wrapper/params/common.yaml

@@ -37,13 +37,14 @@ video:
     extrinsic_in_camera_frame:  true                            # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1]
 
 depth:
-    quality:                    3                               # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA
+    quality:                    1                               # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA
     sensing_mode:               0                               # '0': STANDARD, '1': FILL (not use FILL for robotic applications)
     depth_stabilization:        1                               # `0`: disabled, `1`: enabled
     openni_depth_mode:          false                           # 'false': 32bit float meters, 'true': 16bit uchar millimeters
     depth_downsample_factor:    1.0                             # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...)
 
 pos_tracking:
+    pos_tracking_enabled:       true                            # True to enable positional tracking from start
     publish_tf:                 true                            # publish `odom -> base_link` TF
     publish_map_tf:             true                            # publish `map -> odom` TF
     map_frame:                  'map'

zed_wrapper/params/zed2.yaml

@@ -6,7 +6,7 @@ general:
     camera_model:               'zed2'
 
 depth:
-    min_depth:                  0.7             # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory
+    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
 
 pos_tracking:
@@ -18,7 +18,7 @@ sensors:
 
 object_detection:
     od_enabled:                 false           # True to enable Object Detection [only ZED 2]
-    model:                      0               # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE
+    model:                      3               # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_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