From 4403b9f8c10ca0ad05cb7abe9a551231a20f21f2 Mon Sep 17 00:00:00 2001 From: Silvio Date: Wed, 18 Sep 2024 14:37:05 +0200 Subject: [PATCH] maissensor: Add support to specify value returned by getEncoderArrayName method via sensorName parameter --- .../include/yarp/dev/MaisSensorDriver.h | 3 ++- plugins/maissensor/src/MaisSensorDriver.cpp | 20 +++++++++++++++---- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/plugins/maissensor/include/yarp/dev/MaisSensorDriver.h b/plugins/maissensor/include/yarp/dev/MaisSensorDriver.h index 61b842230..dd429993b 100644 --- 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 controlboard_joint_names; std::vector 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 index 061acb9ed..f65135223 100644 --- 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) {