Add Angular Accelerometer and Linear Velocity Sensor Interfaces to the Sensor Remapper

cmake/YarpIDL.cmake

@@ -193,7 +193,7 @@ endfunction()
 function(YARP_IDL_TO_DIR)
 
   # Flag to control whether IDL generation is allowed.
-  option(ALLOW_IDL_GENERATION "Allow YARP to (re)build IDL files as needed" OFF)
+  option(ALLOW_IDL_GENERATION "Allow YARP to (re)build IDL files as needed" ON)
 
   set(_options
     VERBOSE

src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift

@@ -23,14 +23,16 @@ struct SensorStreamingData
 {
   1: SensorMeasurements ThreeAxisGyroscopes;
   2: SensorMeasurements ThreeAxisLinearAccelerometers;
-  3: SensorMeasurements ThreeAxisMagnetometers;
-  4: SensorMeasurements OrientationSensors;
-  5: SensorMeasurements TemperatureSensors;
-  6: SensorMeasurements SixAxisForceTorqueSensors;
-  7: SensorMeasurements ContactLoadCellArrays;
-  8: SensorMeasurements EncoderArrays;
-  9: SensorMeasurements SkinPatches;
-  10: SensorMeasurements PositionSensors;
+  3: SensorMeasurements ThreeAxisAngularAccelerometers;
+  4: SensorMeasurements ThreeAxisMagnetometers;
+  5: SensorMeasurements OrientationSensors;
+  6: SensorMeasurements TemperatureSensors;
+  7: SensorMeasurements SixAxisForceTorqueSensors;
+  8: SensorMeasurements ContactLoadCellArrays;
+  9: SensorMeasurements EncoderArrays;
+  10: SensorMeasurements SkinPatches;
+  11: SensorMeasurements PositionSensors;
+  12: SensorMeasurements VelocitySensors;
 }
 
 struct SensorMetadata {
@@ -43,14 +45,16 @@ struct SensorRPCData
 {
   1: list<SensorMetadata> ThreeAxisGyroscopes;
   2: list<SensorMetadata> ThreeAxisLinearAccelerometers;
-  3: list<SensorMetadata> ThreeAxisMagnetometers;
-  4: list<SensorMetadata> OrientationSensors;
-  5: list<SensorMetadata> TemperatureSensors;
-  6: list<SensorMetadata> SixAxisForceTorqueSensors;
-  7: list<SensorMetadata> ContactLoadCellArrays;
-  8: list<SensorMetadata> EncoderArrays;
-  9: list<SensorMetadata> SkinPatches;
-  10: list<SensorMetadata> PositionSensors;
+  3: list<SensorMetadata> ThreeAxisAngularAccelerometers;
+  4: list<SensorMetadata> ThreeAxisMagnetometers;
+  5: list<SensorMetadata> OrientationSensors;
+  6: list<SensorMetadata> TemperatureSensors;
+  7: list<SensorMetadata> SixAxisForceTorqueSensors;
+  8: list<SensorMetadata> ContactLoadCellArrays;
+  9: list<SensorMetadata> EncoderArrays;
+  10: list<SensorMetadata> SkinPatches;
+  11: list<SensorMetadata> PositionSensors;
+  12: list<SensorMetadata> VelocitySensors;
 }
 
 service MultipleAnalogSensorsMetadata

src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp

@@ -378,6 +378,33 @@ bool MultipleAnalogSensorsClient::getThreeAxisLinearAccelerometerMeasure(size_t
                              m_streamingPort.receivedData.ThreeAxisLinearAccelerometers, sens_index, out, timestamp);
 }
 
+size_t MultipleAnalogSensorsClient::getNrOfThreeAxisAngularAccelerometers() const
+{
+    return genericGetNrOfSensors(m_sensorsMetadata.ThreeAxisAngularAccelerometers,
+                                 m_streamingPort.receivedData.ThreeAxisAngularAccelerometers);
+}
+
+yarp::dev::MAS_status MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerStatus(size_t sens_index) const
+{
+    return genericGetStatus();
+}
+
+bool MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const
+{
+    return genericGetName(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers", sens_index, name);
+}
+
+bool MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const
+{
+    return genericGetFrameName(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers", sens_index, frameName);
+}
+
+bool MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
+{
+    return genericGetMeasure(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers",
+                             m_streamingPort.receivedData.ThreeAxisAngularAccelerometers, sens_index, out, timestamp);
+}
+
 size_t MultipleAnalogSensorsClient::getNrOfThreeAxisMagnetometers() const
 {
     return genericGetNrOfSensors(m_sensorsMetadata.ThreeAxisMagnetometers,
@@ -458,6 +485,32 @@ bool MultipleAnalogSensorsClient::getPositionSensorMeasure(size_t sens_index, ya
     return genericGetMeasure(m_sensorsMetadata.PositionSensors, "PositionSensors", m_streamingPort.receivedData.PositionSensors, sens_index, out, timestamp);
 }
 
+size_t MultipleAnalogSensorsClient::getNrOfVelocitySensors() const
+{
+    return genericGetNrOfSensors(m_sensorsMetadata.VelocitySensors,
+                                 m_streamingPort.receivedData.VelocitySensors);
+}
+
+yarp::dev::MAS_status MultipleAnalogSensorsClient::getVelocitySensorStatus(size_t sens_index) const
+{
+    return genericGetStatus();
+}
+
+bool MultipleAnalogSensorsClient::getVelocitySensorName(size_t sens_index, std::string& name) const
+{
+    return genericGetName(m_sensorsMetadata.VelocitySensors, "VelocitySensors", sens_index, name);
+}
+
+bool MultipleAnalogSensorsClient::getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const
+{
+    return genericGetFrameName(m_sensorsMetadata.VelocitySensors, "VelocitySensors", sens_index, frameName);
+}
+
+bool MultipleAnalogSensorsClient::getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
+{
+    return genericGetMeasure(m_sensorsMetadata.VelocitySensors, "VelocitySensors", m_streamingPort.receivedData.VelocitySensors, sens_index, out, timestamp);
+}
+
 size_t MultipleAnalogSensorsClient::getNrOfTemperatureSensors() const
 {
     return genericGetNrOfSensors(m_sensorsMetadata.TemperatureSensors,

src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h

@@ -56,8 +56,10 @@ class MultipleAnalogSensorsClient :
         public yarp::dev::DeviceDriver,
         public yarp::dev::IThreeAxisGyroscopes,
         public yarp::dev::IThreeAxisLinearAccelerometers,
+        public yarp::dev::IThreeAxisAngularAccelerometers,
         public yarp::dev::IThreeAxisMagnetometers,
         public yarp::dev::IPositionSensors,
+        public yarp::dev::IVelocitySensors,
         public yarp::dev::IOrientationSensors,
         public yarp::dev::ITemperatureSensors,
         public yarp::dev::ISixAxisForceTorqueSensors,
@@ -112,6 +114,13 @@ class MultipleAnalogSensorsClient :
     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;
 
+    /* IThreeAxisAngularAccelerometers methods */
+    size_t getNrOfThreeAxisAngularAccelerometers() const override;
+    yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const override;
+    bool getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const override;
+    bool getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const override;
+    bool getThreeAxisAngularAccelerometerMeasure(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;
@@ -126,6 +135,13 @@ class MultipleAnalogSensorsClient :
     bool getPositionSensorFrameName(size_t sens_index, std::string& frameName) const override;
     bool getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override;
 
+    /* IVelocitySensors methods */
+    size_t getNrOfVelocitySensors() const override;
+    yarp::dev::MAS_status getVelocitySensorStatus(size_t sens_index) const override;
+    bool getVelocitySensorName(size_t sens_index, std::string& name) const override;
+    bool getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override;
+    bool getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override;
+
     /* IOrientationSensors methods */
     size_t getNrOfOrientationSensors() const override;
     yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override;

src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp

@@ -18,8 +18,8 @@ namespace {
 YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSREMAPPER, "yarp.device.multipleanalogsensorsremapper")
 }
 
-const size_t MAS_NrOfSensorTypes{10};
-static_assert(MAS_SensorType::PositionSensors+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType");
+const size_t MAS_NrOfSensorTypes{12};
+static_assert(MAS_SensorType::VelocitySensors+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType");
 
 /**
  * Internal identifier of the type of sensors.
@@ -34,6 +34,9 @@ inline std::string MAS_getTagFromEnum(const MAS_SensorType type)
         case ThreeAxisLinearAccelerometers:
             return "ThreeAxisLinearAccelerometers";
             break;
+        case ThreeAxisAngularAccelerometers:
+            return "ThreeAxisAngularAccelerometers";
+            break;
         case ThreeAxisMagnetometers:
             return "ThreeAxisMagnetometers";
             break;
@@ -58,6 +61,9 @@ inline std::string MAS_getTagFromEnum(const MAS_SensorType type)
         case PositionSensors:
             return "PositionSensors";
             break;
+        case VelocitySensors:
+            return "VelocitySensors";
+            break;
         default:
             assert(false);
             return "MAS_getTagFromEnum_notExpectedEnum";
@@ -224,10 +230,14 @@ bool MultipleAnalogSensorsRemapper::attachAll(const PolyDriverList &polylist)
                                 &IThreeAxisGyroscopes::getThreeAxisGyroscopeName, &IThreeAxisGyroscopes::getNrOfThreeAxisGyroscopes);
     ok = ok && genericAttachAll(ThreeAxisLinearAccelerometers, m_iThreeAxisLinearAccelerometers, polylist,
                                 &IThreeAxisLinearAccelerometers::getThreeAxisLinearAccelerometerName, &IThreeAxisLinearAccelerometers::getNrOfThreeAxisLinearAccelerometers);
+    ok = ok && genericAttachAll(ThreeAxisAngularAccelerometers, m_iThreeAxisAngularAccelerometers, polylist,
+                                &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerName, &IThreeAxisAngularAccelerometers::getNrOfThreeAxisAngularAccelerometers);
     ok = ok && genericAttachAll(ThreeAxisMagnetometers, m_iThreeAxisMagnetometers, polylist,
                                 &IThreeAxisMagnetometers::getThreeAxisMagnetometerName, &IThreeAxisMagnetometers::getNrOfThreeAxisMagnetometers);
     ok = ok && genericAttachAll(PositionSensors, m_iPositionSensors, polylist,
                                 &IPositionSensors::getPositionSensorName, &IPositionSensors::getNrOfPositionSensors);
+    ok = ok && genericAttachAll(VelocitySensors, m_iVelocitySensors, polylist,
+                                &IVelocitySensors::getVelocitySensorName, &IVelocitySensors::getNrOfVelocitySensors);
     ok = ok && genericAttachAll(OrientationSensors, m_iOrientationSensors, polylist,
                                 &IOrientationSensors::getOrientationSensorName, &IOrientationSensors::getNrOfOrientationSensors);
     ok = ok && genericAttachAll(TemperatureSensors, m_iTemperatureSensors, polylist,
@@ -248,8 +258,10 @@ bool MultipleAnalogSensorsRemapper::detachAll()
 {
     m_iThreeAxisGyroscopes.resize(0);
     m_iThreeAxisLinearAccelerometers.resize(0);
+    m_iThreeAxisAngularAccelerometers.resize(0);
     m_iThreeAxisMagnetometers.resize(0);
     m_iPositionSensors.resize(0);
+    m_iVelocitySensors.resize(0);
     m_iOrientationSensors.resize(0);
     m_iTemperatureSensors.resize(0);
     m_iSixAxisForceTorqueSensors.resize(0);
@@ -443,6 +455,31 @@ bool MultipleAnalogSensorsRemapper::getThreeAxisLinearAccelerometerMeasure(size_
      return genericGetMeasure(ThreeAxisLinearAccelerometers, sens_index, out, timestamp, m_iThreeAxisLinearAccelerometers, &IThreeAxisLinearAccelerometers::getThreeAxisLinearAccelerometerMeasure);
 }
 
+size_t MultipleAnalogSensorsRemapper::getNrOfThreeAxisAngularAccelerometers() const
+{
+    return m_indicesMap[ThreeAxisAngularAccelerometers].size();
+}
+
+MAS_status MultipleAnalogSensorsRemapper::getThreeAxisAngularAccelerometerStatus(size_t sens_index) const
+{
+    return genericGetStatus(ThreeAxisAngularAccelerometers, sens_index, m_iThreeAxisAngularAccelerometers, &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerStatus);
+}
+
+bool MultipleAnalogSensorsRemapper::getThreeAxisAngularAccelerometerName(size_t sens_index, std::string& name) const
+{
+     return genericGetName(ThreeAxisAngularAccelerometers, sens_index, name, m_iThreeAxisAngularAccelerometers, &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerName);
+}
+
+bool MultipleAnalogSensorsRemapper::getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string& frameName) const
+{
+     return genericGetFrameName(ThreeAxisAngularAccelerometers, sens_index, frameName, m_iThreeAxisAngularAccelerometers, &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerFrameName);
+}
+
+bool MultipleAnalogSensorsRemapper::getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
+{
+     return genericGetMeasure(ThreeAxisAngularAccelerometers, sens_index, out, timestamp, m_iThreeAxisAngularAccelerometers, &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerMeasure);
+}
+
 size_t MultipleAnalogSensorsRemapper::getNrOfThreeAxisMagnetometers() const
 {
     return m_indicesMap[ThreeAxisMagnetometers].size();
@@ -493,6 +530,31 @@ bool MultipleAnalogSensorsRemapper::getPositionSensorMeasure(size_t sens_index,
     return genericGetMeasure(PositionSensors, sens_index, out, timestamp, m_iPositionSensors, &IPositionSensors::getPositionSensorMeasure);
 }
 
+size_t MultipleAnalogSensorsRemapper::getNrOfVelocitySensors() const
+{
+    return m_indicesMap[VelocitySensors].size();
+}
+
+MAS_status MultipleAnalogSensorsRemapper::getVelocitySensorStatus(size_t sens_index) const
+{
+    return genericGetStatus(VelocitySensors, sens_index, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorStatus);
+}
+
+bool MultipleAnalogSensorsRemapper::getVelocitySensorName(size_t sens_index, std::string& name) const
+{
+    return genericGetName(VelocitySensors, sens_index, name, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorName);
+}
+
+bool MultipleAnalogSensorsRemapper::getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const
+{
+    return genericGetFrameName(VelocitySensors, sens_index, frameName, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorFrameName);
+}
+
+bool MultipleAnalogSensorsRemapper::getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
+{
+    return genericGetMeasure(VelocitySensors, sens_index, out, timestamp, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorMeasure);
+}
+
 size_t MultipleAnalogSensorsRemapper::getNrOfOrientationSensors() const
 {
     return m_indicesMap[OrientationSensors].size();