From 691f61062be772e4fe2ec6d2d44127c405f22354 Mon Sep 17 00:00:00 2001 From: Hosameldin Mohamed <45564317+HosameldinMohamed@users.noreply.github.com> Date: Mon, 31 Jul 2023 15:54:03 +0200 Subject: [PATCH] [gazebo_yarp_forcetorque] Expose fake temperature measurement (#656) --- .gitignore | 2 + CHANGELOG.md | 4 + .../include/yarp/dev/ForceTorqueDriver.h | 41 ++++++---- plugins/forcetorque/src/ForceTorqueDriver.cpp | 77 +++++++++++++++++++ 4 files changed, 108 insertions(+), 16 deletions(-) diff --git a/.gitignore b/.gitignore index 3f433150e..39b2b19c3 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,5 @@ build*/ .cproject .settings/ .vscode/ +**/compile_commands.json +**/.cache diff --git a/CHANGELOG.md b/CHANGELOG.md index 4c61a99fd..4ccf003af 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,10 @@ 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::ITemperatureSensors` interface, so it can stream fake temperature values (https://github.com/robotology/gazebo-yarp-plugins/pull/656). + ## [4.7.0] - 2023-06-05 ### Changed diff --git a/plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h b/plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h index 129ad39a7..b1dfe0fd5 100644 --- a/plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h +++ b/plugins/forcetorque/include/yarp/dev/ForceTorqueDriver.h @@ -51,7 +51,8 @@ class yarp::dev::GazeboYarpForceTorqueDriver: public yarp::dev::IAnalogSensor, public yarp::dev::IPreciselyTimed, public yarp::dev::DeviceDriver, - public yarp::dev::ISixAxisForceTorqueSensors + public yarp::dev::ISixAxisForceTorqueSensors, + public yarp::dev::ITemperatureSensors { public: GazeboYarpForceTorqueDriver(); @@ -64,27 +65,35 @@ class yarp::dev::GazeboYarpForceTorqueDriver: */ //DEVICE DRIVER - virtual bool open(yarp::os::Searchable& config); - virtual bool close(); + virtual bool open(yarp::os::Searchable& config) override; + virtual bool close() override; //ANALOG SENSOR - virtual int read(yarp::sig::Vector& out); - virtual int getState(int channel); - virtual int getChannels(); - virtual int calibrateChannel(int channel, double v); - virtual int calibrateSensor(); - virtual int calibrateSensor(const yarp::sig::Vector& value); - virtual int calibrateChannel(int channel); + virtual int read(yarp::sig::Vector& out) override; + virtual int getState(int channel) override; + virtual int getChannels() override; + virtual int calibrateChannel(int channel, double v) override; + virtual int calibrateSensor() override; + virtual int calibrateSensor(const yarp::sig::Vector& value) override; + virtual int calibrateChannel(int channel) override; // 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; + virtual size_t getNrOfSixAxisForceTorqueSensors() const override; + virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override; + virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const override; + virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const override; + virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override; + + // TEMPERATURE SENSORS + virtual size_t getNrOfTemperatureSensors() const override; + virtual yarp::dev::MAS_status getTemperatureSensorStatus(size_t sens_index) const override; + virtual bool getTemperatureSensorName(size_t sens_index, std::string &name) const override; + virtual bool getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const override; + virtual bool getTemperatureSensorMeasure(size_t sens_index, double& out, double& timestamp) const override; + virtual bool getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override; //PRECISELY TIMED - virtual yarp::os::Stamp getLastInputStamp(); + virtual yarp::os::Stamp getLastInputStamp() override; private: diff --git a/plugins/forcetorque/src/ForceTorqueDriver.cpp b/plugins/forcetorque/src/ForceTorqueDriver.cpp index 50b1c74bd..61bf8c751 100644 --- a/plugins/forcetorque/src/ForceTorqueDriver.cpp +++ b/plugins/forcetorque/src/ForceTorqueDriver.cpp @@ -21,6 +21,9 @@ using namespace yarp::dev; const unsigned YarpForceTorqueChannelsNumber = 6; //The ForceTorque sensor has 6 fixed channels const std::string YarpForceTorqueScopedName = "sensorScopedName"; +const unsigned YarpTemperatureChannelsNumber = 1; //The Temperature sensor has 1 fixed channel +const double fakeTemperatureValue = 25.0; + GazeboYarpForceTorqueDriver::GazeboYarpForceTorqueDriver() {} GazeboYarpForceTorqueDriver::~GazeboYarpForceTorqueDriver() {} @@ -166,6 +169,7 @@ bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorName(size_t sens_in return false; } + std::lock_guard lock(m_dataMutex); name = m_sensorName; return true; } @@ -177,6 +181,7 @@ bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorFrameName(size_t se return false; } + std::lock_guard lock(m_dataMutex); frameName = m_frameName; return true; } @@ -203,6 +208,78 @@ bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorMeasure(size_t sens return true; } +// TEMPERATURE SENSORS +size_t GazeboYarpForceTorqueDriver::getNrOfTemperatureSensors() const +{ + return 1; +} + +yarp::dev::MAS_status GazeboYarpForceTorqueDriver::getTemperatureSensorStatus(size_t sens_index) const +{ + if (sens_index >= 1) + { + return MAS_UNKNOWN; + } + + return MAS_OK; +} + +bool GazeboYarpForceTorqueDriver::getTemperatureSensorName(size_t sens_index, std::string &name) const +{ + if (sens_index >= 1) + { + return false; + } + + std::lock_guard lock(m_dataMutex); + name = m_sensorName; + return true; +} + +bool GazeboYarpForceTorqueDriver::getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const +{ + if (sens_index >= 1) + { + return false; + } + + std::lock_guard lock(m_dataMutex); + frameName = m_frameName; + return true; +} + +bool GazeboYarpForceTorqueDriver::getTemperatureSensorMeasure(size_t sens_index, double& out, double& timestamp) const +{ + if (sens_index >= 1) + { + return false; + } + + std::lock_guard lock(m_dataMutex); + out = fakeTemperatureValue; + timestamp = m_lastTimestamp.getTime(); + + return true; +} + +bool GazeboYarpForceTorqueDriver::getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +{ + if (sens_index >= 1) + { + return false; + } + + if (out.size() != YarpTemperatureChannelsNumber) { + out.resize(YarpTemperatureChannelsNumber); + } + + std::lock_guard lock(m_dataMutex); + out[0] = fakeTemperatureValue; + timestamp = m_lastTimestamp.getTime(); + + return true; +} + //PRECISELY TIMED yarp::os::Stamp GazeboYarpForceTorqueDriver::getLastInputStamp()