maissensor: Add support to specify value returned by getEncoderArrayN…

…ame method via sensorName parameter
robotology · Sep 18, 2024 · 4403b9f · 4403b9f
1 parent 18ff959
commit 4403b9f
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 5 deletions.
diff --git a/plugins/maissensor/include/yarp/dev/MaisSensorDriver.h b/plugins/maissensor/include/yarp/dev/MaisSensorDriver.h
@@ -130,6 +130,7 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
     std::vector<std::string> controlboard_joint_names;
     std::vector<gazebo::physics::JointPtr> m_jointPointers; /* pointers for each joint, avoiding several calls to getJoint(joint_name) */
     gazebo::transport::NodePtr m_gazeboNode;
+    std::string m_encoderArrayName{"gazebo_yarp_maisdriver_default_sensor_name"};
 
 
     bool started;
@@ -150,7 +151,7 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
     virtual int calibrateSensor(const yarp::sig::Vector& value);
     virtual int calibrateChannel(int ch);
     virtual int calibrateChannel(int ch, double value);
-    
+
     /**
      * \brief convert data read from Gazebo to user unit sistem,
      *  e.g. degrees for revolute joints and meters for prismatic joints

diff --git a/plugins/maissensor/src/MaisSensorDriver.cpp b/plugins/maissensor/src/MaisSensorDriver.cpp
@@ -55,6 +55,19 @@ bool GazeboYarpMaisSensorDriver::gazebo_init()
 
     if (!setJointNames()) return false;      // this function also fills in the m_jointPointers vector
 
+    // Force users to set the sensorName parameter to specify the value returned by the getEncoderArrayName,
+    // as in this case we do not have a SDF sensor corresponding to the mais sensor, so we can't get the name from the URDF/SDF model
+    if (m_pluginParameters.check("disableImplicitNetworkWrapper") && !m_pluginParameters.check("sensorName"))
+    {
+        yError() << "GazeboYarpMaisSensor: Missing parameter sensorName (it is the value returned by the getEncoderArrayName method).";
+        return false;
+    }
+
+    if (m_pluginParameters.check("sensorName"))
+    {
+        m_encoderArrayName = m_pluginParameters.find("sensorName").asString();
+    }
+
     m_channels_num = 15;
     m_numberOfJoints = m_jointNames.size();
 
@@ -351,9 +364,8 @@ bool GazeboYarpMaisSensorDriver::getEncoderArrayName(size_t sens_index, std::str
         return false;
     }
 
-    // TODO(traversaro): we need to understand which name to return
-    name = "";
-    return false;
+    name = m_encoderArrayName;
+    return true;
 }
 
 bool GazeboYarpMaisSensorDriver::getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const 
@@ -370,7 +382,7 @@ bool GazeboYarpMaisSensorDriver::getEncoderArrayMeasure(size_t sens_index, yarp:
     return true;
 }
 
-size_t GazeboYarpMaisSensorDriver::getEncoderArraySize(size_t sens_index) const 
+size_t GazeboYarpMaisSensorDriver::getEncoderArraySize(size_t sens_index) const
 {
     if (sens_index >= 1)
     {