Merge branch 'bus-load' into road-to-canhalla

libraries/CanBusSharerLib/ICanMessageNotifier.hpp

@@ -0,0 +1,33 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#ifndef __CAN_MESSAGE_NOTIFIER_HPP__
+#define __CAN_MESSAGE_NOTIFIER_HPP__
+
+#include "CanMessage.hpp"
+
+namespace roboticslab
+{
+
+/**
+ * @ingroup CanBusSharerLib
+ * @brief Implementation-agnostic consumer for TX CAN transfers.
+ *
+ * Implementors can use this class to forward implementation-specific CAN message
+ * structures from the internal TX buffer (if any) to the point of consumption
+ * and the final CAN read routines.
+ *
+ * Cf. @ref CanSenderDelegate.
+ */
+class CanMessageNotifier
+{
+public:
+    //! Virtual destructor.
+    virtual ~CanMessageNotifier() = default;
+
+    //! Notify observers that a new CAN message has arrived.
+    virtual bool notifyMessage(const can_message & msg) = 0;
+};
+
+} // namespace roboticslab
+
+#endif // __CAN_MESSAGE_NOTIFIER_HPP__

libraries/YarpPlugins/CanBusControlboard/BusLoadMonitor.cpp

@@ -0,0 +1,39 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "BusLoadMonitor.hpp"
+
+#include <cmath>
+
+using namespace roboticslab;
+
+// -----------------------------------------------------------------------------
+
+namespace
+{
+    inline unsigned long computeLength(unsigned int len)
+    {
+        // 44-bit base frame + 3-bit intermission field + stuff bits, see https://w.wiki/GDt
+        return 8 * len + 44 + std::floor((34 + 8 * len - 1) / 4.0) + 3;
+    }
+}
+
+// -----------------------------------------------------------------------------
+
+bool BusLoadMonitor::notifyMessage(const can_message & msg)
+{
+    bits += computeLength(msg.len);
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+void BusLoadMonitor::run()
+{
+    double rate = bits.exchange(0) / getPeriod();
+    auto & b = prepare();
+    b.clear();
+    b.addFloat64(rate / bitrate);
+    write();
+}
+
+// -----------------------------------------------------------------------------

libraries/YarpPlugins/CanBusControlboard/BusLoadMonitor.hpp

@@ -0,0 +1,47 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#ifndef __BUS_LOAD_MONITOR_HPP__
+#define __BUS_LOAD_MONITOR_HPP__
+
+#include <atomic>
+
+#include <yarp/os/Bottle.h>
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/os/PortWriterBuffer.h>
+
+#include "CanMessageNotifier.hpp"
+
+namespace roboticslab
+{
+
+/**
+ * @ingroup CanBusControlboard
+ * @brief Periodically sends CAN bus load stats through a YARP port.
+ */
+class BusLoadMonitor final : public yarp::os::PeriodicThread,
+                             public yarp::os::PortWriterBuffer<yarp::os::Bottle>,
+                             public CanMessageNotifier
+{
+public:
+    //! Constructor.
+    BusLoadMonitor(double period) : yarp::os::PeriodicThread(period), bitrate(1.0)
+    { }
+
+    //! Tell observers a new CAN message has arrived.
+    virtual bool notifyMessage(const can_message & msg) override;
+
+    void setBitrate(unsigned int bitrate)
+    { this->bitrate = bitrate; }
+
+protected:
+    //! The thread will invoke this periodically.
+    virtual void run() override;
+
+private:
+    unsigned int bitrate;
+    std::atomic<unsigned long> bits;
+};
+
+} // namespace roboticslab
+
+#endif // __BUS_LOAD_MONITOR_HPP__

libraries/YarpPlugins/CanBusControlboard/CMakeLists.txt

@@ -42,6 +42,8 @@ if(NOT SKIP_CanBusControlboard)
                                        CanRxTxThreads.cpp
                                        SdoReplier.hpp
                                        SdoReplier.cpp
+                                       BusLoadMonitor.hpp
+                                       BusLoadMonitor.cpp
                                        YarpCanSenderDelegate.hpp
                                        YarpCanSenderDelegate.cpp)
 

libraries/YarpPlugins/CanBusControlboard/CanBusBroker.cpp

@@ -18,7 +18,8 @@ CanBusBroker::CanBusBroker(const std::string & _name)
       writerThread(nullptr),
       iCanBus(nullptr),
       iCanBusErrors(nullptr),
-      iCanBufferFactory(nullptr)
+      iCanBufferFactory(nullptr),
+      busLoadMonitor(nullptr)
 { }
 
 // -----------------------------------------------------------------------------
@@ -30,7 +31,9 @@ CanBusBroker::~CanBusBroker()
     dumpPort.close();
     sendPort.close();
     sdoPort.close();
+    busLoadPort.close();
 
+    delete busLoadMonitor;
     delete readerThread;
     delete writerThread;
 }
@@ -51,6 +54,19 @@ bool CanBusBroker::configure(const yarp::os::Searchable & config)
         return false;
     }
 
+    if (config.check("busLoadPeriod", "CAN bus load monitor period (seconds)"))
+    {
+        double busLoadPeriod = config.find("busLoadPeriod").asFloat64();
+
+        if (busLoadPeriod <= 0.0)
+        {
+            CD_WARNING("Illegal CAN bus load monitor option period: %f.\n", busLoadPeriod);
+            return false;
+        }
+
+        busLoadMonitor = new BusLoadMonitor(busLoadPeriod);
+    }
+
     readerThread = new CanReaderThread(name, rxDelay, rxBufferSize);
     writerThread = new CanWriterThread(name, txDelay, txBufferSize);
 
@@ -84,6 +100,19 @@ bool CanBusBroker::registerDevice(yarp::dev::PolyDriver * driver)
         return false;
     }
 
+    if (busLoadMonitor)
+    {
+        unsigned int bitrate;
+
+        if (!iCanBus->canGetBaudRate(&bitrate))
+        {
+            CD_WARNING("Cannot get bitrate.\n");
+            return false;
+        }
+
+        busLoadMonitor->setBitrate(bitrate);
+    }
+
     if (readerThread)
     {
         readerThread->setCanHandles(iCanBus, iCanBusErrors, iCanBufferFactory);
@@ -119,15 +148,23 @@ bool CanBusBroker::createPorts(const std::string & prefix)
         return false;
     }
 
+    if (busLoadMonitor && !busLoadPort.open(prefix + "/load:o"))
+    {
+        CD_WARNING("Cannot open bus load port.\n");
+        return false;
+    }
+
     if (readerThread)
     {
         readerThread->attachDumpWriter(&dumpWriter, &dumpMutex);
         readerThread->attachCanNotifier(&sdoReplier);
+        readerThread->attachBusLoadMonitor(busLoadMonitor);
     }
 
     if (writerThread)
     {
         writerThread->attachDumpWriter(&dumpWriter, &dumpMutex);
+        writerThread->attachBusLoadMonitor(busLoadMonitor);
         sdoReplier.configureSender(writerThread->getDelegate());
     }
 
@@ -140,6 +177,12 @@ bool CanBusBroker::createPorts(const std::string & prefix)
 
     sdoPort.setReader(sdoReplier);
 
+    if (busLoadMonitor)
+    {
+        busLoadPort.setInputMode(false);
+        busLoadMonitor->attach(busLoadPort);
+    }
+
     return true;
 }
 
@@ -187,6 +230,12 @@ bool CanBusBroker::clearFilters()
 
 bool CanBusBroker::startThreads()
 {
+    if (busLoadMonitor && !busLoadMonitor->start())
+    {
+        CD_WARNING("Cannot start bus load monitor thread.\n");
+        return false;
+    }
+
     if (!readerThread || !readerThread->start())
     {
         CD_WARNING("Cannot start reader thread.\n");
@@ -210,6 +259,12 @@ bool CanBusBroker::stopThreads()
     commandReader.disableCallback();
     dumpPort.interrupt();
     sdoPort.interrupt();
+    busLoadPort.interrupt();
+
+    if (busLoadMonitor && busLoadMonitor->isRunning())
+    {
+        busLoadMonitor->stop();
+    }
 
     bool ok = true;
 

libraries/YarpPlugins/CanBusControlboard/CanBusBroker.hpp

@@ -19,6 +19,7 @@
 
 #include "CanRxTxThreads.hpp"
 #include "SdoReplier.hpp"
+#include "BusLoadMonitor.hpp"
 
 namespace roboticslab
 {
@@ -99,6 +100,9 @@ class CanBusBroker final : public yarp::os::TypedReaderCallback<yarp::os::Bottle
 
     yarp::os::RpcServer sdoPort;
     SdoReplier sdoReplier;
+
+    yarp::os::Port busLoadPort;
+    BusLoadMonitor * busLoadMonitor;
 };
 
 } // namespace roboticslab

libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.cpp

@@ -36,21 +36,21 @@ void CanReaderWriterThread::onStop()
 
 // -----------------------------------------------------------------------------
 
-void CanReaderWriterThread::dumpMessage(const yarp::dev::CanMessage & msg)
+void CanReaderWriterThread::dumpMessage(const can_message & msg)
 {
     std::lock_guard<std::mutex> lock(*dumpMutex);
 
     yarp::os::Bottle & b = dumpWriter->prepare();
     b.clear();
-    b.addInt16(msg.getId());
+    b.addInt16(msg.id);
 
-    if (msg.getLen() != 0)
+    if (msg.len != 0)
     {
         yarp::os::Bottle & data = b.addList();
 
-        for (int j = 0; j < msg.getLen(); j++)
+        for (int j = 0; j < msg.len; j++)
         {
-            data.addInt8(msg.getData()[j]);
+            data.addInt8(msg.data[j]);
         }
     }
 
@@ -97,12 +97,12 @@ void CanReaderThread::run()
 
         for (int i = 0; i < read; i++)
         {
-            const yarp::dev::CanMessage & msg = canBuffer[i];
-            auto it = canIdToHandle.find(msg.getId() & 0x7F);
+            can_message msg {canBuffer[i].getId(), canBuffer[i].getLen(), canBuffer[i].getData()};
+            auto it = canIdToHandle.find(msg.id & 0x7F);
 
             if (it != canIdToHandle.end())
             {
-                it->second->notifyMessage({msg.getId(), msg.getLen(), msg.getData()});
+                it->second->notifyMessage(msg);
             }
 
             if (dumpWriter)
@@ -112,7 +112,12 @@ void CanReaderThread::run()
 
             if (canMessageNotifier)
             {
-                canMessageNotifier->notifyMessage({msg.getId(), msg.getLen(), msg.getData()});
+                canMessageNotifier->notifyMessage(msg);
+            }
+
+            if (busLoadMonitor)
+            {
+                busLoadMonitor->notifyMessage(msg);
             }
         }
     }
@@ -162,11 +167,21 @@ void CanWriterThread::flush()
         return;
     }
 
-    if (dumpWriter)
+    if (dumpWriter || busLoadMonitor)
     {
         for (int i = 0; i < sent; i++)
         {
-            dumpMessage(canBuffer[i]);
+            can_message msg {canBuffer[i].getId(), canBuffer[i].getLen(), canBuffer[i].getData()};
+
+            if (dumpWriter)
+            {
+                dumpMessage(msg);
+            }
+
+            if (busLoadMonitor)
+            {
+                busLoadMonitor->notifyMessage(msg);
+            }
         }
     }
 

libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.hpp

@@ -31,7 +31,7 @@ class CanReaderWriterThread : public yarp::os::Thread
     //! Constructor.
     CanReaderWriterThread(const std::string & type, const std::string & id, double delay, unsigned int bufferSize)
         : iCanBus(nullptr), iCanBusErrors(nullptr), iCanBufferFactory(nullptr),
-          dumpWriter(nullptr), dumpMutex(nullptr),
+          dumpWriter(nullptr), dumpMutex(nullptr), busLoadMonitor(nullptr),
           bufferSize(bufferSize), delay(delay), type(type), id(id)
     { }
 
@@ -69,9 +69,13 @@ class CanReaderWriterThread : public yarp::os::Thread
     void attachDumpWriter(yarp::os::PortWriterBuffer<yarp::os::Bottle> * dumpWriter, std::mutex * dumpMutex)
     { this->dumpWriter = dumpWriter; this->dumpMutex = dumpMutex; }
 
+    //! Attach CAN bus load monitor.
+    void attachBusLoadMonitor(CanMessageNotifier * busLoadMonitor)
+    { this->busLoadMonitor = busLoadMonitor; }
+
 protected:
     //! Dump CAN message through a YARP port.
-    void dumpMessage(const yarp::dev::CanMessage & msg);
+    void dumpMessage(const can_message & msg);
 
     yarp::dev::ICanBus * iCanBus;
     yarp::dev::ICanBusErrors * iCanBusErrors;
@@ -81,6 +85,8 @@ class CanReaderWriterThread : public yarp::os::Thread
     yarp::os::PortWriterBuffer<yarp::os::Bottle> * dumpWriter;
     std::mutex * dumpMutex;
 
+    CanMessageNotifier * busLoadMonitor;
+
     unsigned int bufferSize;
     double delay;