From eded4bb68f99f3534fd4bf7463c806dae677ce1f Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 18 Sep 2024 09:28:47 +0200 Subject: [PATCH 1/4] 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 #include #include +#include #include #include #include @@ -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 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 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 lock(m_mutex); + return m_positions.size(); +} + +// end yarp::dev::IEncoderArrays methods From 919527b803ffce2ccceb0fdcecde55465c3af241 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 18 Sep 2024 12:32:07 +0200 Subject: [PATCH 2/4] maissensor: Add support for disableImplicitNetworkWrapper and yarpDeviceName in gazebo_yarp_maissensor --- .../include/gazebo/ControlBoard.hh | 2 +- .../maissensor/include/gazebo/MaisSensor.hh | 5 + plugins/maissensor/src/MaisSensor.cc | 204 +++++++++++------- .../maissensor/src/MaisSensorDeviceDriver.cpp | 2 +- 4 files changed, 133 insertions(+), 80 deletions(-) diff --git a/plugins/controlboard/include/gazebo/ControlBoard.hh b/plugins/controlboard/include/gazebo/ControlBoard.hh index c8cf2c1e7..463e58e2b 100644 --- a/plugins/controlboard/include/gazebo/ControlBoard.hh +++ b/plugins/controlboard/include/gazebo/ControlBoard.hh @@ -59,7 +59,7 @@ private: bool m_useVirtAnalogSensor = false; #endif yarp::dev::PolyDriver m_controlboardDriver; - bool m_deviceRegistered; + bool m_deviceRegistered{false}; std::string m_scopedDeviceName; std::string m_yarpDeviceName; diff --git a/plugins/maissensor/include/gazebo/MaisSensor.hh b/plugins/maissensor/include/gazebo/MaisSensor.hh index 47bfa9f7f..bd29a6862 100644 --- a/plugins/maissensor/include/gazebo/MaisSensor.hh +++ b/plugins/maissensor/include/gazebo/MaisSensor.hh @@ -36,6 +36,11 @@ public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); private: + yarp::dev::PolyDriver m_maisEncodersDriver; + bool m_deviceRegistered{false}; + std::string m_scopedDeviceName; + std::string m_yarpDeviceName; + yarp::dev::PolyDriver m_wrapper; yarp::dev::IMultipleWrapper* m_iWrap; diff --git a/plugins/maissensor/src/MaisSensor.cc b/plugins/maissensor/src/MaisSensor.cc index 03d7a44a0..8516b8b81 100644 --- a/plugins/maissensor/src/MaisSensor.cc +++ b/plugins/maissensor/src/MaisSensor.cc @@ -25,7 +25,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpMaisSensor) GazeboYarpMaisSensor::GazeboYarpMaisSensor() : m_iWrap(0) { - + } GazeboYarpMaisSensor::~GazeboYarpMaisSensor() @@ -38,7 +38,17 @@ GazeboYarpMaisSensor::~GazeboYarpMaisSensor() if (m_wrapper.isValid()) m_wrapper.close(); - GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_sensorName); + if (m_deviceRegistered) + { + if (m_parameters.check("disableImplicitNetworkWrapper")) + { + GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName); + } + else + { + GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_sensorName); + } + } GazeboYarpPlugins::Handler::getHandler()->removeRobot(m_robotName); yarp::os::Network::fini(); @@ -74,101 +84,139 @@ void GazeboYarpMaisSensor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) return; } - yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER"); - if(wrapper_group.isNull()) + bool disable_wrapper = m_parameters.check("disableImplicitNetworkWrapper"); + + if (disable_wrapper && !m_parameters.check("yarpDeviceName")) { - yError("GazeboYarpMaisSensor : [WRAPPER] group not found in config file\n"); + yError() << "GazeboYarpMaisSensor : missing yarpDeviceName parameter for one device in robot " << m_robotName; return; } - if(m_parameters.check("ROS")) + if (!disable_wrapper) { - std::string ROS; - ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); - wrapper_group.append(yarp::os::Bottle(ROS)); - } - - //Open the wrapper - if( m_wrapper.open(wrapper_group) ) - { - } - else - { - yError()<<"GazeboYarpMaisSensor Plugin failed: error in opening yarp driver wrapper"; - return; - } + yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER"); + if(wrapper_group.isNull()) + { + yError("GazeboYarpMaisSensor : [WRAPPER] group not found in config file\n"); + return; + } - - yarp::os::Bottle *netList = wrapper_group.find("networks").asList(); + if(m_parameters.check("ROS")) + { + std::string ROS; + ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); + wrapper_group.append(yarp::os::Bottle(ROS)); + } - if (netList->isNull()) - { - yError("GazeboYarpMaisSensor : net list to attach to was not found, load failed."); - m_wrapper.close(); - return; - } - - //--------------------------------------------- - yarp::dev::PolyDriverDescriptor newPoly; - - if (netList->size()!=1) - { - yError("GazeboYarpMaisSensor: size of 'networks' parameter cannot be != 1"); - } - - newPoly.key = netList->get(0).asString(); - m_sensorName = m_robotName + "::" + newPoly.key.c_str(); - newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(m_sensorName); + //Open the wrapper + if( m_wrapper.open(wrapper_group) ) + { + } + else + { + yError()<<"GazeboYarpMaisSensor Plugin failed: error in opening yarp driver wrapper"; + return; + } - yarp::os::Bottle driver_group; - if( newPoly.poly != NULL) - { - // device already exists, use it, setting it againg to increment the usage counter. - yError("mais %s already opened", newPoly.key.c_str()); - } - else - { - driver_group = m_parameters.findGroup(newPoly.key.c_str()); - if (driver_group.isNull()) + yarp::os::Bottle *netList = wrapper_group.find("networks").asList(); + + if (netList->isNull()) { - yError("GazeboYarpMaisSensor::Load Error: [%s] group not found in config file. Closing wrapper for device [%s].", newPoly.key.c_str(), m_sensorName.c_str()); + yError("GazeboYarpMaisSensor : net list to attach to was not found, load failed."); + m_wrapper.close(); return; } - m_parameters.put("name", newPoly.key.c_str()); - m_parameters.fromString(driver_group.toString(), false); + //--------------------------------------------- + yarp::dev::PolyDriverDescriptor newPoly; + + if (netList->size()!=1) + { + yError("GazeboYarpMaisSensor: size of 'networks' parameter cannot be != 1"); + } + + newPoly.key = netList->get(0).asString(); + m_sensorName = m_robotName + "::" + newPoly.key.c_str(); + newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(m_sensorName); + + yarp::os::Bottle driver_group; + if( newPoly.poly != NULL) + { + // device already exists, use it, setting it againg to increment the usage counter. + yError("mais %s already opened", newPoly.key.c_str()); + } + else + { + driver_group = m_parameters.findGroup(newPoly.key.c_str()); + if (driver_group.isNull()) + { + yError("GazeboYarpMaisSensor::Load Error: [%s] group not found in config file. Closing wrapper for device [%s].", newPoly.key.c_str(), m_sensorName.c_str()); + return; + } + + m_parameters.put("name", newPoly.key.c_str()); + m_parameters.fromString(driver_group.toString(), false); + m_parameters.put("robotScopedName", m_robotName); + m_parameters.put("device","gazebo_maissensor"); + + newPoly.poly = new yarp::dev::PolyDriver; + if(! newPoly.poly->open(m_parameters) || ! newPoly.poly->isValid()) + { + yError() << "mais <" << newPoly.key << "> did not open!!"; + newPoly.poly->close(); + return; + } + } + GazeboYarpPlugins::Handler::getHandler()->setDevice(m_sensorName, newPoly.poly); + + //------------------ + if (!m_wrapper.isValid()) + { + yError("GazeboYarpMaisSensor: wrapper did not open"); + } + + if (!m_wrapper.view(m_iWrap)) + { + yError("Wrapper interface not found"); + } + + //Attach the driver to the wrapper + yarp::dev::PolyDriverList driver_list; + + m_deviceRegistered = true; + yInfo() << "GazeboYarpMaisSensor: Registered YARP device with instance name:" << m_sensorName; + driver_list.push(newPoly.poly,"dummy"); + + if( m_iWrap->attachAll(driver_list) ) { + } else { + yError() << "GazeboYarpMaisSensor : error in connecting wrapper and device "; + } + } + else + { + m_yarpDeviceName = m_parameters.find("yarpDeviceName").asString(); + m_scopedDeviceName = m_robotName + "::" + m_yarpDeviceName; + + m_parameters.put("name", m_scopedDeviceName); m_parameters.put("robotScopedName", m_robotName); m_parameters.put("device","gazebo_maissensor"); - newPoly.poly = new yarp::dev::PolyDriver; - if(! newPoly.poly->open(m_parameters) || ! newPoly.poly->isValid()) + if (!m_maisEncodersDriver.open(m_parameters) || ! m_maisEncodersDriver.isValid()) { - yError() << "mais <" << newPoly.key << "> did not open!!"; - newPoly.poly->close(); + yError() << "mais <" << m_yarpDeviceName.c_str() << "> did not open."; + m_maisEncodersDriver.close(); return; } - } - GazeboYarpPlugins::Handler::getHandler()->setDevice(m_sensorName, newPoly.poly); - - //------------------ - if (!m_wrapper.isValid()) - { - yError("GazeboYarpMaisSensor: wrapper did not open"); - } - if (!m_wrapper.view(m_iWrap)) - { - yError("Wrapper interface not found"); - } - - //Attach the driver to the wrapper - yarp::dev::PolyDriverList driver_list; - - driver_list.push(newPoly.poly,"dummy"); - - if( m_iWrap->attachAll(driver_list) ) { - } else { - yError() << "GazeboYarpForceTorque : error in connecting wrapper and device "; + //Register the device with the given name + if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(m_scopedDeviceName, &m_maisEncodersDriver)) + { + yError() << "GazeboYarpMaisSensor: failed setting scopedDeviceName(=" << m_scopedDeviceName << ")"; + return; + } + m_deviceRegistered = true; + yInfo() << "GazeboYarpMaisSensor: Registered YARP device with instance name:" << m_scopedDeviceName; + } } diff --git a/plugins/maissensor/src/MaisSensorDeviceDriver.cpp b/plugins/maissensor/src/MaisSensorDeviceDriver.cpp index 299348e14..3f5e6b5d3 100644 --- a/plugins/maissensor/src/MaisSensorDeviceDriver.cpp +++ b/plugins/maissensor/src/MaisSensorDeviceDriver.cpp @@ -22,7 +22,7 @@ bool GazeboYarpMaisSensorDriver::open(yarp::os::Searchable& config) std::string robotName; robotName = m_pluginParameters.find("robotScopedName").asString().c_str(); - + if (robotName == "") { yError() << "GazeboYarpMaisSensorDriver error: 'robotName' parameter not found'"; From 18ff959504303ee60ce9ab8fe6418695a23285ad Mon Sep 17 00:00:00 2001 From: Silvio Date: Wed, 18 Sep 2024 14:18:07 +0200 Subject: [PATCH 3/4] Cleanup whitespace --- plugins/maissensor/src/MaisSensorDriver.cpp | 22 +++++++++------------ 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/plugins/maissensor/src/MaisSensorDriver.cpp b/plugins/maissensor/src/MaisSensorDriver.cpp index 4791aaa20..061acb9ed 100644 --- a/plugins/maissensor/src/MaisSensorDriver.cpp +++ b/plugins/maissensor/src/MaisSensorDriver.cpp @@ -179,11 +179,7 @@ bool GazeboYarpMaisSensorDriver::setJointNames() //WORKS for (unsigned int gazebo_joint = 0; gazebo_joint < gazebo_models_joints.size() && !joint_found; gazebo_joint++) { std::string gazebo_joint_name = gazebo_models_joints[gazebo_joint]->GetName(); - - //char buff[1000]; - //sprintf(buff, "full:'%s' sub:'%s'", gazebo_joint_name.c_str(),controlboard_joint_names[i].c_str()); - //yDebug() << "***" << buff; - + if (GazeboYarpPlugins::hasEnding(gazebo_joint_name,controlboard_joint_names[i])) { joint_found = true; @@ -305,7 +301,7 @@ int GazeboYarpMaisSensorDriver::calibrateSensor() { std::lock_guard lock(m_mutex); // not implemented - return 0; + return 0; } int GazeboYarpMaisSensorDriver::calibrateSensor(const yarp::sig::Vector& value) @@ -333,14 +329,14 @@ int GazeboYarpMaisSensorDriver::calibrateChannel(int ch, double value) // start yarp::dev::IEncoderArrays methods -size_t GazeboYarpMaisSensorDriver::getNrOfEncoderArrays() const +size_t GazeboYarpMaisSensorDriver::getNrOfEncoderArrays() const { return 1; } -yarp::dev::MAS_status GazeboYarpMaisSensorDriver::getEncoderArrayStatus(size_t sens_index) const +yarp::dev::MAS_status GazeboYarpMaisSensorDriver::getEncoderArrayStatus(size_t sens_index) const { - if (sens_index >= 1) + if (sens_index >= 1) { return yarp::dev::MAS_UNKNOWN; } @@ -348,9 +344,9 @@ yarp::dev::MAS_status GazeboYarpMaisSensorDriver::getEncoderArrayStatus(size_t s return yarp::dev::MAS_OK; } -bool GazeboYarpMaisSensorDriver::getEncoderArrayName(size_t sens_index, std::string &name) const +bool GazeboYarpMaisSensorDriver::getEncoderArrayName(size_t sens_index, std::string &name) const { - if (sens_index >= 1) + if (sens_index >= 1) { return false; } @@ -362,7 +358,7 @@ bool GazeboYarpMaisSensorDriver::getEncoderArrayName(size_t sens_index, std::str bool GazeboYarpMaisSensorDriver::getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const { - if (sens_index >= 1) + if (sens_index >= 1) { return false; } @@ -376,7 +372,7 @@ bool GazeboYarpMaisSensorDriver::getEncoderArrayMeasure(size_t sens_index, yarp: size_t GazeboYarpMaisSensorDriver::getEncoderArraySize(size_t sens_index) const { - if (sens_index >= 1) + if (sens_index >= 1) { return 0; } From 4403b9f8c10ca0ad05cb7abe9a551231a20f21f2 Mon Sep 17 00:00:00 2001 From: Silvio Date: Wed, 18 Sep 2024 14:37:05 +0200 Subject: [PATCH 4/4] 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) {