diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml index 1044598..7c2685b 100644 --- a/.github/workflows/conda-forge-ci.yml +++ b/.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 diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp index a7152be..3516a72 100644 --- a/src/devices/openxrheadset/CustomPosePublisher.cpp +++ b/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; } diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp index 288d77c..08a3b5d 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.cpp +++ b/src/devices/openxrheadset/OpenXrHeadset.cpp @@ -1021,3 +1021,11 @@ bool yarp::dev::OpenXrHeadset::setCustomPoseRelativeOrientation(const std::strin static_cast(angle2), static_cast(angle3)}); } + +bool yarp::dev::OpenXrHeadset::resetTransforms() +{ + std::lock_guard lock(m_mutex); + m_tfPublisher->clear(); + m_posesManager.reset(); + return true; +} diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h index 8344565..bf6336f 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.h +++ b/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 diff --git a/src/devices/openxrheadset/OpenXrInterface.cpp b/src/devices/openxrheadset/OpenXrInterface.cpp index 4964064..b6cb366 100644 --- a/src/devices/openxrheadset/OpenXrInterface.cpp +++ b/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> 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; diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/PosePublisher.cpp index bc51b2c..80b40d0 100644 --- a/src/devices/openxrheadset/PosePublisher.cpp +++ b/src/devices/openxrheadset/PosePublisher.cpp @@ -46,6 +46,7 @@ void PosePublisher::configurePublisher(std::shared_ptr 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(); +} diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/PosePublisher.h index f4c6e75..e1ba732 100644 --- a/src/devices/openxrheadset/PosePublisher.h +++ b/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 diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp index 05fb331..9699f7b 100644 --- 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(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(); + } +} diff --git a/src/devices/openxrheadset/PosesManager.h b/src/devices/openxrheadset/PosesManager.h index 96dccd2..679af29 100644 --- a/src/devices/openxrheadset/PosesManager.h +++ b/src/devices/openxrheadset/PosesManager.h @@ -61,6 +61,8 @@ class PosesManager bool setCustomPoseRelativeOrientation(const std::string& customFrameName, const Eigen::Vector3f& relativeOrientationEulerAngles); + void reset(); + }; diff --git a/src/devices/openxrheadset/impl/OpenXrInterfaceImpl.h b/src/devices/openxrheadset/impl/OpenXrInterfaceImpl.h index 81d62f7..470f016 100644 --- a/src/devices/openxrheadset/impl/OpenXrInterfaceImpl.h +++ b/src/devices/openxrheadset/impl/OpenXrInterfaceImpl.h @@ -204,7 +204,7 @@ class OpenXrInterface::Implementation } else { - yCError(OPENXRHEADSET) << format; + yCError(OPENXRHEADSET) << format << "(error:" << result << ")"; } return false; diff --git a/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift b/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift index 84f84b9..cc9fde3 100644 --- a/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift +++ b/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(); }