Add message downsampler to avoid logger flooding when changing control mode

src/libraries/icubmod/embObjLib/CMakeLists.txt

@@ -40,7 +40,8 @@ set(EXTRA_SOURCE            ${CMAKE_CURRENT_SOURCE_DIR}/hostTransceiver.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/serviceParser.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/theNVmanager.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/embObjGeneralDevPrivData.cpp
-                            )
+                            ${CMAKE_CURRENT_SOURCE_DIR}/mcEventDownsampler.cpp)
+
 set(NVS_CBK_SOURCE  ${CMAKE_CURRENT_SOURCE_DIR}/protocolCallbacks/EoProtocolMN_fun_userdef.c
                     ${CMAKE_CURRENT_SOURCE_DIR}/protocolCallbacks/EoProtocolMC_fun_userdef.c
                     ${CMAKE_CURRENT_SOURCE_DIR}/protocolCallbacks/EoProtocolAS_fun_userdef.c
@@ -99,4 +100,3 @@ icub_export_library(${PROJECT_NAME})
 endif(NOT ICUB_HAS_icub_firmware_shared)
 
 endif(ICUB_COMPILE_EMBOBJ_LIBRARY)
-

src/libraries/icubmod/embObjLib/mcEventDownsampler.cpp

@@ -0,0 +1,109 @@
+// --------------------------------------------------------------------------------------------------------------------
+// - public interface
+// --------------------------------------------------------------------------------------------------------------------
+
+#include "mcEventDownsampler.h"
+
+namespace mced {
+
+
+    YARP_LOG_COMPONENT(MC_EVENT_DOWNSAMPLER, "mc.msgdownsampler.skipped-cmd-wrong-mode")
+
+
+    mcEventDownsampler::mcEventDownsampler() 
+    {
+        mutex = new std::mutex();
+    };
+
+    mcEventDownsampler::~mcEventDownsampler()
+    {
+
+        stop();
+
+        if(nullptr != timer)
+        {
+            delete timer;
+            timer = nullptr;
+        }
+
+        delete mutex;
+    }
+
+    bool mcEventDownsampler::canprint()
+    {
+        mutex->lock();
+
+        counter++;
+        size_t diff = counter - latch_1;
+        bool cp = !isdownsampling;
+
+        mutex->unlock();
+
+        if(diff > 5)
+        {
+            return false;
+        } 
+        else
+        {
+            return cp;
+        }
+    }
+
+    bool mcEventDownsampler::stop()
+    {
+
+        if(nullptr != timer)
+        {
+            timer->stop();
+            return true;
+        }
+
+        return false;
+    }
+
+    bool mcEventDownsampler::start(const Config &config)
+    {
+        if(nullptr != timer)
+        {
+            stop(); 
+        }
+
+        expire_time = yarp::os::Time::now();        
+        yarp::os::TimerSettings ts(config.period);
+        timer = new yarp::os::Timer(ts, &mcEventDownsampler::step, this, true, mutex);
+
+        if (timer != nullptr) 
+        {
+            timer->start();
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+
+    bool mcEventDownsampler::step(const yarp::os::YarpTimerEvent &event)
+    {
+        if (yarp::os::Time::now() - expire_time >= 1)
+        {
+            expire_time = yarp::os::Time::now();
+
+            isdownsampling = (counter - latch_1 > 5);
+
+            if (isdownsampling)
+            {
+                printreport();
+                latch_2 = counter;
+            }
+            latch_1 = counter;            
+        }
+        return true;
+    }
+
+    void mcEventDownsampler::printreport()
+    {
+        yCError(MC_EVENT_DOWNSAMPLER) <<  "Detected " << counter - latch_2 << " events on aggregate since the last message";
+    }
+
+} // mced

src/libraries/icubmod/embObjLib/mcEventDownsampler.h

@@ -0,0 +1,110 @@
+
+#ifndef __EVENT_DOWNSAMPLER_H_
+#define __EVENT_DOWNSAMPLER_H_
+
+#include <string>
+#include <mutex>
+
+#include <stddef.h>
+#include <stdint.h>
+#include <yarp/os/Timer.h>
+#include <yarp/os/Time.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/LogComponent.h>
+
+namespace mced {
+
+    class mcEventDownsampler
+    {
+    public:
+
+        /**
+         * @brief Contains the configurable parameters of the event downsampler
+         * @var Config::period the period of the timer
+         */
+        struct Config
+        {
+            double period {0.};
+            Config() = default;
+            constexpr Config(double c) : period(c) {} 
+            bool isvalid() const { return 0. != period; }
+        };
+
+        /**
+         * @brief Construct a new Event Downsampler object and instantiates the mutex
+         *        needed by the timer
+         */
+        mcEventDownsampler();
+
+        /**
+         * @brief Destroy the Event Downsampler object. It stops the timer then deletes it.
+         * 
+         */
+        ~mcEventDownsampler();      
+
+        /**
+         * @brief Instantiates the yarp::os::Timer object and starts it.
+         * @param config Structure containing the configuration parameters of the Event Downsampler
+         * @return true if the instantiation was successful, false otherwise.
+         */
+        bool start(const Config &config);   
+
+        /**
+         * @brief Stops the timer
+         * @return true if the operation was successful, false otherwise.
+         */
+        bool stop();
+
+        /**
+         * @brief Called by the object that implements the downsampler. Compares the difference between
+         *        counter and latch_1 is and the threshold.
+         * @return true if the difference is lower or equal to the threshold, false if it is higher. 
+         */
+        bool canprint();
+        Config config = 0.1;
+
+    private: 
+        /**
+         * @brief Callback function called by the timer at each tick. Implements the downsampling logic:
+         *        the downsampling is activated if the number of events exceed a threshold, and a report
+         *        showing the number of events since the last report is printed.
+         *        The mutex is locked before entering the function, and unlocked right after returning.
+         *        
+         * @param event Structure containing data regarding the callback.
+         * @return true 
+         */
+        bool step(const yarp::os::YarpTimerEvent &event); 
+
+        /**
+         * @brief Prints an error message in the logger with a cumulative number of events occurred since the
+         *        last call.
+         */
+        void printreport();
+
+        /** @brief Pointer to the timer object. */
+        yarp::os::Timer* timer {nullptr};
+
+        /**  @brief Number of seconds since epoch. Used to check when one second has elapsed. */
+        double expire_time;
+
+        /**  @brief Indicates when the downsampling is active. It is set to true when the number of events
+         * exceeds a threshold
+         */
+        bool isdownsampling {false};
+
+        /**  @brief Events counter. size_t should be enough for the next 5 billion years. */
+        size_t counter {0};
+
+        /**  @brief Support variable used to evaluate the difference with the counter. */
+        size_t latch_1{0};
+
+        /**  @brief Support variable used to evaluate the difference with the counter. */
+        size_t latch_2{0};
+
+        /**  @brief Mutex needed by the timer. */
+        std::mutex *mutex {nullptr};
+    };
+
+} // mced
+
+#endif  // __EVENT_DOWNSAMPLER_H_

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp

@@ -399,6 +399,11 @@ bool embObjMotionControl::open(yarp::os::Searchable &config)
         }
     }
 
+    // Initialize the downsampler timer
+
+    event_downsampler = new mced::mcEventDownsampler();
+    event_downsampler->config.period = 0.01;
+    event_downsampler->start(event_downsampler->config);
 
     if(false == res->serviceVerifyActivate(eomn_serv_category_mc, servparam))
     {
@@ -1442,7 +1447,7 @@ bool embObjMotionControl::close()
         mcdiagnostics.config.par16 = 0;
     }
 
-
+    delete event_downsampler;
     // in cleanup, at date of 23feb2016 there is a call to ethManager->releaseResource() which ...
     // send to config all the boards and stops tx and rx treads.
     // thus, in here we cannot call serviceStop(mc) because there will be tx/rx activity only for the first call of ::close().
@@ -1894,7 +1899,10 @@ bool embObjMotionControl::velocityMoveRaw(int j, double sp)
         (mode != VOCAB_CM_IMPEDANCE_VEL) &&
         (mode != VOCAB_CM_IDLE))
     {
-        yError() << "velocityMoveRaw: skipping command because " << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_VELOCITY mode";
+        if(event_downsampler->canprint())
+        {
+            yError() << "velocityMoveRaw: skipping command because " << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_VELOCITY mode";
+        }
         return true;
     }
 
@@ -2221,7 +2229,10 @@ bool embObjMotionControl::positionMoveRaw(int j, double ref)
         (mode != VOCAB_CM_IMPEDANCE_POS) &&
         (mode != VOCAB_CM_IDLE))
     {
-        yError() << "positionMoveRaw: skipping command because " << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_POSITION mode";
+        if (event_downsampler->canprint())
+        {
+            yError() << "positionMoveRaw: skipping command because " << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_POSITION mode";
+        }
         return true;
     }
 
@@ -4103,7 +4114,10 @@ bool embObjMotionControl::setPositionRaw(int j, double ref)
     if (mode != VOCAB_CM_POSITION_DIRECT &&
         mode != VOCAB_CM_IDLE)
     {
-        yError() << "setReferenceRaw: skipping command because" << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_POSITION_DIRECT mode";
+        if(event_downsampler->canprint())
+        {
+            yError() << "setReferenceRaw: skipping command because" << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_POSITION_DIRECT mode";
+        }
         return true;
     }
 

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h

@@ -35,6 +35,7 @@ using namespace std;
 //  Yarp stuff
 #include <yarp/os/Bottle.h>
 #include <yarp/os/BufferedPort.h>
+#include <yarp/os/Timer.h>
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/ControlBoardHelper.h>
@@ -52,6 +53,8 @@ using namespace std;
 #include "eomcParser.h"
 #include "measuresConverter.h"
 
+#include "mcEventDownsampler.h"
+
 #ifdef NETWORK_PERFORMANCE_BENCHMARK 
 #include <PeriodicEventsVerifier.h>
 #endif
@@ -177,7 +180,7 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     public ICurrentControlRaw,
     public ImplementCurrentControl,
     public eth::IethResource
-{
+    {
 
 
 private:
@@ -229,7 +232,9 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     int *                                   _axisMap;   /** axies map*/
     std::vector<eomc::axisInfo_t>           _axesInfo;
     /////// end configuration info
-
+
+    // event downsampler
+    mced::mcEventDownsampler* event_downsampler;
 
 #ifdef VERIFY_ROP_SETIMPEDANCE
     uint32_t *impedanceSignature;
@@ -607,9 +612,6 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     virtual bool setRefCurrentsRaw(const int n_joint, const int *joints, const double *t) override;
     virtual bool getRefCurrentsRaw(double *t) override;
     virtual bool getRefCurrentRaw(int j, double *t) override;
-
-
 };
 
 #endif // include guard
-