Fix set pose

latest_changes.md

@@ -1,6 +1,9 @@
 LATEST CHANGES
 ==============
 
+2022-03-30
+- Fix wrong TP broadcasting when calling the `set_pose`, `reset_tracking`, and `reset_odometry` services
+
 2022-03-28
 - Add parameter `sensors/max_pub_rate` to set the maximum publishing frequency of sensors data
 - Improve Sensors thread

zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp

@@ -1240,18 +1240,8 @@ bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_
     mInitialBasePose[4] = req.P;
     mInitialBasePose[5] = req.Y;
 
-    if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4],
-            mInitialBasePose[5])) {
-        res.done = false;
-        return false;
-    }
-
     std::lock_guard<std::mutex> lock(mPosTrkMutex);
 
-    // Disable tracking
-    mPosTrackingActivated = false;
-    mZed.disablePositionalTracking();
-
     // Restart tracking
     start_pos_tracking();
 
@@ -1267,18 +1257,8 @@ bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Reques
         return false;
     }
 
-    if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4],
-            mInitialBasePose[5])) {
-        res.reset_done = false;
-        return false;
-    }
-
     std::lock_guard<std::mutex> lock(mPosTrkMutex);
 
-    // Disable tracking
-    mPosTrackingActivated = false;
-    mZed.disablePositionalTracking();
-
     // Restart tracking
     start_pos_tracking();
 
@@ -1470,6 +1450,7 @@ void ZEDWrapperNodelet::start_pos_tracking()
 
     NODELET_INFO(" * Waiting for valid static transformations...");
 
+    mPosTrackingReady = false; // Useful to not publish wrong TF with IMU frame broadcasting
     bool transformOk = false;
     double elapsed = 0.0;