stereolabs · Myzhar · Sep 20, 2023 · Sep 8, 2023 · Sep 11, 2023 · Sep 11, 2023
diff --git a/.clang-format b/.clang-format
@@ -0,0 +1,65 @@
+---
+BasedOnStyle: Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: true
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit: 120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard: Auto
+IndentWidth: 2
+TabWidth: 2
+UseTab: Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles: false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
diff --git a/README.md b/README.md
@@ -17,30 +17,27 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera
 ### Prerequisites
 
 - Ubuntu 20.04
-- [ZED SDK **≥ 4.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads)
+- [ZED SDK **≥ 4.0.6**](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
 
 The zed_ros_wrapper is a catkin package. It depends on the following ROS packages:
 
-   - nav_msgs
-   - tf2_geometry_msgs
-   - message_runtime
-   - catkin
-   - roscpp
-   - stereo_msgs
-   - rosconsole
-   - robot_state_publisher
-   - urdf
-   - sensor_msgs
-   - image_transport
-   - roslint
-   - diagnostic_updater
-   - dynamic_reconfigure
-   - tf2_ros
-   - message_generation
-   - nodelet
+ - roscpp
+- image_transport
+- rosconsole
+- sensor_msgs
+- stereo_msgs
+- std_msgs
+- message_filters
+- tf2_ros
+- nodelet
+- tf2_geometry_msgs
+- message_generation
+- diagnostic_updater    
+- dynamic_reconfigure
+- zed_interfaces
 
 Open a terminal, clone the repository, update the dependencies and build the packages:
 
@@ -93,7 +90,7 @@ ZED X Mini camera:
 
     $ roslaunch zed_wrapper zedxm.launch  
 
- To select the ZED from its serial number:
+ To select the camera from its serial number:
 
      $ roslaunch zed_wrapper zed.launch serial_number:=1010 #replace 1010 with the actual SN
 

diff --git a/zed-ros-interfaces b/zed-ros-interfaces
diff --git a/zed_nodelets/cfg/Zed.cfg b/zed_nodelets/cfg/Zed.cfg
@@ -6,7 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 
 group_general = gen.add_group("general")
-group_general.add("pub_frame_rate",         double_t,   0, "Video and Depth data frequency",                        15.0,   0.1,    60.0);
 
 group_depth = gen.add_group("depth")
 group_depth.add("depth_confidence",         int_t,      1, "Depth confidence threshold",                            50,     1,      100)

diff --git a/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp b/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp
@@ -55,7 +55,7 @@ void RgbdSensorsDemuxNodelet::onInit()
   NODELET_INFO_STREAM(" * Subscribed to topic: " << mSub.getTopic().c_str());
 }
 
-void RgbdSensorsDemuxNodelet::msgCallback(const zed_interfaces::RGBDSensorsPtr &msg)
+void RgbdSensorsDemuxNodelet::msgCallback(const zed_interfaces::RGBDSensorsPtr& msg)
 {
   if (!msg->rgb.header.stamp.isZero())
   {

diff --git a/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp b/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp
@@ -209,10 +209,10 @@ void RgbdSensorsSyncNodelet::readParameters()
   NODELET_INFO(" * sub_mag -> %s", mUseMag ? "true" : "false");
 }
 
-void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr &rgb,
-                                          const sensor_msgs::ImageConstPtr &depth,
-                                          const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo,
-                                          const sensor_msgs::CameraInfoConstPtr &depthCameraInfo)
+void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr& rgb,
+                                          const sensor_msgs::ImageConstPtr& depth,
+                                          const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo,
+                                          const sensor_msgs::CameraInfoConstPtr& depthCameraInfo)
 {
   // ----> Frequency calculation
   static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
@@ -251,20 +251,22 @@ void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr &rgb,
 
   if (rgbStamp != rgb->header.stamp.toSec())
   {
-    NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
-                  "sure the node publishing the topics doesn't override the same data after publishing them. A "
-                  "solution is to use this node within another nodelet manager. ");
-    NODELET_ERROR("Stamps: "
-                  "rgb=%f->%f",
-                  rgbStamp, rgb->header.stamp.toSec());
+    NODELET_ERROR(
+        "Input stamps changed between the beginning and the end of the callback! Make "
+        "sure the node publishing the topics doesn't override the same data after publishing them. A "
+        "solution is to use this node within another nodelet manager. ");
+    NODELET_ERROR(
+        "Stamps: "
+        "rgb=%f->%f",
+        rgbStamp, rgb->header.stamp.toSec());
   }
 }
 
-void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr &rgb,
-                                             const sensor_msgs::ImageConstPtr &depth,
-                                             const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo,
-                                             const sensor_msgs::CameraInfoConstPtr &depthCameraInfo,
-                                             const sensor_msgs::ImuConstPtr &imu)
+void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr& rgb,
+                                             const sensor_msgs::ImageConstPtr& depth,
+                                             const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo,
+                                             const sensor_msgs::CameraInfoConstPtr& depthCameraInfo,
+                                             const sensor_msgs::ImuConstPtr& imu)
 {
   // ----> Frequency calculation
   static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
@@ -309,20 +311,22 @@ void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr &r
 
   if (rgbStamp != rgb->header.stamp.toSec() || imuStamp != imu->header.stamp.toSec())
   {
-    NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
-                  "sure the node publishing the topics doesn't override the same data after publishing them. A "
-                  "solution is to use this node within another nodelet manager. ");
-    NODELET_ERROR("Stamps: "
-                  "rgb=%f->%f IMU=%f->%f",
-                  rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec());
+    NODELET_ERROR(
+        "Input stamps changed between the beginning and the end of the callback! Make "
+        "sure the node publishing the topics doesn't override the same data after publishing them. A "
+        "solution is to use this node within another nodelet manager. ");
+    NODELET_ERROR(
+        "Stamps: "
+        "rgb=%f->%f IMU=%f->%f",
+        rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec());
   }
 }
 
-void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr &rgb,
-                                             const sensor_msgs::ImageConstPtr &depth,
-                                             const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo,
-                                             const sensor_msgs::CameraInfoConstPtr &depthCameraInfo,
-                                             const sensor_msgs::MagneticFieldConstPtr &mag)
+void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr& rgb,
+                                             const sensor_msgs::ImageConstPtr& depth,
+                                             const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo,
+                                             const sensor_msgs::CameraInfoConstPtr& depthCameraInfo,
+                                             const sensor_msgs::MagneticFieldConstPtr& mag)
 {
   // ----> Frequency calculation
   static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
@@ -367,21 +371,23 @@ void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr &r
 
   if (rgbStamp != rgb->header.stamp.toSec() || magStamp != mag->header.stamp.toSec())
   {
-    NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
-                  "sure the node publishing the topics doesn't override the same data after publishing them. A "
-                  "solution is to use this node within another nodelet manager. ");
-    NODELET_ERROR("Stamps: "
-                  "rgb=%f->%f MAG=%f->%f",
-                  rgbStamp, rgb->header.stamp.toSec(), magStamp, mag->header.stamp.toSec());
+    NODELET_ERROR(
+        "Input stamps changed between the beginning and the end of the callback! Make "
+        "sure the node publishing the topics doesn't override the same data after publishing them. A "
+        "solution is to use this node within another nodelet manager. ");
+    NODELET_ERROR(
+        "Stamps: "
+        "rgb=%f->%f MAG=%f->%f",
+        rgbStamp, rgb->header.stamp.toSec(), magStamp, mag->header.stamp.toSec());
   }
 }
 
-void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr &rgb,
-                                          const sensor_msgs::ImageConstPtr &depth,
-                                          const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo,
-                                          const sensor_msgs::CameraInfoConstPtr &depthCameraInfo,
-                                          const sensor_msgs::ImuConstPtr &imu,
-                                          const sensor_msgs::MagneticFieldConstPtr &mag)
+void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr& rgb,
+                                          const sensor_msgs::ImageConstPtr& depth,
+                                          const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo,
+                                          const sensor_msgs::CameraInfoConstPtr& depthCameraInfo,
+                                          const sensor_msgs::ImuConstPtr& imu,
+                                          const sensor_msgs::MagneticFieldConstPtr& mag)
 {
   // ----> Frequency calculation
   static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
@@ -431,13 +437,14 @@ void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr &rgb,
   if (rgbStamp != rgb->header.stamp.toSec() || imuStamp != imu->header.stamp.toSec() ||
       magStamp != mag->header.stamp.toSec())
   {
-    NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
-                  "sure the node publishing the topics doesn't override the same data after publishing them. A "
-                  "solution is to use this node within another nodelet manager. ");
-    NODELET_ERROR("Stamps: "
-                  "rgb=%f->%f IMU=%f->%f MAG=%f->%f",
-                  rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec(), magStamp,
-                  mag->header.stamp.toSec());
+    NODELET_ERROR(
+        "Input stamps changed between the beginning and the end of the callback! Make "
+        "sure the node publishing the topics doesn't override the same data after publishing them. A "
+        "solution is to use this node within another nodelet manager. ");
+    NODELET_ERROR(
+        "Stamps: "
+        "rgb=%f->%f IMU=%f->%f MAG=%f->%f",
+        rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec(), magStamp, mag->header.stamp.toSec());
   }
 }
 

diff --git a/zed_nodelets/src/tools/include/sl_tools.h b/zed_nodelets/src/tools/include/sl_tools.h
@@ -52,6 +52,38 @@ std::string getSDKVersion(int& major, int& minor, int& sub_minor);
  */
 ros::Time slTime2Ros(sl::Timestamp t);
 
+/*! \brief check if ZED
+ * \param camModel the model to check
+ */
+bool isZED(sl::MODEL camModel);
+
+/*! \brief check if ZED Mini
+ * \param camModel the model to check
+ */
+bool isZEDM(sl::MODEL camModel);
+
+/*! \brief check if ZED2 or ZED2i
+ * \param camModel the model to check
+ */
+bool isZED2OrZED2i(sl::MODEL camModel);
+
+/*! \brief check if ZED-X or ZED-X Mini
+ * \param camModel the model to check
+ */
+bool isZEDX(sl::MODEL camModel);
+
+/*! \brief Creates an sl::Mat containing a ROI from a polygon
+ *  \param poly the ROI polygon. Coordinates must be normalized from 0.0 to 1.0
+ *  \param out_roi the `sl::Mat` containing the ROI
+ */
+bool generateROI(const std::vector<sl::float2>& poly, sl::Mat& out_roi);
+
+/*! \brief Parse a vector of vector of floats from a string.
+ *  \param input
+ *  \param error_return
+ *  Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */
+std::vector<std::vector<float>> parseStringVector(const std::string& input, std::string& error_return);
+
 /*! \brief sl::Mat to ros message conversion
  * \param imgMsgPtr : the image topic message to publish
  * \param img : the image to publish
+7 −0		CHANGELOG.md
+3 −0		CMakeLists.txt
+14 −0		msg/PosTrackStatus.msg
+2 −0		srv/reset_roi.srv
+8 −0		srv/set_roi.srv