Static poses should be published relative to the parent frame

ami-iit · Oct 1, 2024 · 41d5d0b · 41d5d0b
1 parent 9560733
commit 41d5d0b
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 4 deletions.
diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -219,7 +219,7 @@ bool CustomPosePublisherSettings::parseFromConfigurationFile(const yarp::os::Bot
             {
                 yCError(OPENXRHEADSET) << "Failed to parse" << maskName
                                        << "at element" << i
-                                       << ". Only float numbers and the special carachter \"*\" are allowed";
+                                       << ". Only float numbers and the special character \"*\" are allowed";
                 return false;
             }
         }
@@ -237,5 +237,23 @@ bool CustomPosePublisherSettings::parseFromConfigurationFile(const yarp::os::Bot
         return false;
     }
 
+    if (staticPose)
+    {
+        for (size_t i = 0; i < 3; ++i)
+        {
+            if (!positionMask[i])
+            {
+                yCError(OPENXRHEADSET) << "The relative_position mask is not valid for a static pose.";
+                return false;
+            }
+
+            if (!rotationMask[i])
+            {
+                yCError(OPENXRHEADSET) << "The relative_rotation mask is not valid for a static pose.";
+                return false;
+            }
+        }
+    }
+
     return true;
 }
diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/PosePublisher.cpp
@@ -64,6 +64,11 @@ const std::string &PosePublisher::tfBaseFrame() const
     return m_tfBaseFrame;
 }
 
+const bool PosePublisher::staticPose() const
+{
+    return m_staticPose;
+}
+
 bool PosePublisher::configured() const
 {
     return m_tfPublisher != nullptr;
@@ -148,6 +153,10 @@ void PosePublisher::publish()
         {
             yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame (static). It will not be published again.";
         }
+        else
+        {
+            yCInfo(OPENXRHEADSET) << "Published transformation from" << m_tfBaseFrame << "to" << m_label << " (static).";
+        }
         return;
     }
 

diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/PosePublisher.h
@@ -55,6 +55,8 @@ class PosePublisher
 
     const std::string& tfBaseFrame() const;
 
+    const bool staticPose() const;
+
     virtual bool configured() const;
 
     virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input);

diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
@@ -35,7 +35,14 @@ void PosesManager::initialize(const std::string &editedRootFrame, const std::vec
         m_customPoses.emplace_back();
         auto customOptions = std::make_shared<CustomPosePublisherSettings>(customPose);
         customOptions->tfPublisher = m_settings->tfPublisher;
-        customOptions->tfBaseFrame = editedRootFrame;
+        if (customOptions->staticPose)
+        {
+            customOptions->tfBaseFrame = customOptions->parentFrame; //for static poses we publish wrt the parent frame
+        }
+        else
+        {
+            customOptions->tfBaseFrame = editedRootFrame; //for dynamic poses we publish wrt the root frame
+        }
         m_customPoses.back().configure(customOptions);
         m_customPosesMap[customPose.name] = m_customPoses.size() - 1;
     }
@@ -71,8 +78,11 @@ void PosesManager::publishFrames()
         auto& customPose = m_customPoses[i];
         const std::string& relativeFrame = customPose.relativeFrame();
         OpenXrInterface::NamedPoseVelocity parentFramePose;
-
-        if (relativeFrame == m_rootFramePublisher.name())
+        if (customPose.staticPose())
+        {
+            parentFramePose = OpenXrInterface::NamedPoseVelocity::Identity(relativeFrame);
+        }
+        else if (relativeFrame == m_rootFramePublisher.name())
         {
             parentFramePose = m_rootPose;
         }