Merge branch '11-release-4-0-6' into 'master'

Resolve "Release 4.0.6"

Closes #11

See merge request sl/zed-ros2-wrapper!28

.ci/download_and_install_sdk.sh

@@ -17,7 +17,7 @@ apt-get update -y || true
 apt-get install --no-install-recommends lsb-release wget less udev sudo  build-essential cmake zstd python3 python3-pip libpng-dev libgomp1 -y && \
     python3 -m pip install --upgrade pip; python3 -m pip install numpy opencv-python-headless && \
     wget -q -O ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run https://download.stereolabs.com/zedsdk/${ZED_SDK_MAJOR}.${ZED_SDK_MINOR}/cu${CUDA_MAJOR}${CUDA_MINOR%.*}/ubuntu${UBUNTU_RELEASE_YEAR} && \
-    chmod +x ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run ; ./ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run -- silent skip_tools skip_cuda && \
+    chmod +x ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run ; ./ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run -- silent skip_tools skip_cuda skip_python skip_hub && \
     ln -sf /lib/x86_64-linux-gnu/libusb-1.0.so.0 /usr/lib/x86_64-linux-gnu/libusb-1.0.so && \
     rm ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run && \
     rm -rf /var/lib/apt/lists/*

.ci/jetson_build_humble_src.sh

@@ -35,7 +35,7 @@ wget https://github.com/ros/xacro/archive/refs/tags/${XACRO_VERSION}.tar.gz -O -
 DIAGNOSTICS_VERSION=3.0.0
 wget https://github.com/ros/diagnostics/archive/refs/tags/${DIAGNOSTICS_VERSION}.tar.gz -O - | tar -xvz && mv diagnostics-${DIAGNOSTICS_VERSION} diagnostics
 # lint
-AMENT_LINT_VERSION=0.12.4
+AMENT_LINT_VERSION=0.12.7
 wget https://github.com/ament/ament_lint/archive/refs/tags/${AMENT_LINT_VERSION}.tar.gz -O - | tar -xvz && mv ament_lint-${AMENT_LINT_VERSION} ament-lint
 # Geographic Info
 GEOGRAPHIC_INFO_VERSION=1.0.4
@@ -61,6 +61,9 @@ cd ${WS_DIR}
 apt-get update -y || true && rosdep update
 rosdep install --from-paths src --ignore-src -r -y
 
+# force install cython to make sure all pacakges are clean
+python3 -m pip install --force-reinstall cython
+
 echo "${ttk} Build the ZED ROS2 Package and the dependencies"
 cd ${WS_DIR}
 colcon build --cmake-args ' -DCMAKE_BUILD_TYPE=Release' ' -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs' ' -DCMAKE_CXX_FLAGS="-Wl,--allow-shlib-undefined"' ' --no-warn-unused-cli' --parallel-workers $(nproc)

CHANGELOG.rst

@@ -1,27 +1,21 @@
 LATEST CHANGES
 ==============
 
-2023-08-02
+2023-08-04
+----------
+- Add support for ZED SDK v4.0.6
+- Add new GNSS calibration parameters: `enable_reinitialization`, `enable_rolling_calibration`, `enable_translation_uncertainty_target`, `gnss_vio_reinit_threshold`, `target_translation_uncertainty`, `target_yaw_uncertainty`
+- Add new Plane Detection paramters: `pd_max_distance_threshold`, `pd_normal_similarity_threshold`
+
+v4.0.5
 ----------
 - The parameter `general.pub_resolution` can now take only `NATIVE` and `CUSTOM` values. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission
 - Add new parameter `general.pub_downscale_factor` to be used with the new option `CUSTOM` for the parameter `general.pub_resolution`
 - `ULTRA` is the new default value for `depth.depth_mode` (better performance for odometry and positional tracking)
 - Add resolution `HD1080` for ZED X
-
-2023-07-31
-----------
 - Fix issue with Body Tracking start/stop by service call. Now Body Tracking can be restarted multiple times
-
-2023-06-28
-----------
 - Fix depth grab performance by removing a [not required `PNG Write` call](https://github.com/stereolabs/zed-ros2-wrapper/pull/164). Thank you Esteban Zamora @ezamoraa 
-
-2023-06-15
-----------
 - Fix bug with `general.pub_resolution` value, not allowing to select the correct data publish resolution
-
-2023-05-22
-----------
 - Add new launch parameter `ros_params_override_path` to provide the path to a custom YAML file to override the parameters of the ZED Node without modifying the original files in the `zed_wrapper/config` folder. Thank you David Lu @MetroRobots
 
 v4.0.0

docker/Dockerfile.l4t35_1-humble-release

@@ -77,6 +77,8 @@ RUN apt-get update -y || true && rosdep update && \
   rosdep install --from-paths src --ignore-src -r -y && \
   rm -rf /var/lib/apt/lists/*
 
+RUN python3 -m pip install --upgrade cython
+
 # Build the dependencies and the ZED ROS2 Wrapper
 RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/install/setup.bash && \
   colcon build --parallel-workers $(nproc) --symlink-install \

docker/README.md

@@ -67,7 +67,7 @@ If you plan to use the AI module of the ZED SDK (Object Detection, Skeleton Trac
 
 This is easily done by using the following option:
 
-    -v /tmp/zed_ai/:/usr/local/zed/resources/
+    -v ${HOME}/zed_docker_ai/:/usr/local/zed/resources/
 
 The first time you use the AI model inside the Docker image, it will be downloaded and optimized in the local bound-mounted folder, and stored there for the next runs.
 
@@ -86,7 +86,7 @@ The following command starts an interactive BaSH session:
 ```bash
 docker run --runtime nvidia -it --privileged --ipc=host --pid=host -e DISPLAY \
   -v /dev/shm:/dev/shm -v /tmp/.X11-unix/:/tmp/.X11-unix \
-  -v /tmp/zed_ai/:/usr/local/zed/resources/ \
+  -v ${HOME}/zed_docker_ai/:/usr/local/zed/resources/ \
   <image_tag>
 ```
 
@@ -96,6 +96,6 @@ For ZED X and ZED X Mini
 docker run --runtime nvidia -it --privileged --ipc=host --pid=host -e DISPLAY \
   -v /dev/shm:/dev/shm \
   -v /tmp/:/tmp/ -v /var/nvidia/nvcam/settings/:/var/nvidia/nvcam/settings/ \
-  -v /tmp/zed_ai/:/usr/local/zed/resources/ \
+  -v ${HOME}/zed_docker_ai/:/usr/local/zed/resources/ \
   <image_tag>
 ```

zed_components/CMakeLists.txt

@@ -75,7 +75,7 @@ endif()
 
 #############################################
 # Dependencies
-find_package(ZED 3 REQUIRED)
+find_package(ZED 4 REQUIRED)
 
 exec_program(uname ARGS -p OUTPUT_VARIABLE CMAKE_SYSTEM_NAME2)
 if(CMAKE_SYSTEM_NAME2 MATCHES "aarch64") # Jetson TX

zed_components/src/zed_camera/include/zed_camera_component.hpp

@@ -307,7 +307,16 @@ class ZedCamera : public rclcpp::Node
   bool mPublishPoseCov = true;
   bool mGnssFusionEnabled = false;
   std::string mGnssTopic = "/gps/fix";
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
   double mGnssInitDistance = 5.0;
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  bool mGnssEnableReinitialization = true;
+  bool mGnssEnableRollingCalibration = true;
+  bool mGnssEnableTranslationUncertaintyTarget = false;
+  double mGnssVioReinitThreshold = 5.0;
+  double mGnssTargetTranslationUncertainty = 10e-2;
+  double mGnssTargetYawUncertainty = 0.1;
+#endif
   bool mGnssZeroAltitude = false;
   bool mPublishUtmTf = true;
   bool mUtmAsParent = true;
@@ -345,6 +354,11 @@ class ZedCamera : public rclcpp::Node
   double mBodyTrkConfThresh = 50.0;
   int mBodyTrkMinKp = 10;
 
+  #if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  double mPdMaxDistanceThreshold = 0.15;
+  double mPdNormalSimilarityThreshold = 15.0;
+  #endif
+
   // TODO(Walter) remove QoS parameters, use instead the new ROS2 Humble QoS settings engine
 
   // QoS parameters
@@ -616,7 +630,13 @@ class ZedCamera : public rclcpp::Node
   bool mPosTrackingReady = false;
   sl::POSITIONAL_TRACKING_STATE mPosTrackingStatusWorld;
   sl::POSITIONAL_TRACKING_STATE mPosTrackingStatusCamera;
+
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
   sl::POSITIONAL_TRACKING_STATE mGeoPoseStatus;
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  sl::GNSS_CALIBRATION_STATE mGeoPoseStatus;
+#endif
+
   bool mResetOdom = false;
   bool mSpatialMappingRunning = false;
   bool mObjDetRunning = false;

zed_components/src/zed_camera/src/zed_camera_component.cpp

@@ -1142,6 +1142,14 @@ void ZedCamera::getMappingParams()
 
   getParam(
     "mapping.clicked_point_topic", mClickedPtTopic, mClickedPtTopic, " * Clicked point topic: ");
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  getParam(
+    "mapping.pd_max_distance_threshold", mPdMaxDistanceThreshold, mPdMaxDistanceThreshold,
+    " * Plane Det. Max Dist. Thresh.: ");
+  getParam(
+    "mapping.pd_normal_similarity_threshold", mPdNormalSimilarityThreshold,
+    mPdNormalSimilarityThreshold, " * Plane Det. Normals Sim. Thresh.: ");
+#endif
   // ------------------------------------------
 
   paramName = "mapping.qos_history";
@@ -1411,9 +1419,41 @@ void ZedCamera::getGnssFusionParams()
   if (mGnssFusionEnabled) {
     getParam("gnss_fusion.gnss_frame", mGnssFrameId, mGnssFrameId, " * GNSS frame: ");
     getParam("gnss_fusion.gnss_fix_topic", mGnssTopic, mGnssTopic, " * GNSS topic name: ");
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
     getParam(
       "gnss_fusion.gnss_init_distance", mGnssInitDistance, mGnssInitDistance,
       " * GNSS init. distance: ");
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+    getParam(
+      "gnss_fusion.enable_reinitialization", mGnssEnableReinitialization,
+      mGnssEnableReinitialization);
+    RCLCPP_INFO_STREAM(
+      get_logger(),
+      " * GNSS Reinitialization: " << (mGnssEnableReinitialization ? "TRUE" : "FALSE"));
+    getParam(
+      "gnss_fusion.enable_rolling_calibration", mGnssEnableRollingCalibration,
+      mGnssEnableRollingCalibration);
+    RCLCPP_INFO_STREAM(
+      get_logger(),
+      " * GNSS Rolling Calibration: " << (mGnssEnableRollingCalibration ? "TRUE" : "FALSE"));
+    getParam(
+      "gnss_fusion.enable_translation_uncertainty_target", mGnssEnableTranslationUncertaintyTarget,
+      mGnssEnableTranslationUncertaintyTarget);
+    RCLCPP_INFO_STREAM(
+      get_logger(),
+      " * GNSS Transl. Uncert. Target: " <<
+        (mGnssEnableTranslationUncertaintyTarget ? "TRUE" : "FALSE"));
+    getParam(
+      "gnss_fusion.gnss_vio_reinit_threshold", mGnssVioReinitThreshold, mGnssVioReinitThreshold,
+      " * GNSS VIO Reinit. Thresh.: ");
+    getParam(
+      "gnss_fusion.target_translation_uncertainty", mGnssTargetTranslationUncertainty,
+      mGnssTargetTranslationUncertainty,
+      " * GNSS Target Transl. Uncert.: ");
+    getParam(
+      "gnss_fusion.target_yaw_uncertainty", mGnssTargetYawUncertainty, mGnssTargetYawUncertainty,
+      " * GNSS Target Yaw Uncert.: ");
+#endif
 
     getParam("gnss_fusion.gnss_zero_altitude", mGnssZeroAltitude, mGnssZeroAltitude);
     RCLCPP_INFO_STREAM(
@@ -3434,6 +3474,12 @@ bool ZedCamera::startCamera()
   RCLCPP_INFO_STREAM(get_logger(), " * Camera Model  -> " << sl::toString(mCamRealModel).c_str());
   mCamSerialNumber = camInfo.serial_number;
   RCLCPP_INFO_STREAM(get_logger(), " * Serial Number -> " << mCamSerialNumber);
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  RCLCPP_INFO_STREAM(
+    get_logger(),
+    " * Focal Lenght -> " << camInfo.camera_configuration.calibration_parameters.left_cam.focal_length_metric <<
+      " m");
+#endif
 
   RCLCPP_INFO_STREAM(
     get_logger(),
@@ -4039,7 +4085,19 @@ bool ZedCamera::startPosTracking()
 
     sl::PositionalTrackingFusionParameters params;
     params.enable_GNSS_fusion = mGnssFusionEnabled;
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
     params.gnss_initialisation_distance = mGnssInitDistance;
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+    sl::GNSSCalibrationParameters gnss_par;
+    gnss_par.enable_reinitialization = mGnssEnableReinitialization;
+    gnss_par.enable_rolling_calibration = mGnssEnableRollingCalibration;
+    gnss_par.enable_translation_uncertainty_target = mGnssEnableTranslationUncertaintyTarget;
+    gnss_par.gnss_vio_reinit_threshold = mGnssVioReinitThreshold;
+    gnss_par.target_translation_uncertainty = mGnssTargetTranslationUncertainty;
+    gnss_par.target_yaw_uncertainty = mGnssTargetYawUncertainty;
+
+    params.gnss_calibration_parameters = gnss_par;
+#endif
     sl::FUSION_ERROR_CODE fus_err = mFusion.enablePositionalTracking(params);
 
     if (fus_err != sl::FUSION_ERROR_CODE::SUCCESS) {
@@ -6420,8 +6478,13 @@ void ZedCamera::publishGnssPoseStatus()
   if (statusSub > 0) {
     poseStatusMsgPtr msg = std::make_unique<zed_interfaces::msg::PosTrackStatus>();
 
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
     if (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK &&
       mGeoPoseStatus == sl::POSITIONAL_TRACKING_STATE::OK)
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+    if (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK &&
+      mGeoPoseStatus == sl::GNSS_CALIBRATION_STATE::CALIBRATED)
+#endif
     {
       msg->status = static_cast<uint8_t>(sl::POSITIONAL_TRACKING_STATE::OK);
     } else {
@@ -6447,8 +6510,13 @@ void ZedCamera::publishGeoPoseStatus()
   if (statusSub > 0) {
     poseStatusMsgPtr msg = std::make_unique<zed_interfaces::msg::PosTrackStatus>();
 
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
     if (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK &&
       mGeoPoseStatus == sl::POSITIONAL_TRACKING_STATE::OK)
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+    if (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK &&
+      mGeoPoseStatus == sl::GNSS_CALIBRATION_STATE::CALIBRATED)
+#endif
     {
       msg->status = static_cast<uint8_t>(sl::POSITIONAL_TRACKING_STATE::OK);
     } else {
@@ -6546,9 +6614,13 @@ void ZedCamera::processGeoPose()
   mGeoPoseStatus = mFusion.getGeoPose(mLastGeoPose);
 
   publishGeoPoseStatus();
-
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
   if (mGeoPoseStatus != sl::POSITIONAL_TRACKING_STATE::OK ||
     mPosTrackingStatusWorld != sl::POSITIONAL_TRACKING_STATE::OK)
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  if (mGeoPoseStatus != sl::GNSS_CALIBRATION_STATE::CALIBRATED ||
+    mPosTrackingStatusWorld != sl::POSITIONAL_TRACKING_STATE::OK)
+#endif
   {
     rclcpp::Clock steady_clock(RCL_STEADY_TIME);
     RCLCPP_DEBUG_THROTTLE(
@@ -8691,7 +8763,14 @@ void ZedCamera::callback_clickedPoint(const geometry_msgs::msg::PointStamped::Sh
 
   // ----> Extract plane from clicked point
   sl::Plane plane;
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
   sl::ERROR_CODE err = mZed.findPlaneAtHit(sl::uint2(u, v), plane);
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  sl::PlaneDetectionParameters params;
+  params.max_distance_threshold = mPdMaxDistanceThreshold;
+  params.normal_similarity_threshold = mPdNormalSimilarityThreshold;
+  sl::ERROR_CODE err = mZed.findPlaneAtHit(sl::uint2(u, v), plane, params);
+#endif
   if (err != sl::ERROR_CODE::SUCCESS) {
     RCLCPP_WARN(
       get_logger(), "Error extracting plane at point [%.3f,%.3f,%.3f]: %s", X, Y, Z,
@@ -9015,7 +9094,11 @@ void ZedCamera::callback_toLL(
     return;
   }
 
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
   if (mGeoPoseStatus != sl::POSITIONAL_TRACKING_STATE::OK) {
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  if (mGeoPoseStatus != sl::GNSS_CALIBRATION_STATE::CALIBRATED) {
+#endif
     RCLCPP_WARN(get_logger(), " * GNSS fusion is not ready");
     return;
   }
@@ -9054,7 +9137,11 @@ void ZedCamera::callback_fromLL(
     return;
   }
 
+#if (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION < 6)
   if (mGeoPoseStatus != sl::POSITIONAL_TRACKING_STATE::OK) {
+#elif (ZED_SDK_MINOR_VERSION == 0 && ZED_SDK_PATCH_VERSION >= 6)
+  if (mGeoPoseStatus != sl::GNSS_CALIBRATION_STATE::CALIBRATED) {
+#endif
     RCLCPP_WARN(get_logger(), " * GNSS fusion is not ready");
     return;
   }

zed_wrapper/CMakeLists.txt

@@ -75,7 +75,7 @@ endif()
 
 #############################################
 # Dependencies
-find_package(ZED 3 REQUIRED)
+find_package(ZED 4 REQUIRED)
 find_package(CUDA REQUIRED)
 
 find_package(ament_cmake_auto REQUIRED)

zed_wrapper/config/common.yaml

@@ -85,18 +85,26 @@
         gnss_fusion:
             gnss_fusion_enabled: false # fuse 'sensor_msg/NavSatFix' message information into pose data
             gnss_fix_topic: "/gps/fix" # Name of the GNSS topic of type NavSatFix to subscribe [Default: "/gps/fix"]
-            gnss_init_distance: 5.0 # The minimum distance traveled by the robot required to initilize the GNSS fusion
+            gnss_init_distance: 5.0 # The minimum distance traveled by the robot required to initilize the GNSS fusion [SDK <= v4.0.5]
             gnss_zero_altitude: false # Set to `true` to ignore GNSS altitude information
             gnss_frame: "gnss_link" # [usually overwritten by launch file] The TF frame of the GNSS sensor
             publish_utm_tf: true # Publish `utm` -> `map` TF
             broadcast_utm_transform_as_parent_frame: false # if 'true' publish `utm` -> `map` TF, otherwise `map` -> `utm`
+            enable_reinitialization: true # determines whether reinitialization should be performed between GNSS and VIO fusion when a significant disparity is detected between GNSS data and the current fusion data. It becomes particularly crucial during prolonged GNSS signal loss scenarios.
+            enable_rolling_calibration: true # If this parameter is set to true, the fusion algorithm will used a rough VIO / GNSS calibration at first and then refine it. This allow you to quickly get a fused position.
+            enable_translation_uncertainty_target: false # When this parameter is enabled (set to true), the calibration process between GNSS and VIO accounts for the uncertainty in the determined translation, thereby facilitating the calibration termination. The maximum allowable uncertainty is controlled by the 'target_translation_uncertainty' parameter.
+            gnss_vio_reinit_threshold: 5 # determines the threshold for GNSS/VIO reinitialization. If the fused position deviates beyond out of the region defined by the product of the GNSS covariance and the gnss_vio_reinit_threshold, a reinitialization will be triggered.
+            target_translation_uncertainty: 10e-2 # defines the target translation uncertainty at which the calibration process between GNSS and VIO concludes. By default, the threshold is set at 10 centimeters.
+            target_yaw_uncertainty: 0.1 # defines the target yaw uncertainty at which the calibration process between GNSS and VIO concludes. The unit of this parameter is in radian. By default, the threshold is set at 0.1 radians.
 
         mapping:
             mapping_enabled: false # True to enable mapping and fused point cloud pubblication
             resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f]
             max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
             fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud
             clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection
+            pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference.
+            pd_normal_similarity_threshold:  15.0 # Plane detection: controls the spread of plane by checking the angle difference.
             qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL
             qos_depth: 1 # Queue size if using KEEP_LAST
             qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT -