From c81176c3a5b535876f611ab490e8bbb09f0ffe64 Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Mon, 16 Oct 2023 09:46:50 +0200
Subject: [PATCH] Moved from transformClient to frameTransformClient (#41)

* Moved from transformClient to frameTransformClient

* Specifying the port prefix when opening the frameTransformClient

* Fixed opening of local_rpc port in visualizer.

Added possibility to specify the rpc port name.
---
 src/devices/openxrheadset/OpenXrHeadset.cpp | 28 ++++++++++++++++-----
 src/devices/openxrheadset/OpenXrHeadset.h   |  1 +
 src/utils/OpenXrFrameViz/main.cpp           | 13 +++++++---
 3 files changed, 33 insertions(+), 9 deletions(-)

diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
index 94a2a9b..ddfa21d 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.cpp
+++ b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -39,6 +39,16 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
         m_prefix = name;
     }
 
+    std::string rpcName = cfg.check("rpc_name", yarp::os::Value("rpc")).toString();
+    if (rpcName.front() != '/')
+    {
+        m_rpcPortName = m_prefix +  '/' + rpcName;
+    }
+    else
+    {
+        m_rpcPortName = m_prefix + rpcName;
+    }
+
     //checking the additional guis parameter in the configuration file..
     {
         constexpr unsigned int STRING = 0;
@@ -305,9 +315,15 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
 
     //opening tf client
     yarp::os::Property tfClientCfg;
-    tfClientCfg.put("device", cfg.check("tfDevice", yarp::os::Value("transformClient")).asString());
-    tfClientCfg.put("local",  cfg.check("tfLocal", yarp::os::Value(m_prefix + "/tf")).asString());
-    tfClientCfg.put("remote", cfg.check("tfRemote", yarp::os::Value("/transformServer")).asString());
+    tfClientCfg.put("device", cfg.check("tfDevice", yarp::os::Value("frameTransformClient")).asString());
+    tfClientCfg.put("filexml_option",  cfg.check("tfFile", yarp::os::Value("ftc_yarp_only.xml")).asString());
+    tfClientCfg.put("ft_client_prefix", cfg.check("tfLocal", yarp::os::Value(m_prefix + "/tf")).asString());
+    if (cfg.check("tfRemote"))
+    {
+        tfClientCfg.put("ft_server_prefix", cfg.find("tfRemote").asString());
+    }
+    tfClientCfg.put("period", period);
+    tfClientCfg.put("local_rpc", m_prefix + "/tf/local_rpc");
 
     if (!m_driver.open(tfClientCfg))
     {
@@ -320,7 +336,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
         yCError(OPENXRHEADSET) << "Unable to view IFrameTransform interface.";
         return false;
     }
-    yCInfo(OPENXRHEADSET) << "TransformCLient successfully opened at port: " << cfg.find("tfLocal").asString();
+    yCInfo(OPENXRHEADSET) << "Transform client successfully opened.";
 
     FilteredPosePublisherSettings posePublisherSettings;
     posePublisherSettings.tfPublisher = m_tfPublisher;
@@ -436,9 +452,9 @@ bool yarp::dev::OpenXrHeadset::threadInit()
         std::lock_guard<std::mutex> lock(m_mutex);
 
         this->yarp().attachAsServer(this->m_rpcPort);
-        if(!m_rpcPort.open(m_prefix + "/rpc"))
+        if(!m_rpcPort.open(m_rpcPortName))
         {
-            yCError(OPENXRHEADSET) << "Could not open" << m_prefix + "/rpc" << " RPC port.";
+            yCError(OPENXRHEADSET) << "Could not open" << m_rpcPortName << " RPC port.";
             return false;
         }
     }
diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h
index 7fce5fa..e5a352e 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.h
+++ b/src/devices/openxrheadset/OpenXrHeadset.h
@@ -310,6 +310,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
 
     OpenXrInterface::Pose m_rootFrameRawHRootFrame;
 
+    std::string m_rpcPortName;
     yarp::os::Port m_rpcPort;
 
     std::atomic_bool m_closed{ false };
diff --git a/src/utils/OpenXrFrameViz/main.cpp b/src/utils/OpenXrFrameViz/main.cpp
index ad375aa..3fa59de 100644
--- a/src/utils/OpenXrFrameViz/main.cpp
+++ b/src/utils/OpenXrFrameViz/main.cpp
@@ -95,10 +95,17 @@ int main(int argc, char** argv)
         return EXIT_FAILURE;
     }
 
+    std::string name = rf.check("name", yarp::os::Value("OpenXrFrameViewer")).asString();
+
     yarp::os::Property tfClientCfg;
-    tfClientCfg.put("device", "transformClient");
-    tfClientCfg.put("local",  rf.check("tfLocal", yarp::os::Value("/OpenXrFrameViewer/tf")).asString());
-    tfClientCfg.put("remote", rf.check("tfRemote", yarp::os::Value("/transformServer")).asString());
+    tfClientCfg.put("device", rf.check("tfDevice", yarp::os::Value("frameTransformClient")).asString());
+    tfClientCfg.put("filexml_option",  rf.check("tfFile", yarp::os::Value("ftc_yarp_only.xml")).asString());
+    tfClientCfg.put("ft_client_prefix", "/" + name + "/tf");
+    if (rf.check("tfRemote"))
+    {
+        tfClientCfg.put("ft_server_prefix", rf.find("tfRemote").asString());
+    }
+    tfClientCfg.put("local_rpc", "/" + name + "/tf/local_rpc");
 
     yarp::dev::PolyDriver driver;
     yarp::dev::IFrameTransform* tfReader;