From eded4bb68f99f3534fd4bf7463c806dae677ce1f Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Wed, 18 Sep 2024 09:28:47 +0200
Subject: [PATCH] maissensor: Expose data via yarp::dev::IEncoderArrays

---
 .../include/yarp/dev/MaisSensorDriver.h       | 12 +++-
 plugins/maissensor/src/MaisSensorDriver.cpp   | 59 +++++++++++++++++++
 2 files changed, 69 insertions(+), 2 deletions(-)

diff --git a/plugins/maissensor/include/yarp/dev/MaisSensorDriver.h b/plugins/maissensor/include/yarp/dev/MaisSensorDriver.h
index 4a3a8eea4..61b842230 100644
--- a/plugins/maissensor/include/yarp/dev/MaisSensorDriver.h
+++ b/plugins/maissensor/include/yarp/dev/MaisSensorDriver.h
@@ -13,6 +13,7 @@
 #include <yarp/dev/ControlBoardInterfacesImpl.h>
 #include <yarp/dev/ControlBoardInterfaces.h>
 #include <yarp/dev/IAnalogSensor.h>
+#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/sig/Vector.h>
 #include <yarp/os/Time.h>
 #include <yarp/os/Stamp.h>
@@ -61,7 +62,8 @@ namespace gazebo {
 class yarp::dev::GazeboYarpMaisSensorDriver:
     public DeviceDriver,
     public DeviceResponder,
-    public IAnalogSensor
+    public IAnalogSensor,
+    public IEncoderArrays
 {
 public:
 
@@ -83,6 +85,12 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
     virtual bool open(yarp::os::Searchable& config);
     virtual bool close();
 
+    // yarp::dev::IEncoderArrays
+    virtual size_t getNrOfEncoderArrays() const override;
+    virtual yarp::dev::MAS_status getEncoderArrayStatus(size_t sens_index) const override;
+    virtual bool getEncoderArrayName(size_t sens_index, std::string &name) const override;
+    virtual bool getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
+    virtual size_t getEncoderArraySize(size_t sens_index) const override;
 
 private:
 
@@ -114,7 +122,7 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
 
     yarp::os::Stamp m_lastTimestamp; /**< timestamp, updated with simulation time at each onUpdate call */
 
-    std::mutex m_mutex;
+    mutable std::mutex m_mutex;
     yarp::sig::VectorOf<JointType> m_jointTypes;
 
     int m_channels_num;
diff --git a/plugins/maissensor/src/MaisSensorDriver.cpp b/plugins/maissensor/src/MaisSensorDriver.cpp
index 37de46084..4791aaa20 100644
--- a/plugins/maissensor/src/MaisSensorDriver.cpp
+++ b/plugins/maissensor/src/MaisSensorDriver.cpp
@@ -280,6 +280,7 @@ double * GazeboYarpMaisSensorDriver::convertUserToGazebo(double *values)
     return values;
 }
 
+// start yarp::dev::IAnalogSensor methods
 
 int GazeboYarpMaisSensorDriver::read(yarp::sig::Vector &out)
 {
@@ -327,3 +328,61 @@ int GazeboYarpMaisSensorDriver::calibrateChannel(int ch, double value)
     // not implemented
     return 0;
 }
+
+// end yarp::dev::IAnalogSensor methods
+
+// start yarp::dev::IEncoderArrays methods
+
+size_t GazeboYarpMaisSensorDriver::getNrOfEncoderArrays() const 
+{
+    return 1;
+}
+
+yarp::dev::MAS_status GazeboYarpMaisSensorDriver::getEncoderArrayStatus(size_t sens_index) const 
+{
+    if (sens_index >= 1) 
+    {
+        return yarp::dev::MAS_UNKNOWN;
+    }
+
+    return yarp::dev::MAS_OK;
+}
+
+bool GazeboYarpMaisSensorDriver::getEncoderArrayName(size_t sens_index, std::string &name) const 
+{
+    if (sens_index >= 1) 
+    {
+        return false;
+    }
+
+    // TODO(traversaro): we need to understand which name to return
+    name = "";
+    return false;
+}
+
+bool GazeboYarpMaisSensorDriver::getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const 
+{
+    if (sens_index >= 1) 
+    {
+        return false;
+    }
+
+    std::lock_guard<std::mutex> lock(m_mutex);
+    timestamp = m_lastTimestamp.getTime();
+    out.resize(m_positions.size());
+    out = m_positions;
+    return true;
+}
+
+size_t GazeboYarpMaisSensorDriver::getEncoderArraySize(size_t sens_index) const 
+{
+    if (sens_index >= 1) 
+    {
+        return 0;
+    }
+
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return m_positions.size();
+}
+
+// end yarp::dev::IEncoderArrays methods