Merge pull request #46 from ami-iit/static_poses

Added possibility to publish custom static poses

.github/workflows/conda-forge-ci.yml

@@ -21,7 +21,7 @@ jobs:
     steps:
     - uses: actions/checkout@v2
 
-    - uses: conda-incubator/setup-miniconda@v2
+    - uses: conda-incubator/setup-miniconda@v3
       with:
         miniforge-variant: Mambaforge
         miniforge-version: latest

src/devices/openxrheadset/CustomPosePublisher.cpp

@@ -146,6 +146,8 @@ bool CustomPosePublisherSettings::parseFromConfigurationFile(const yarp::os::Bot
         return false;
     }
 
+    staticPose = inputGroup.check("static_pose") && (inputGroup.find("static_pose").isNull() || inputGroup.find("static_pose").asBool());
+
     std::string parametrization = inputGroup.check("euler_angles", yarp::os::Value("")).toString();
 
     if (parametrization.size() != 3)
@@ -217,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;
             }
         }
@@ -235,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;
 }

src/devices/openxrheadset/OpenXrHeadset.cpp

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

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 bool resetTransforms() override;
+
 private:
 
     struct GuiParam

src/devices/openxrheadset/OpenXrInterface.cpp

@@ -169,7 +169,29 @@ bool OpenXrInterface::prepareXrInstance()
     };
     instanceCreateInfo.next = &debugCallbackSettings;
 
-    XrResult result = xrCreateInstance(&instanceCreateInfo, &(m_pimpl->instance));
+    XrResult result = XR_ERROR_API_VERSION_UNSUPPORTED;
+
+    std::vector<std::pair<XrVersion, std::string>> api_versions = {
+#ifdef XR_API_VERSION_1_0
+                                                                   {XR_API_VERSION_1_0, "XR_API_VERSION_1_0"},
+#endif
+                                                                   {XR_CURRENT_API_VERSION, "XR_CURRENT_API_VERSION"},
+#ifdef XR_API_VERSION_1_1
+                                                                   {XR_API_VERSION_1_1, "XR_API_VERSION_1_1"},
+#endif // XR_API_VERSION_1_1
+                                                                   };
+    size_t version_index = 0;
+
+    while ((result == XR_ERROR_API_VERSION_UNSUPPORTED || result == XR_ERROR_INITIALIZATION_FAILED) && version_index < api_versions.size())
+    {
+        instanceCreateInfo.applicationInfo.apiVersion = api_versions[version_index].first;
+        result = xrCreateInstance(&instanceCreateInfo, &(m_pimpl->instance));
+        if (!XR_SUCCEEDED(result))
+        {
+            yCWarning(OPENXRHEADSET) << "The runtime does not support the API version" << api_versions[version_index].second;
+        }
+        version_index++;
+    }
 
     if (!m_pimpl->checkXrOutput(result, "Failed to create XR instance."))
         return false;

src/devices/openxrheadset/PosePublisher.cpp

@@ -46,6 +46,7 @@ void PosePublisher::configurePublisher(std::shared_ptr<PosePublisherSettings> se
 
     m_tfBaseFrame = settings->tfBaseFrame;
     m_tfPublisher = settings->tfPublisher;
+    m_staticPose = settings->staticPose;
 }
 
 void PosePublisher::setLabel(const std::string &label)
@@ -63,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;
@@ -103,6 +109,11 @@ void PosePublisher::publish()
         return;
     }
 
+    if (m_staticPose && m_publishedOnce)
+    {
+        return;
+    }
+
     if (!m_publishedOnce)
     {
         if (m_label.empty())
@@ -136,8 +147,30 @@ void PosePublisher::publish()
         }
     }
 
+    if (m_staticPose)
+    {
+        if (!m_tfPublisher->setTransformStatic(m_label, m_tfBaseFrame, m_localPose))
+        {
+            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;
+    }
+
     if (!m_tfPublisher->setTransform(m_label, m_tfBaseFrame, m_localPose))
     {
         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

@@ -18,6 +18,7 @@ struct PosePublisherSettings
 {
     yarp::dev::IFrameTransform* tfPublisher{nullptr};
     std::string tfBaseFrame;
+    bool staticPose{ false };
 };
 
 class PosePublisher
@@ -30,6 +31,7 @@ class PosePublisher
     bool m_publishedOnce{false};
     double m_lastWarningTime{0.0};
     size_t m_warningCount{0};
+    bool m_staticPose{ false };
 
     void resetWarnings();
 
@@ -53,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);
@@ -61,6 +65,8 @@ class PosePublisher
 
     void publish();
 
+    void reset();
+
 };
 
 #endif // YARP_DEV_POSEPUBLISHER_H

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;
         }
@@ -195,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/impl/OpenXrInterfaceImpl.h

@@ -204,7 +204,7 @@ class OpenXrInterface::Implementation
         }
         else
         {
-            yCError(OPENXRHEADSET) << format;
+            yCError(OPENXRHEADSET) << format << "(error:" << result << ")";
         }
 
         return false;

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.
+     */
+     bool resetTransforms();
 }