Merge branch 'fix_imu_tf_no_depth' into 'master'

Fix `publish_imu_tf` behavior with `NO DEPTH` See merge request sl/zed-ros2-wrapper!32
stereolabs · Sep 7, 2023 · c542554 · c542554
2 parents 4a23eb4 + 71f1837
commit c542554
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 17 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -1,6 +1,10 @@
 LATEST CHANGES
 ==============
 
+2023-09-07
+----------
+- Move parameter `publish_imu_tf` from `pos_tracking` to `sensors` to make it available also in "no depth" configurations of the node
+
 2023-09-04
 ----------
 - Add new parameter `pos_tracking.pos_tracking_mode` to exploit the new ZED SDK `QUALITY` mode for improved odometry and localization

diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp
@@ -288,9 +288,9 @@ class ZedCamera : public rclcpp::Node
   double mSensPubRate = 400.;
   bool mPosTrackingEnabled = false;
 
-  bool mPublishTF = true;
-  bool mPublishMapTF = true;
-  bool mPublishImuTF = true;
+  bool mPublishTF = false;
+  bool mPublishMapTF = false;
+  bool mPublishImuTF = false;
   bool mPoseSmoothing = false;
   bool mAreaMemory = true;
   std::string mAreaMemoryDbPath = "";

diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp
@@ -1049,6 +1049,11 @@ void ZedCamera::getSensorsParams()
     return;
   }
 
+  getParam("sensors.publish_imu_tf", mPublishImuTF, mPublishImuTF);
+  RCLCPP_INFO_STREAM(
+    get_logger(),
+    " * Broadcast IMU TF [not for ZED]: " << (mPublishImuTF ? "TRUE" : "FALSE"));
+
   getParam("sensors.sensors_image_sync", mSensCameraSync, mSensCameraSync);
   RCLCPP_INFO_STREAM(
     get_logger(), " * Sensors Camera Sync: " << (mSensCameraSync ? "TRUE" : "FALSE"));
@@ -1288,10 +1293,6 @@ void ZedCamera::getPosTrackingParams()
     mPublishMapTF = false;
   }
   RCLCPP_INFO_STREAM(get_logger(), " * Broadcast Pose TF: " << (mPublishMapTF ? "TRUE" : "FALSE"));
-  getParam("pos_tracking.publish_imu_tf", mPublishImuTF, mPublishImuTF);
-  RCLCPP_INFO_STREAM(
-    get_logger(),
-    " * Broadcast IMU TF [not for ZED]: " << (mPublishImuTF ? "TRUE" : "FALSE"));
 
   getParam(
     "pos_tracking.depth_min_range", mPosTrackDepthMinRange, mPosTrackDepthMinRange,
@@ -8660,13 +8661,6 @@ void ZedCamera::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWr
         stat.add("Pos. Tracking status", "INACTIVE");
       }
 
-      if (mPublishImuTF) {
-        double freq = 1. / mPubImuTF_sec->getAvg();
-        stat.addf("TF IMU", "Mean Frequency: %.1f Hz", freq);
-      } else {
-        stat.add("TF IMU", "DISABLED");
-      }
-
       if (mObjDetRunning) {
         if (mObjDetSubscribed) {
           double freq = 1. / mObjDetPeriodMean_sec->getAvg();
@@ -8701,6 +8695,13 @@ void ZedCamera::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWr
     } else {
       stat.add("Depth status", "INACTIVE");
     }
+
+    if (mPublishImuTF) {
+      double freq = 1. / mPubImuTF_sec->getAvg();
+      stat.addf("TF IMU", "Mean Frequency: %.1f Hz", freq);
+    } else {
+      stat.add("TF IMU", "DISABLED");
+    }
   } else {
     stat.summaryf(
       diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Camera error: %s",

diff --git a/zed_wrapper/config/common.yaml b/zed_wrapper/config/common.yaml
@@ -60,8 +60,7 @@
             pos_tracking_mode: 'STANDARD' # Matches the ZED SDK setting: 'QUALITY', 'STANDARD'
             imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
             publish_tf: true # [usually overwritten by launch file] publish `odom -> base_link` TF
-            publish_map_tf: true # [usually overwritten by launch file] publish `map -> odom` TF
-            publish_imu_tf: true # [usually overwritten by launch file] enable/disable the IMU TF broadcasting
+            publish_map_tf: true # [usually overwritten by launch file] publish `map -> odom` TF            
             base_frame: "base_link" # usually overwritten by launch file
             map_frame: "map"
             odometry_frame: "odom"
@@ -112,6 +111,7 @@
             qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE
 
         sensors:
+            publish_imu_tf: true # [usually overwritten by launch file] enable/disable the IMU TF broadcasting
             sensors_image_sync: false # Synchronize Sensors messages with latest published video/depth message
             sensors_pub_rate: 200. # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate
             qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL

diff --git a/zed_wrapper/launch/include/zed_camera.launch.py b/zed_wrapper/launch/include/zed_camera.launch.py
@@ -151,7 +151,7 @@ def launch_setup(context, *args, **kwargs):
                 'general.serial_number': serial_number,
                 'pos_tracking.publish_tf': publish_tf,
                 'pos_tracking.publish_map_tf': publish_map_tf,
-                'pos_tracking.publish_imu_tf': publish_imu_tf
+                'sensors.publish_imu_tf': publish_imu_tf
             },
             ros_params_override_path,
         ]