gazebo_yarp_imu: add multipleanalogsensors interface

CHANGELOG.md

@@ -15,3 +15,8 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ### Fixed
 - Fix invalid camera parameters in `gazebo_depthCamera` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/408).
+
+## [3.3.0] - UNRELEASED
+### Added
+- Add the usage of `MultipleAnalogInterfaces`(https://www.yarp.it/group__dev__iface__multiple__analog.html)
+  in the `gazebo_yarp_imu`.

plugins/imu/include/gazebo/IMU.hh

@@ -10,6 +10,7 @@
 
 #include <gazebo/common/Plugin.hh>
 
+#include <yarp/dev/IMultipleWrapper.h>
 #include <yarp/dev/PolyDriver.h>
 
 #include <string>
@@ -25,32 +26,54 @@ namespace gazebo
     /// Gazebo Plugin emulating the yarp imu device in Gazebo.
     ///
     /// This plugin instantiate a yarp imu driver for the Gazebo simulator
-    /// and instantiate a network wrapper (provided by yarp::dev::ServerInertial)
-    /// to expose the sensor on the yarp network.
+    /// and two network wrapper, the yarp::dev::MultipleAnalogSensorsServer and yarp::dev::ServerInertial
+    /// for maintaining backward compatibility.
+    /// It is supported also the legacy behaviour of the device-subdevice
+    /// mechanism; in this case only the ServerInertial is instantiated.
     ///
-    /// It can be configurated using the yarpConfigurationFile sdf tag,
+    /// It can be configured using the yarpConfigurationFile sdf tag,
     /// that contains a Gazebo URI pointing at a yarp .ini configuration file
-    /// containing the configuration parameters of the controlBoard
+    /// containing the configuration parameters of the controlBoard.
     ///
-    /// The parameter that the yarpConfigurationFile must contain are:
-    ///  <TABLE>
-    ///  <TR><TD> name </TD><TD> Port name to assign to the wrapper to this device. </TD></TR>
-    ///  <TR><TD> period </TD><TD> Update period (in s) of yarp port that publish the measure. </TD></TR>
-    ///  </TABLE>
-    /// If the required parameters are not specified, their value will be the
-    /// default one assigned by the yarp::dev::ServerInertial wrapper .
+    /// Here is an example of ini file for load the plugin:
+    ///
+    ///  [include "gazebo_icub_robotname.ini"]
+    ///  [WRAPPER]
+    ///  device multipleanalogsensorsserver
+    ///  name /${gazeboYarpPluginsRobotName}/head/inertials
+    ///  period 10
+    ///
+    ///  [ADDITIONAL_WRAPPER]
+    ///  device inertial
+    ///  name /${gazeboYarpPluginsRobotName}/inertial
+    ///  period 0.01
+    ///
+    ///  [IMU_DRIVER]
+    ///  device gazebo_imu
+    ///
+    /// On the other hand, here is an example of ini file that exploits the legacy behaviour
+    ///
+    ///  [include "gazebo_icub_robotname.ini"]
+    ///  name /${gazeboYarpPluginsRobotName}/inertial
+    ///  period 0.01
+    ///  device inertial
+    ///  subdevice gazebo_imu
     ///
     class GazeboYarpIMU : public SensorPlugin
     {
     public:
         GazeboYarpIMU();
         virtual ~GazeboYarpIMU();
-        virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
+        void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) override;
 
     private:
         yarp::os::Property m_parameters; 
         yarp::dev::PolyDriver m_imuDriver;
-        std::string m_sensorName;
+        yarp::dev::PolyDriver m_MASWrapper;
+        yarp::dev::PolyDriver m_AdditionalWrapper; // for the legacy wrapper ServerInertial
+        yarp::dev::IMultipleWrapper* m_iWrap{nullptr};
+        yarp::dev::IMultipleWrapper* m_iWrapAdditional{nullptr};
+        std::string m_scopedSensorName;
     };
 }
 

plugins/imu/include/yarp/dev/IMUDriver.h

@@ -9,6 +9,7 @@
 
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/GenericSensorInterfaces.h>
+#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/os/Stamp.h>
 #include <yarp/dev/PreciselyTimed.h>
 #include <yarp/os/Semaphore.h>
@@ -41,7 +42,11 @@ extern const std::string YarpIMUScopedName;
 class yarp::dev::GazeboYarpIMUDriver:
     public yarp::dev::IGenericSensor,
     public yarp::dev::IPreciselyTimed,
-    public yarp::dev::DeviceDriver
+    public yarp::dev::DeviceDriver,
+    public yarp::dev::IThreeAxisGyroscopes,
+    public yarp::dev::IThreeAxisLinearAccelerometers,
+    public yarp::dev::IThreeAxisMagnetometers,
+    public yarp::dev::IOrientationSensors
 {
 public:
     GazeboYarpIMUDriver();
@@ -55,24 +60,80 @@ class yarp::dev::GazeboYarpIMUDriver:
      */
 
     //DEVICE DRIVER
-    virtual bool open(yarp::os::Searchable& config);
-    virtual bool close();
+    bool open(yarp::os::Searchable& config) override;
+    bool close() override;
 
     //GENERIC SENSOR
-    virtual bool read(yarp::sig::Vector& outVector);
-    virtual bool getChannels(int* numberOfChannels);
-    virtual bool calibrate(int channelIndex, double v);
+    bool read(yarp::sig::Vector& outVector) override;
+    bool getChannels(int* numberOfChannels) override;
+    bool calibrate(int channelIndex, double v) override;
 
     //PRECISELY TIMED
-    virtual yarp::os::Stamp getLastInputStamp();
+    yarp::os::Stamp getLastInputStamp() override;
+
+    /* IThreeAxisGyroscopes methods */
+
+    size_t getNrOfThreeAxisGyroscopes() const override;
+
+    yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override;
+
+    bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override;
+
+    bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override;
+
+    bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
+
+    /* IThreeAxisLinearAccelerometers methods */
+
+    size_t getNrOfThreeAxisLinearAccelerometers() const override;
+
+    yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override;
+
+    bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override;
+
+    bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override;
+
+    bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
+
+    /* IThreeAxisMagnetometers methods */
+
+    size_t getNrOfThreeAxisMagnetometers() const override;
+
+    yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override;
+
+    bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override;
+
+    bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override;
+
+    bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
+
+    /* IOrientationSensors methods */
+
+    size_t getNrOfOrientationSensors() const override;
+
+    yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override;
+
+    bool getOrientationSensorName(size_t sens_index, std::string &name) const override;
+
+    bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override;
+
+    bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy, double& timestamp) const override;
 
 
 private:
+
+    yarp::dev::MAS_status genericGetStatus(size_t sens_index) const;
+    bool genericGetSensorName(size_t sens_index, std::string &name) const;
+    bool genericGetFrameName(size_t sens_index, std::string &frameName) const;
+    bool genericGetMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp, size_t startIdx) const;
+
     yarp::sig::Vector m_imuData; //buffer for imu data
     yarp::os::Stamp m_lastTimestamp; //buffer for last timestamp data
-    yarp::os::Semaphore m_dataMutex; //mutex for accessing the data
+    mutable yarp::os::Semaphore m_dataMutex; //mutex for accessing the data
+    std::string m_sensorName{"sensor_imu_gazebo"};
+    std::string m_frameName{"sensor_imu_gazebo"};
 
-    gazebo::sensors::ImuSensor* m_parentSensor;
+    gazebo::sensors::ImuSensor* m_parentSensor{};
     gazebo::event::ConnectionPtr m_updateConnection;
 
 };

plugins/imu/src/IMU.cc

@@ -28,13 +28,31 @@ GazeboYarpIMU::GazeboYarpIMU() : SensorPlugin()
 
 GazeboYarpIMU::~GazeboYarpIMU()
 {
-    m_imuDriver.close();
-    GazeboYarpPlugins::Handler::getHandler()->removeSensor(m_sensorName);
+    if(m_iWrap) {
+        m_iWrap->detachAll();
+        m_iWrap = nullptr;
+    }
+    if(m_iWrapAdditional) {
+        m_iWrapAdditional->detachAll();
+        m_iWrapAdditional = nullptr;
+    }
+    if(m_MASWrapper.isValid()) {
+        m_MASWrapper.close();
+    }
+    if(m_AdditionalWrapper.isValid()) {
+        m_AdditionalWrapper.close();
+    }
+    if(m_imuDriver.isValid()) {
+        m_imuDriver.close();
+    }
+    GazeboYarpPlugins::Handler::getHandler()->removeSensor(m_scopedSensorName);
     yarp::os::Network::fini();
 }
 
 void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
 {
+    // The legacy behaviour is to support the old format of ini file with the device/subdevice structure
+    bool legacy_behaviour{false};
     yarp::os::Network::init();
     if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
         yError() << "GazeboYarpIMU::Load error: yarp network does not seem to be available, is the yarpserver running?";
@@ -57,18 +75,92 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
     if (!configuration_loaded) {
         return;
     }
-    m_sensorName = _sensor->ScopedName();
+    m_scopedSensorName = _sensor->ScopedName();
 
     //Insert the pointer in the singleton handler for retriving it in the yarp driver
     GazeboYarpPlugins::Handler::getHandler()->setSensor(_sensor.get());
 
-    m_parameters.put(YarpIMUScopedName.c_str(), m_sensorName.c_str());
+    /*
+    * Open the driver wrapper
+    */
+    //Retrieve wrapper properties
+    ::yarp::os::Bottle MASwrapperProp = m_parameters.findGroup("WRAPPER");
+    if(MASwrapperProp.isNull())
+    {
+        yWarning("GazeboYarpIMU : [WRAPPER] group not found in config file, maybe you are using the old version of the ini file, please update icub-gazebo\n");
+        yWarning("GazeboYarpIMU : trying to open it with the legacy behaviour device-subdevice");
+        legacy_behaviour = true;
+    }
+
+    if (legacy_behaviour) {
+        m_parameters.put(YarpIMUScopedName.c_str(), m_scopedSensorName.c_str());
+        m_parameters.put("sensor_name",_sensor->Name());
+        if (!m_imuDriver.open(m_parameters)) {
+            yError() << "GazeboYarpIMU Plugin Load failed: error in opening yarp driver";
+        }
+        return;
+    }
+
+    //Open the driver wrapper
+    if (!m_MASWrapper.open(MASwrapperProp))
+    {
+        yError() << "GazeboYarpIMU Plugin Load failed: error in opening the yarp wrapper";
+    }
+
+    /*
+    * Open the old wrapper
+    */
+    //Retrieve wrapper properties
+    ::yarp::os::Bottle AdditionalWrapperProp = m_parameters.findGroup("ADDITIONAL_WRAPPER");
+    if(AdditionalWrapperProp.isNull())
+    {
+        yError("GazeboYarpIMU : [ADDITIONAL_WRAPPER] group not found in config file\n");
+        return;
+    }
+
+    //Open the driver wrapper
+    if (!m_AdditionalWrapper.open(AdditionalWrapperProp))
+    {
+        yError() << "GazeboYarpIMU Plugin Load failed: error in opening the yarp wrapper";
+    }
+
+
+    /*
+     * Open the imu driver
+     */
+    //Retrieve part driver properties
+    ::yarp::os::Bottle imu_properties = m_parameters.findGroup("IMU_DRIVER");
+    if(imu_properties.isNull())
+    {
+        yError("GazeboYarpIMU : [IMU_DRIVER] group not found in config file\n");
+        return;
+    }
+
+    //Add the model scoped name for later retrieval of the child sensors from the Handler
+    yarp::os::Bottle& robotNameProp = imu_properties.addList();
+    robotNameProp.addString(YarpIMUScopedName);
+    robotNameProp.addString(m_scopedSensorName);
+
+    imu_properties.addString("sensor_name");
+    imu_properties.addString(_sensor->Name());
 
     //Open the driver
-    if (m_imuDriver.open(m_parameters)) {
+    if (m_imuDriver.open(imu_properties)) {
     } else {
         yError() << "GazeboYarpIMU Plugin Load failed: error in opening yarp driver";
     }
+
+    //Attach the part driver to the wrapper
+    if(!m_MASWrapper.view(m_iWrap) || (!m_AdditionalWrapper.view(m_iWrapAdditional))){
+        yError()<< "GazeboYarpIMU Plugin Load failed: unable to view iMultipleWrapper interfaces";
+        return;
+    }
+    ::yarp::dev::PolyDriverList driverList;
+    driverList.push(&m_imuDriver,"dummy");
+    if(!m_iWrap->attachAll(driverList) || ! m_iWrapAdditional->attachAll(driverList))
+    {
+        yError() << "GazeboYarpIMU: error in connecting wrapper and device ";
+    }
 }
 
 }