Merge pull request #279 from robotology/streamImprovedFT

added feature to open ports to stream the filteredFT sensor values af…

Author: traversaro

src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

@@ -27,6 +27,7 @@ const double wholeBodyDynamics_sensorTimeoutInSeconds = 2.0;
 
 WholeBodyDynamicsDevice::WholeBodyDynamicsDevice(): RateThread(10),
                                                     portPrefix("/wholeBodyDynamics"),
+                                                    streamFilteredFT(false),
                                                     correctlyConfigured(false),
                                                     sensorReadCorrectly(false),
                                                     estimationWentWell(false),
@@ -116,6 +117,19 @@ bool WholeBodyDynamicsDevice::closeExternalWrenchesPorts()
     return true;
 }
 
+bool WholeBodyDynamicsDevice::closeFilteredFTPorts()
+{
+    for(unsigned int i = 0; i < outputFTPorts.size(); i++ )
+    {
+        if( outputFTPorts[i] )
+        {
+            outputFTPorts[i]->close();
+            outputFTPorts[i].reset();
+        }
+    }
+    return true;
+}
+
 
 
 void addVectorOfStringToProperty(yarp::os::Property& prop, std::string key, std::vector<std::string> & list)
@@ -508,6 +522,33 @@ bool WholeBodyDynamicsDevice::openExternalWrenchesPorts(os::Searchable& config)
     return ok;
 }
 
+bool WholeBodyDynamicsDevice::openFilteredFTPorts(os::Searchable& config)
+{
+    bool ok = true;
+    // create port names
+
+    std::string sensorName;
+    std::string portName;
+    outputFTPorts.resize(estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE));
+    for(int ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
+    {
+        sensorName= estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() ;
+
+        yInfo() << "wholeBodyDynamics: creating port name and opening port for  " << sensorName;
+
+        portName=(portPrefix+"/filteredFT/"+sensorName);
+        outputFTPorts[ft]=std::make_unique<yarp::os::BufferedPort<yarp::sig::Vector> >();
+        ok = ok && outputFTPorts[ft]->open(portName);
+    }
+
+    if( !ok )
+    {
+        yError() << "wholeBodyDynamics impossible to open port for publishing filtered ft wrenches";
+        return false;
+    }
+    return ok;
+}
+
 
 void WholeBodyDynamicsDevice::resizeBuffers()
 {
@@ -638,6 +679,14 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config)
         portPrefix = prop.find("portPrefix").asString();
     }
 
+    // Enable or disable ft streaming. The default value is false;
+    // is set in the device constructor
+    if( prop.check("streamFilteredFT") &&
+        prop.find("streamFilteredFT").isBool())
+    {
+        streamFilteredFT = prop.find("streamFilteredFT").asBool();
+    }
+
     std::string useJointVelocityOptionName = "useJointVelocity";
     if( !(prop.check(useJointVelocityOptionName.c_str()) && prop.find(useJointVelocityOptionName.c_str()).isBool()) )
     {
@@ -904,6 +953,15 @@ bool WholeBodyDynamicsDevice::open(os::Searchable& config)
         return false;
     } 
 
+    // Open the ports related to publishing filtered ft wrenches
+    if (streamFilteredFT){
+        ok = this->openFilteredFTPorts(config);
+        if( !ok )
+        {
+            yError() << "wholeBodyDynamics: Problem in opening filtered ft ports.";
+            return false;
+        }
+    }
 
     return true;
 }
@@ -1601,7 +1659,9 @@ void WholeBodyDynamicsDevice::publishEstimatedQuantities()
             //publishFilteredInertialForGravityCompensator();
 
             //Send filtered force torque sensor measurment, if requested
-            //publishFilteredFTWithoutOffset();
+            if( streamFilteredFT){
+                publishFilteredFTWithoutOffset();
+            }
         }
     }
 }
@@ -1736,6 +1796,23 @@ void WholeBodyDynamicsDevice::publishExternalWrenches()
     }
 }
 
+void WholeBodyDynamicsDevice::publishFilteredFTWithoutOffset()
+{
+
+        iDynTree::Wrench filteredFTMeasure;
+        yarp::sig::Vector filteredFT;
+        // Get filtered wrenches and publish it on the port
+        for(int ft=0; ft <outputFTPorts.size(); ft++ )
+        {
+            filteredSensorMeasurements.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft,filteredFTMeasure);
+            iDynTree::toYarp(filteredFTMeasure,filteredFT);
+            broadcastData<yarp::sig::Vector>(filteredFT,
+                                             *(outputFTPorts[ft]));
+
+        }
+
+}
+
 void WholeBodyDynamicsDevice::run()
 {
     yarp::os::LockGuard guard(this->deviceMutex);
@@ -1786,6 +1863,7 @@ bool WholeBodyDynamicsDevice::detachAll()
     this->remappedControlBoardInterfaces.multwrap->detachAll();
     this->remappedVirtualAnalogSensorsInterfaces.multwrap->detachAll();
 
+    closeFilteredFTPorts();
     closeExternalWrenchesPorts();
     closeRPCPort();
     closeSettingsPort();

src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h

@@ -131,6 +131,7 @@ class wholeBodyDynamicsDeviceFilters
  * | defaultContactFrames |      -   | vector of strings |  -    |    -          | Yes      | If not data is read from the skin, specify the location of the default contacts | For each submodel induced by the FT sensor, the first not used frame that belongs to that submodel is selected from the list. An error is raised if not suitable frame is found for a submodel. |
  * | useJointVelocity     |        - | bool              |  -    |      true     |  No      | Select if the measured joint velocities (read from the getEncoderSpeeds method) are used for estimation, or if they should be forced to 0.0 . | The default value of true is deprecated, and in the future the parameter will be required. |
  * | useJointAcceleration |        - | bool              |  -    |      true     |  No      | Select if the measured joint accelerations (read from the getEncoderAccelerations method) are used for estimation, or if they should be forced to 0.0 . | The default value of true is deprecated, and in the future the parameter will be required. |
+ * | streamFilteredFT     |        - | bool              |  -    |      false    |  No      | Select if the filtered and offset removed forces will be streamed or not. The name of the ports have the following syntax:  portname=(portPrefix+"/filteredFT/"+sensorName). Example: "myPrefix/filteredFT/l_leg_ft_sensor" | The value streamed by this ports is affected by the secondary calibration matrix, the estimated offset and temperature coefficients ( if any ). |
  * | IDYNTREE_SKINDYNLIB_LINKS |  -  | group             | -     | -             | Yes      |  Group describing the mapping between link names and skinDynLib identifiers. | |
  * |                |   linkName_1   | string (name of a link in the model) | - | - | Yes   | Bottle of three elements describing how the link with linkName is described in skinDynLib: the first element is the name of the frame in which the contact info is expressed in skinDynLib (tipically DH frames), the second a integer describing the skinDynLib BodyPart , and the third a integer describing the skinDynLib LinkIndex  | |
  * |                |   ...   | string (name of a link in the model) | - | -     | Yes      | Bottle of three elements describing how the link with linkName is described in skinDynLib: the first element is the name of the frame in which the contact info is expressed in skinDynLib (tipically DH frames), the second a integer describing the skinDynLib BodyPart , and the third a integer describing the skinDynLib LinkIndex  | |
@@ -314,6 +315,12 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
      */
     double  lastReadingSkinContactListStamp;
 
+    /**
+      * Flag set to false at the beginning, and true depending on configuration flag
+      * If true it will stream the filtered ft sensor values
+      */
+    bool streamFilteredFT;
+
     /**
      * Names of the axis (joint with at least a degree of freedom) used in estimation.
      */
@@ -376,7 +383,8 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     bool openEstimator(os::Searchable& config);
     bool openDefaultContactFrames(os::Searchable& config);
     bool openSkinContactListPorts(os::Searchable& config);
-    bool openExternalWrenchesPorts(os::Searchable& config);
+    bool openExternalWrenchesPorts(os::Searchable& config);    
+    bool openFilteredFTPorts(os::Searchable& config);
 
     /**
      * Close-related methods
@@ -385,6 +393,7 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     bool closeRPCPort();
     bool closeSkinContactListsPorts();
     bool closeExternalWrenchesPorts();
+    bool closeFilteredFTPorts();
 
     /**
      * Attach-related methods
@@ -457,6 +466,7 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     void publishExternalWrenches();
     void publishEstimatedQuantities();
     void publishGravityCompensation();
+    void publishFilteredFTWithoutOffset();
 
     /**
      * Load settings from config.
@@ -736,6 +746,11 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     //  informations on individual ports)
     std::vector< outputWrenchPortInformation > outputWrenchPorts;
 
+    /**
+     * Ports for streaming fitelerd ft data without offset
+     */
+    std::vector< std::unique_ptr <yarp::os::BufferedPort <yarp::sig::Vector> > > outputFTPorts;
+
     // Buffer for external forces
     /**
      * The element netExternalWrenchesExertedByTheEnviroment[i] is the