Merge pull request #136 from lrapetti/feature/statePublisher-fixed-ba…

…seTransform Feature: Add base transform fixed position and orientation parameter to human state publisher
robotology · Aug 21, 2019 · 3f7387f · 3f7387f
2 parents f91920f + 3c18e11
commit 3f7387f
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 3 deletions.
diff --git a/conf/xml/HumanStateProvider_second.xml b/conf/xml/HumanStateProvider_second.xml
@@ -155,6 +155,7 @@
         <param name="baseTFName">/Human_2/Pelvis</param>
         <param name="humanJointsTopic">/Human_2/joint_states</param>
         <param name="portprefix">second</param>
+        <param name="fixBasePosition">(0.0 0.0 1.1)</param>
         <action phase="startup" level="5" type="attach">
             <paramlist name="networks">
                 <elem name="HumanStatePublisherLabel">HumanStateProvider_2</elem>

diff --git a/publishers/HumanStatePublisher/HumanStatePublisher.cpp b/publishers/HumanStatePublisher/HumanStatePublisher.cpp
@@ -65,6 +65,8 @@ class HumanStatePublisher::impl
     hde::interfaces::IHumanState* humanState = nullptr;
 
     bool firstRun = true;
+    bool fixBasePosition = false;
+    bool fixBaseOrientation = false;
     std::string baseTFName;
 
     // Buffers
@@ -98,6 +100,36 @@ bool HumanStatePublisher::open(yarp::os::Searchable& config)
         useDefaultPeriod = true;
     }
 
+    yarp::os::Bottle* fixedBasePosition;
+    if (config.check("fixBasePosition")) {
+        if (config.find("fixBasePosition").isList() &&
+                config.find("fixBasePosition").asList()->size() == 3) {
+            pImpl->fixBasePosition = true;
+            fixedBasePosition = config.find("fixBasePosition").asList();
+            yInfo() << LogPrefix << "Using a fixed position for the base frame: "
+                    << fixedBasePosition;
+        }
+        else {
+            yError() << LogPrefix << "Parameter 'fixBasePosition' invalid";
+            return false;
+        }
+    }
+
+    yarp::os::Bottle* fixedBaseOrientation;
+    if (config.check("fixBaseOrientation")) {
+        if (config.find("fixBaseOrientation").isList() &&
+                config.find("fixBaseOrientation").asList()->size() == 4) {
+            pImpl->fixBaseOrientation = true;
+            fixedBaseOrientation = config.find("fixBaseOrientation").asList();
+            yInfo() << LogPrefix << "Using a fixed orientation for the base frame: "
+                    << fixedBaseOrientation;
+        }
+        else {
+            yError() << LogPrefix << "Parameter 'fixBaseOrientation' invalid (required quaternion)";
+            return false;
+        }
+    }
+
     // ROS TOPICS
     // ==========
 
@@ -143,6 +175,19 @@ bool HumanStatePublisher::open(yarp::os::Searchable& config)
         pImpl->node = new yarp::os::Node({"/" + DeviceName});
     }
 
+    if (pImpl->fixBasePosition) {
+        pImpl->humanStateBuffers.basePosition[0] = fixedBasePosition->get(0).asFloat64();
+        pImpl->humanStateBuffers.basePosition[1] = fixedBasePosition->get(1).asFloat64();
+        pImpl->humanStateBuffers.basePosition[2] = fixedBasePosition->get(2).asFloat64();
+    }
+
+    if (pImpl->fixBaseOrientation) {
+        pImpl->humanStateBuffers.baseOrientation[0] = fixedBaseOrientation->get(0).asFloat64();
+        pImpl->humanStateBuffers.baseOrientation[1] = fixedBaseOrientation->get(1).asFloat64();
+        pImpl->humanStateBuffers.baseOrientation[2] = fixedBaseOrientation->get(2).asFloat64();
+        pImpl->humanStateBuffers.baseOrientation[3] = fixedBaseOrientation->get(3).asFloat64();
+    }
+
     yInfo() << LogPrefix << "*** =====================";
     yInfo() << LogPrefix << "*** Period              :" << period;
     if (hasPortPrefix) {
@@ -277,9 +322,14 @@ void HumanStatePublisher::run()
     // PREPARE THE BASE POSITION MESSAGE
     // =================================
 
-    // Get the data from the interface
-    pImpl->humanStateBuffers.basePosition = pImpl->humanState->getBasePosition();
-    pImpl->humanStateBuffers.baseOrientation = pImpl->humanState->getBaseOrientation();
+    // Get the data from the interface if not using fixed values
+    if (!pImpl->fixBasePosition) {
+        pImpl->humanStateBuffers.basePosition = pImpl->humanState->getBasePosition();
+    }
+    if (!pImpl->fixBaseOrientation) {
+        pImpl->humanStateBuffers.baseOrientation = pImpl->humanState->getBaseOrientation();
+    }
+
     // This is the buffer of the message with base data which will be sent.
     // Here we get the handlt to the first (and only) tf which is sent.
     auto& baseMessageBufferTransform = pImpl->humanBasePoseROS.message.transforms[0];