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,105 @@
+// --------------------------------------------------------------------------------------------------------------------
+// - public interface
+// --------------------------------------------------------------------------------------------------------------------
+
+#include "mcEventDownsampler.h"
+
+namespace mced {
+
+    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()
+    {
+        yError() <<  "Detected " << (counter - latch_2) << " events on aggregate since the last message";
+    }
+
+} // mced

src/libraries/icubmod/embObjLib/mcEventDownsampler.h

@@ -0,0 +1,51 @@
+
+// - include guard ----------------------------------------------------------------------------------------------------
+
+#ifndef __TICKER_H_
+#define __TICKER_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>
+
+namespace mced {
+
+    class mcEventDownsampler
+    {
+    public:
+
+        struct Config
+        {
+            double period {0};    // time between two ticks
+            Config() = default;
+            constexpr Config(double c) : period(c) {} 
+            bool isvalid() const { return 0 != period; }
+        };
+
+        mcEventDownsampler();
+        ~mcEventDownsampler();            
+        bool start(const Config &config);              
+        bool stop(); 
+        bool canprint();
+        Config config = 0.1;
+
+    private: 
+        bool step(const yarp::os::YarpTimerEvent &event); 
+        void printreport();
+        yarp::os::Timer* timer {nullptr};    
+        double expire_time;
+        bool isdownsampling {false};
+        size_t counter {0};  // size_t should be enough for the next 5 billion years
+        size_t latch_1{0};
+        size_t latch_2{0};
+        std::mutex *mutex {nullptr};
+    };
+
+} // mced
+
+#endif  // include-guard

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;
@@ -256,7 +261,7 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     double  *_ref_accs;         // for velocity control, in position min jerk eq is used.
     double  *_encodersStamp;                    /** keep information about acquisition time for encoders read */
     bool  *checking_motiondone;                 /* flag telling if I'm already waiting for motion done */
-    #define MAX_POSITION_MOVE_INTERVAL 0.080
+    #define MAX_POSITION_MOVE_INTERVAL 0.0001
     double *_last_position_move_time;           /** time stamp for last received position move command*/    
     eOmc_impedance_t *_cacheImpedance;    /* cache impedance value to split up the 2 sets */
 
@@ -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
-