Added RPC method to reset static poses.

This is because otherwise static poses cannot be overwritten

src/devices/openxrheadset/OpenXrHeadset.cpp

@@ -1021,3 +1021,10 @@ bool yarp::dev::OpenXrHeadset::setCustomPoseRelativeOrientation(const std::strin
                                                                               static_cast<float>(angle2),
                                                                               static_cast<float>(angle3)});
 }
+
+void yarp::dev::OpenXrHeadset::resetTransforms()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    m_tfPublisher->clear();
+    m_posesManager.reset();
+}

src/devices/openxrheadset/OpenXrHeadset.h

@@ -255,6 +255,14 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
      */
     virtual bool setCustomPoseRelativeOrientation(const std::string& customFrameName, const double angle1, const double angle2, const double angle3) override;
 
+    /**
+     * Reset the transforms all the published tranforms.
+     * This will also delete all the transforms currently stored in the transform server,
+     * so also the static poses will be published again. This must be used with caution,
+     * as it will reset all the transforms, including the ones that are not published by this module.
+     */
+    virtual void resetTransforms() override;
+
 private:
 
     struct GuiParam

src/devices/openxrheadset/PosePublisher.cpp

@@ -165,3 +165,12 @@ void PosePublisher::publish()
         yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame.";
     }
 }
+
+void PosePublisher::reset()
+{
+    m_publishedOnce = false;
+    m_data = OpenXrInterface::NamedPoseVelocity();
+    m_previouslyPublishedData = OpenXrInterface::NamedPoseVelocity();
+    m_localPose.eye();
+    deactivate();
+}

src/devices/openxrheadset/PosePublisher.h

@@ -65,6 +65,8 @@ class PosePublisher
 
     void publish();
 
+    void reset();
+
 };
 
 #endif // YARP_DEV_POSEPUBLISHER_H

src/devices/openxrheadset/PosesManager.cpp

@@ -205,3 +205,18 @@ bool PosesManager::setCustomPoseRelativeOrientation(const std::string &customFra
     m_customPoses[customFrameIt->second].setRelativeOrientation(relativeOrientationEulerAngles);
     return true;
 }
+
+void PosesManager::reset()
+{
+    m_rootFramePublisher.reset();
+
+    for (auto& poseIt : m_poses)
+    {
+        poseIt.second.reset();
+    }
+
+    for (auto& customPose : m_customPoses)
+    {
+        customPose.reset();
+    }
+}

src/devices/openxrheadset/PosesManager.h

@@ -61,6 +61,8 @@ class PosesManager
 
     bool setCustomPoseRelativeOrientation(const std::string& customFrameName, const Eigen::Vector3f& relativeOrientationEulerAngles);
 
+    void reset();
+
 };
 
 

src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift

@@ -179,4 +179,12 @@ service OpenXrHeadsetCommands
      * @return True if successfull, false if the pose is not found
      */
      bool setCustomPoseRelativeOrientation(1:string customFrameName, 2:double angle1, 3:double angle2, 4:double angle3);
+
+     /**
+     * Reset the transforms all the published tranforms.
+     * This will also delete all the transforms currently stored in the transform server,
+     * so also the static poses will be published again. This must be used with caution,
+     * as it will reset all the transforms, including the ones that are not published by this module.
+     */
+     void resetTransforms();
 }