From 11b663663eeb6ee0987f6b6ae08e59aa5e21fc5a Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sun, 19 Jun 2022 19:17:18 +0200 Subject: [PATCH 1/3] Expose yarp::dev::ISixAxisForceTorqueSensors interface in gazebo_yarp_forcetorque plugin --- CHANGELOG.md | 4 +- .../include/yarp/dev/ForceTorqueDriver.h | 15 ++++- plugins/forcetorque/src/ForceTorque.cc | 1 + plugins/forcetorque/src/ForceTorqueDriver.cpp | 64 +++++++++++++++++++ 4 files changed, 81 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 351fcde3b..99fc2b2ce 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,8 +5,10 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] -## [4.4.0] - 2022-05-31 +### Added +- The `gazebo_yarp_forcetorque` plugin now exposes also the `yarp::dev::ISixAxisForceTorqueSensors` interface, so it can be used with ` multipleanalogsensorsserver`, `multipleanalogsensorsremapper` and `multipleanalogsensorsclient` devices (https://github.com/robotology/gazebo-yarp-plugins/issues/384). +## [4.4.0] - 2022-05-31 ### Fixed - Removed implicit conversions from the depth data quantization in `GazeboYarpDepthCameraDriver::getDepthImage` which in at least one occasion caused an unexpected crash of the `rgbdSensor_nws_yarp`. Also, the calculation of the scalar coefficient (involving `math::pow`), used to cut the decimal figures from the depth data, has been moved outside the for loop that cycles through the whole image (https://github.com/robotology/gazebo-yarp-plugins/pull/623). diff --git a/plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h b/plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h index a7e4a358e..129ad39a7 100644 --- a/plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h +++ b/plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -49,7 +50,8 @@ extern const std::string YarpForceTorqueScopedName; class yarp::dev::GazeboYarpForceTorqueDriver: public yarp::dev::IAnalogSensor, public yarp::dev::IPreciselyTimed, - public yarp::dev::DeviceDriver + public yarp::dev::DeviceDriver, + public yarp::dev::ISixAxisForceTorqueSensors { public: GazeboYarpForceTorqueDriver(); @@ -74,6 +76,13 @@ class yarp::dev::GazeboYarpForceTorqueDriver: virtual int calibrateSensor(const yarp::sig::Vector& value); virtual int calibrateChannel(int channel); + // SIX AXIS FORCE TORQUE SENSORS + virtual size_t getNrOfSixAxisForceTorqueSensors() const; + virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const ; + virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const; + virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const; + virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const; + //PRECISELY TIMED virtual yarp::os::Stamp getLastInputStamp(); @@ -81,9 +90,11 @@ class yarp::dev::GazeboYarpForceTorqueDriver: private: yarp::sig::Vector m_forceTorqueData; //buffer for forcetorque sensor data yarp::os::Stamp m_lastTimestamp; //buffer for last timestamp data - std::mutex m_dataMutex; //mutex for accessing the data + mutable std::mutex m_dataMutex; //mutex for accessing the data gazebo::sensors::ForceTorqueSensor* m_parentSensor; gazebo::event::ConnectionPtr m_updateConnection; + std::string m_sensorName; + std::string m_frameName; }; diff --git a/plugins/forcetorque/src/ForceTorque.cc b/plugins/forcetorque/src/ForceTorque.cc index fac4e8521..a11102edd 100644 --- a/plugins/forcetorque/src/ForceTorque.cc +++ b/plugins/forcetorque/src/ForceTorque.cc @@ -106,6 +106,7 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd //Open the driver //Force the device to be of type "gazebo_forcetorque" (it make sense? probably yes) driver_properties.put("device","gazebo_forcetorque"); + driver_properties.put("sensor_name", _sensor->Name()); if( !m_forceTorqueDriver.open(driver_properties) ) { yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver"; return; diff --git a/plugins/forcetorque/src/ForceTorqueDriver.cpp b/plugins/forcetorque/src/ForceTorqueDriver.cpp index 6f4afb65d..263da18c4 100644 --- a/plugins/forcetorque/src/ForceTorqueDriver.cpp +++ b/plugins/forcetorque/src/ForceTorqueDriver.cpp @@ -63,6 +63,9 @@ bool GazeboYarpForceTorqueDriver::open(yarp::os::Searchable& config) //Get gazebo pointers std::string sensorScopedName(config.find(YarpForceTorqueScopedName.c_str()).asString().c_str()); + m_sensorName = config.find("sensor_name").asString(); + m_frameName = m_sensorName; + m_parentSensor = dynamic_cast(GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName)); if (!m_parentSensor) @@ -139,6 +142,67 @@ int GazeboYarpForceTorqueDriver::calibrateChannel(int /*ch*/, double /*v*/) return AS_OK; } +// SIX AXIS FORCE TORQUE SENSORS +size_t GazeboYarpForceTorqueDriver::getNrOfSixAxisForceTorqueSensors() const +{ + return 1; +} + +yarp::dev::MAS_status GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorStatus(size_t sens_index) const +{ + if (sens_index > 1) + { + return MAS_UNKNOWN; + } + + return MAS_OK; +} + +bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const +{ + if (sens_index > 1) + { + return false; + } + + name = m_sensorName; + return true; +} + +bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const +{ + if (sens_index > 1) + { + return false; + } + + frameName = m_frameName; + return true; +} + +bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +{ + if (sens_index > 1) + { + return false; + } + + if (m_forceTorqueData.size() != YarpForceTorqueChannelsNumber) { + return false; + } + + if (out.size() != YarpForceTorqueChannelsNumber) { + out.resize(YarpForceTorqueChannelsNumber); + } + + std::lock_guard lock(m_dataMutex); + out = m_forceTorqueData; + timestamp = m_lastTimestamp.getTime(); + + return true; +} + + //PRECISELY TIMED yarp::os::Stamp GazeboYarpForceTorqueDriver::getLastInputStamp() { From a99794db8e6001ae77aa50f618ccd097d6b9c6a2 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sun, 19 Jun 2022 20:06:56 +0200 Subject: [PATCH 2/3] Update CHANGELOG.md --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 99fc2b2ce..cd62171bf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] ### Added -- The `gazebo_yarp_forcetorque` plugin now exposes also the `yarp::dev::ISixAxisForceTorqueSensors` interface, so it can be used with ` multipleanalogsensorsserver`, `multipleanalogsensorsremapper` and `multipleanalogsensorsclient` devices (https://github.com/robotology/gazebo-yarp-plugins/issues/384). +- The `gazebo_yarp_forcetorque` plugin now exposes also the `yarp::dev::ISixAxisForceTorqueSensors` interface, so it can be used with ` multipleanalogsensorsserver`, `multipleanalogsensorsremapper` and `multipleanalogsensorsclient` devices (https://github.com/robotology/gazebo-yarp-plugins/issues/384, https://github.com/robotology/gazebo-yarp-plugins/pull/628). ## [4.4.0] - 2022-05-31 From 0662a7a77de0c1baa5e39f8d7957b05314bb9033 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sun, 19 Jun 2022 20:07:37 +0200 Subject: [PATCH 3/3] Fixup --- plugins/forcetorque/src/ForceTorqueDriver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/plugins/forcetorque/src/ForceTorqueDriver.cpp b/plugins/forcetorque/src/ForceTorqueDriver.cpp index 263da18c4..cda03f707 100644 --- a/plugins/forcetorque/src/ForceTorqueDriver.cpp +++ b/plugins/forcetorque/src/ForceTorqueDriver.cpp @@ -150,7 +150,7 @@ size_t GazeboYarpForceTorqueDriver::getNrOfSixAxisForceTorqueSensors() const yarp::dev::MAS_status GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorStatus(size_t sens_index) const { - if (sens_index > 1) + if (sens_index >= 1) { return MAS_UNKNOWN; } @@ -160,7 +160,7 @@ yarp::dev::MAS_status GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorSt bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const { - if (sens_index > 1) + if (sens_index >= 1) { return false; } @@ -171,7 +171,7 @@ bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorName(size_t sens_in bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const { - if (sens_index > 1) + if (sens_index >= 1) { return false; } @@ -182,7 +182,7 @@ bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorFrameName(size_t se bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const { - if (sens_index > 1) + if (sens_index >= 1) { return false; }