Merge branch 'master' into fix655

robotology · Jul 31, 2023 · 61ec5a9 · 61ec5a9
2 parents 183ecd3 + 691f610
commit 61ec5a9
Show file tree

Hide file tree

Showing 4 changed files with 107 additions and 16 deletions.
diff --git a/.gitignore b/.gitignore
@@ -8,3 +8,5 @@ build*/
 .cproject
 .settings/
 .vscode/
+**/compile_commands.json
+**/.cache
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,9 @@ 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).
+
 ### Fixed
 - Fix installation of import libraries on Windows (https://github.com/robotology/gazebo-yarp-plugins/pull/657).
 

diff --git 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
@@ -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<std::mutex> lock(m_dataMutex);
     name = m_sensorName;
     return true;
 }
@@ -177,6 +181,7 @@ bool GazeboYarpForceTorqueDriver::getSixAxisForceTorqueSensorFrameName(size_t se
         return false;
     }
 
+    std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(m_dataMutex);
+    out[0] = fakeTemperatureValue;
+    timestamp = m_lastTimestamp.getTime();
+
+    return true;
+}
+
 
 //PRECISELY TIMED
 yarp::os::Stamp GazeboYarpForceTorqueDriver::getLastInputStamp()