Expose yarp::dev::ISixAxisForceTorqueSensors interface in gazebo_yarp_forcetorque plugin

CHANGELOG.md

@@ -5,8 +5,10 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
-## [4.4.0] - 2022-05-31
+### Added
+- The `gazebo_yarp_forcetorque` plugin now exposes also the `yarp::dev::ISixAxisForceTorqueSensors` interface, so it can be used with ` multipleanalogsensorsserver`, `multipleanalogsensorsremapper` and `multipleanalogsensorsclient` devices (https://github.com/robotology/gazebo-yarp-plugins/issues/384, https://github.com/robotology/gazebo-yarp-plugins/pull/628).
 
+## [4.4.0] - 2022-05-31
 
 ### Fixed
 - Removed implicit conversions from the depth data quantization in `GazeboYarpDepthCameraDriver::getDepthImage` which in at least one occasion caused an unexpected crash of the `rgbdSensor_nws_yarp`. Also, the calculation of the scalar coefficient (involving `math::pow`), used to cut the decimal figures from the depth data, has been moved outside the for loop that cycles through the whole image (https://github.com/robotology/gazebo-yarp-plugins/pull/623).

plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h

@@ -9,6 +9,7 @@
 
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/IAnalogSensor.h>
+#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/os/Stamp.h>
 #include <yarp/dev/IPreciselyTimed.h>
 #include <boost/shared_ptr.hpp>
@@ -49,7 +50,8 @@ extern const std::string YarpForceTorqueScopedName;
 class yarp::dev::GazeboYarpForceTorqueDriver: 
     public yarp::dev::IAnalogSensor,
     public yarp::dev::IPreciselyTimed,
-    public yarp::dev::DeviceDriver
+    public yarp::dev::DeviceDriver,
+    public yarp::dev::ISixAxisForceTorqueSensors
 {
 public:
     GazeboYarpForceTorqueDriver();
@@ -74,16 +76,25 @@ class yarp::dev::GazeboYarpForceTorqueDriver:
     virtual int calibrateSensor(const yarp::sig::Vector& value);
     virtual int calibrateChannel(int channel);
 
+    // SIX AXIS FORCE TORQUE SENSORS
+    virtual size_t getNrOfSixAxisForceTorqueSensors() const;
+    virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const ;
+    virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const;
+    virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const;
+    virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const;
+
     //PRECISELY TIMED
     virtual yarp::os::Stamp getLastInputStamp();
 
 
 private:
     yarp::sig::Vector m_forceTorqueData; //buffer for forcetorque sensor data
     yarp::os::Stamp m_lastTimestamp; //buffer for last timestamp data
-    std::mutex m_dataMutex; //mutex for accessing the data
+    mutable std::mutex m_dataMutex; //mutex for accessing the data
     gazebo::sensors::ForceTorqueSensor* m_parentSensor;
     gazebo::event::ConnectionPtr m_updateConnection;
+    std::string m_sensorName;
+    std::string m_frameName;
 
 };
 

plugins/forcetorque/src/ForceTorque.cc

@@ -106,6 +106,7 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     //Open the driver
     //Force the device to be of type "gazebo_forcetorque" (it make sense? probably yes)
     driver_properties.put("device","gazebo_forcetorque");
+    driver_properties.put("sensor_name", _sensor->Name());
     if( !m_forceTorqueDriver.open(driver_properties) ) {
         yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver";
         return;

plugins/forcetorque/src/ForceTorqueDriver.cpp

@@ -63,6 +63,9 @@ bool GazeboYarpForceTorqueDriver::open(yarp::os::Searchable& config)
     //Get gazebo pointers
     std::string sensorScopedName(config.find(YarpForceTorqueScopedName.c_str()).asString().c_str());
 
+    m_sensorName = config.find("sensor_name").asString();
+    m_frameName = m_sensorName;
+
     m_parentSensor = dynamic_cast<gazebo::sensors::ForceTorqueSensor*>(GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName));
 
     if (!m_parentSensor)
@@ -139,6 +142,67 @@ int GazeboYarpForceTorqueDriver::calibrateChannel(int /*ch*/, double /*v*/)
     return AS_OK;
 }
 
+// SIX AXIS FORCE TORQUE SENSORS
+size_t GazeboYarpForceTorqueDriver::getNrOfSixAxisForceTorqueSensors() const
+{
+    return 1;
+}
+
+yarp::dev::MAS_status GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorStatus(size_t sens_index) const
+{
+    if (sens_index >= 1)
+    {
+        return MAS_UNKNOWN;
+    }
+
+    return MAS_OK;
+}
+
+bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const
+{
+    if (sens_index >= 1)
+    {
+        return false;
+    }
+
+    name = m_sensorName;
+    return true;
+}
+
+bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const
+{
+    if (sens_index >= 1)
+    {
+        return false;
+    }
+
+    frameName = m_frameName;
+    return true;
+}
+
+bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
+{
+    if (sens_index >= 1)
+    {
+        return false;
+    }
+
+   if (m_forceTorqueData.size() != YarpForceTorqueChannelsNumber) {
+        return false;
+   }
+
+   if (out.size() != YarpForceTorqueChannelsNumber) {
+       out.resize(YarpForceTorqueChannelsNumber);
+   }
+
+    std::lock_guard<std::mutex> lock(m_dataMutex);
+    out = m_forceTorqueData;
+    timestamp = m_lastTimestamp.getTime();
+
+    return true;
+}
+
+
 //PRECISELY TIMED
 yarp::os::Stamp GazeboYarpForceTorqueDriver::getLastInputStamp()
 {