Enable the temperature reading in RobotInterface::Helper

Author: GiulioRomualdi

src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h

@@ -16,6 +16,8 @@
 #include <yarp/dev/IPositionDirect.h>
 #include <yarp/dev/IVelocityControl.h>
 #include <yarp/dev/IInteractionMode.h>
+#include <yarp/dev/IMotor.h>
+
 #include <yarp/sig/Vector.h>
 #include <yarp/os/Timer.h>
 
@@ -48,6 +50,7 @@ namespace WalkingControllers
         yarp::dev::IControlMode *m_controlModeInterface{nullptr}; /**< Control mode interface. */
         yarp::dev::IControlLimits *m_limitsInterface{nullptr}; /**< Encorders interface. */
         yarp::dev::IInteractionMode *m_interactionInterface{nullptr}; /**< Stiff/compliant mode interface. */
+        yarp::dev::IMotor *m_motorInterface{nullptr}; /**< Motor interface. */
 
         std::unique_ptr<WalkingPIDHandler> m_PIDHandler; /**< Pointer to the PID handler object. */
 
@@ -66,6 +69,13 @@ namespace WalkingControllers
         iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Joint Velocity bounds [rad/s]. */
         iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Joint Position upper bound [rad]. */
         iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Joint Position lower bound [rad]. */
+
+        iDynTree::VectorDynSize m_motorTemperatures; /**< Motor temperature [Celsius]. */
+        mutable std::mutex m_motorTemperatureMutex; /**< Mutex for the motor temperature. */
+        std::atomic_bool m_motorTemperatureTreadIsRunning; /**< True if the motor temperature is updated. */
+        double m_motorTemperatureDt{0.5};
+        std::thread m_motorTemperatureThread; /**< Thread for the motor temperature. */
+
         // yarp::sig::Vector m_positionFeedbackDegFiltered;
         yarp::sig::Vector m_velocityFeedbackDegFiltered; /**< Vector containing the filtered joint velocity [deg/s]. */
         std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_positionFilter; /**< Joint position low pass filter .*/
@@ -127,6 +137,8 @@ namespace WalkingControllers
                                         bool useWrenchFilter,
                                         double cutFrequency,
                                         MeasuredWrench& measuredWrench);
+
+        void readMotorTemperature();
     public:
 
         /**
@@ -205,6 +217,9 @@ namespace WalkingControllers
          */
         const iDynTree::VectorDynSize& getJointVelocity() const;
 
+
+        iDynTree::VectorDynSize getMotorTemperature() const;
+
         /**
          * Get the joint upper limit
          * @return the joint upper bound in radiants

src/RobotInterface/src/Helper.cpp

@@ -313,6 +313,8 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
         }
     }
 
+    m_motorTemperatures.resize(m_actuatedDOFs);
+
     // open the device
     if(!m_robotDevice.open(options))
     {
@@ -363,6 +365,12 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
         return false;
     }
 
+    if (!m_robotDevice.view(m_motorInterface) || !m_motorInterface)
+    {
+        yError() << "[configureRobot] Cannot obtain IMotor interface";
+        return false;
+    }
+
     // resize the buffers
     m_positionFeedbackDeg.resize(m_actuatedDOFs, 0.0);
     m_velocityFeedbackDeg.resize(m_actuatedDOFs, 0.0);
@@ -482,9 +490,31 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
         return false;
     }
 
+    m_motorTemperatureThread = std::thread(&RobotInterface::readMotorTemperature, this);
+
     return true;
 }
 
+void RobotInterface::readMotorTemperature()
+{
+    m_motorTemperatureTreadIsRunning = true;
+
+    while(m_motorTemperatureTreadIsRunning)
+    {
+        {
+            std::lock_guard<std::mutex> lock(m_motorTemperatureMutex);
+
+            if(!m_motorInterface->getTemperatures(m_motorTemperatures.data()))
+            {
+                yError() << "[RobotInterface::readMotorTemperature] Unable to get the motor temperatures.";
+            }
+        }
+
+        // TODO GR you should load the time required for the  read and close it
+        yarp::os::Time::delay(m_motorTemperatureDt);
+    }
+}
+
 bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix,
                                                 const std::string& portInputName,
                                                 const std::string& wholeBodyDynamicsPortName,
@@ -1096,6 +1126,12 @@ bool RobotInterface::setVelocityReferences(const iDynTree::VectorDynSize& desire
 
 bool RobotInterface::close()
 {
+    m_motorTemperatureTreadIsRunning = false;
+    if (m_motorTemperatureThread.joinable())
+    {
+        m_motorTemperatureThread.join();
+    }
+
     // close all the ports
     for(auto& wrench : m_leftFootMeasuredWrench)
         wrench.port->close();
@@ -1119,11 +1155,18 @@ const iDynTree::VectorDynSize& RobotInterface::getJointPosition() const
 {
     return m_positionFeedbackRad;
 }
+
 const iDynTree::VectorDynSize& RobotInterface::getJointVelocity() const
 {
     return m_velocityFeedbackRad;
 }
 
+iDynTree::VectorDynSize RobotInterface::getMotorTemperature() const
+{
+    std::lock_guard lock(m_motorTemperatureMutex);
+    return m_motorTemperatures;
+}
+
 const iDynTree::Wrench& RobotInterface::getLeftWrench() const
 {
     return m_leftWrench;