Merge branch 'yarp-modes' into apocanlypse

libraries/CanBusSharerLib/CanOpen.cpp

@@ -18,9 +18,7 @@ CanOpen::CanOpen(unsigned int id, double sdoTimeout, double stateTimeout, CanSen
       _emcy(new EmcyConsumer(_sdo)),
       _nmt(new NmtProtocol(_id, _sdo, sender)),
       _driveStatus(new DriveStatusMachine(_rpdo1, stateTimeout))
-{
-    _tpdo1->registerHandler<std::uint16_t>([this](std::uint16_t statusword) { _driveStatus->update(statusword); });
-}
+{ }
 
 CanOpen::~CanOpen()
 {

libraries/CanBusSharerLib/CanUtils.cpp

@@ -7,11 +7,11 @@
 
 using namespace roboticslab::CanUtils;
 
-std::string msgToStr(uint8_t id, uint16_t cob, size_t len, const uint8_t * msgData)
+std::string msgToStr(std::uint8_t id, std::uint16_t cob, std::size_t len, const std::uint8_t * msgData)
 {
     std::stringstream tmp;
 
-    for (size_t i = 0; i < len - 1; i++)
+    for (std::size_t i = 0; i < len - 1; i++)
     {
         tmp << std::hex << std::setfill('0') << std::setw(2) << static_cast<int>(msgData[i]) << " ";
     }

libraries/CanBusSharerLib/CanUtils.hpp

@@ -3,7 +3,9 @@
 #ifndef __CAN_UTILS_HPP__
 #define __CAN_UTILS_HPP__
 
-#include <stdint.h>
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
 
 #include <string>
 
@@ -14,7 +16,7 @@ namespace roboticslab
 namespace CanUtils
 {
 
-std::string msgToStr(uint8_t id, uint16_t cob, size_t len, const uint8_t * msgData);
+std::string msgToStr(std::uint8_t id, std::uint16_t cob, std::size_t len, const std::uint8_t * msgData);
 
 inline std::string msgToStr(const yarp::dev::CanMessage & message)
 { return msgToStr(message.getId() & 0x7F, message.getId() & 0xFF80, message.getLen(), message.getData()); }

libraries/CanBusSharerLib/DriveStatusMachine.cpp

@@ -4,6 +4,7 @@
 
 #include <bitset>
 #include <unordered_map>
+#include <vector>
 
 using namespace roboticslab;
 
@@ -89,7 +90,7 @@ namespace
         }
     }
 
-    std::unordered_map<std::pair<DriveState, DriveTransition>, DriveState> transitionToState = {
+    const std::unordered_map<std::pair<DriveState, DriveTransition>, DriveState> transitionToState = {
         {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveTransition::SHUTDOWN), DriveState::READY_TO_SWITCH_ON}, // 2
         {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveTransition::SWITCH_ON), DriveState::SWITCHED_ON}, // 3
         {std::make_pair(DriveState::SWITCHED_ON, DriveTransition::ENABLE_OPERATION), DriveState::OPERATION_ENABLED}, // 4
@@ -104,6 +105,23 @@ namespace
         {std::make_pair(DriveState::QUICK_STOP_ACTIVE, DriveTransition::DISABLE_VOLTAGE), DriveState::SWITCH_ON_DISABLED}, // 12
         {std::make_pair(DriveState::QUICK_STOP_ACTIVE, DriveTransition::ENABLE_OPERATION), DriveState::OPERATION_ENABLED}, // 16
     };
+
+    const std::unordered_map<std::pair<DriveState, DriveState>, std::vector<DriveTransition>> shortestPaths = {
+        {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveState::READY_TO_SWITCH_ON), {DriveTransition::SHUTDOWN}},
+        {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveState::SWITCHED_ON), {DriveTransition::SHUTDOWN, DriveTransition::SWITCH_ON}},
+        {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveState::OPERATION_ENABLED), {DriveTransition::SHUTDOWN, DriveTransition::SWITCH_ON, DriveTransition::ENABLE_OPERATION}},
+        {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveState::SWITCHED_ON), {DriveTransition::SWITCH_ON}},
+        {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveState::OPERATION_ENABLED), {DriveTransition::SWITCH_ON, DriveTransition::ENABLE_OPERATION}},
+        {std::make_pair(DriveState::SWITCHED_ON, DriveState::OPERATION_ENABLED), {DriveTransition::ENABLE_OPERATION}},
+        {std::make_pair(DriveState::OPERATION_ENABLED, DriveState::SWITCHED_ON), {DriveTransition::DISABLE_OPERATION}},
+        {std::make_pair(DriveState::OPERATION_ENABLED, DriveState::READY_TO_SWITCH_ON), {DriveTransition::SHUTDOWN}},
+        {std::make_pair(DriveState::OPERATION_ENABLED, DriveState::SWITCH_ON_DISABLED), {DriveTransition::DISABLE_VOLTAGE}},
+        {std::make_pair(DriveState::SWITCHED_ON, DriveState::READY_TO_SWITCH_ON), {DriveTransition::SHUTDOWN}},
+        {std::make_pair(DriveState::SWITCHED_ON, DriveState::SWITCH_ON_DISABLED), {DriveTransition::DISABLE_VOLTAGE}},
+        {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveState::SWITCH_ON_DISABLED), {DriveTransition::DISABLE_VOLTAGE}},
+        {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveState::SWITCHED_ON), {DriveTransition::SWITCH_ON}},
+        {std::make_pair(DriveState::QUICK_STOP_ACTIVE, DriveState::SWITCH_ON_DISABLED), {DriveTransition::DISABLE_VOLTAGE}}
+    };
 }
 
 bool DriveStatusMachine::update(std::uint16_t statusword)
@@ -116,12 +134,12 @@ bool DriveStatusMachine::update(std::uint16_t statusword)
     return stateObserver.notify();
 }
 
-std::bitset<16> & DriveStatusMachine::controlword()
+DriveStatusMachine::word_t & DriveStatusMachine::controlword()
 {
     return _controlword;
 }
 
-const std::bitset<16> & DriveStatusMachine::statusword() const
+const DriveStatusMachine::word_t & DriveStatusMachine::statusword() const
 {
     std::lock_guard<std::mutex> lock(stateMutex);
     return _statusword;
@@ -134,31 +152,60 @@ DriveState DriveStatusMachine::getCurrentState() const
 
 bool DriveStatusMachine::requestTransition(DriveTransition transition, bool wait)
 {
+    DriveState initialState = getCurrentState();
+    auto op = std::make_pair(initialState, transition);
+    auto it = transitionToState.find(op);
+
+    if (it == transitionToState.cend())
+    {
+        return false;
+    }
+
     prepareDriveTransition(transition, _controlword);
     std::uint16_t data = _controlword.to_ulong();
 
-    DriveState initialState = getCurrentState();
-
     if (!rpdo->write(data))
     {
         return false;
     }
 
     if (wait)
     {
-        auto op = std::make_pair(initialState, transition);
-        std::uint16_t resp;
+        return stateObserver.await() && it->second == getCurrentState();
+    }
 
-        if (!stateObserver.await())
-        {
-            return false;
-        }
+    return true;
+}
 
-        if (transitionToState[op] != resp)
+bool DriveStatusMachine::requestState(DriveState goalState)
+{
+    DriveState initialState = getCurrentState();
+
+    if (goalState == initialState)
+    {
+        return true;
+    }
+
+    auto op = std::make_pair(initialState, goalState);
+    auto it = shortestPaths.find(op);
+
+    if (it == shortestPaths.cend())
+    {
+        return false;
+    }
+
+    for (const auto & transition : it->second)
+    {
+        if (!requestTransition(transition, true))
         {
             return false;
         }
     }
 
     return true;
 }
+
+DriveState parseStatusword(std::uint16_t statusword)
+{
+    return parseDriveState(statusword);
+}

libraries/CanBusSharerLib/DriveStatusMachine.hpp

@@ -42,6 +42,8 @@ enum class DriveTransition
 class DriveStatusMachine
 {
 public:
+    typedef std::bitset<16> word_t;
+
     DriveStatusMachine(ReceivePdo * rpdo, double timeout)
         : rpdo(rpdo), stateObserver(timeout)
     { }
@@ -50,14 +52,17 @@ class DriveStatusMachine
     { this->rpdo = rpdo; }
 
     bool update(std::uint16_t statusword);
-    std::bitset<16> & controlword();
-    const std::bitset<16> & statusword() const;
+    word_t & controlword();
+    const word_t & statusword() const;
     DriveState getCurrentState() const;
     bool requestTransition(DriveTransition transition, bool wait = true);
+    bool requestState(DriveState goalState);
+
+    static DriveState parseStatusword(std::uint16_t statusword);
 
 private:
-    std::bitset<16> _controlword;
-    std::bitset<16> _statusword;
+    word_t _controlword;
+    word_t _statusword;
     ReceivePdo * rpdo;
     StateObserver stateObserver;
     mutable std::mutex stateMutex;

libraries/CanBusSharerLib/ICanBusSharer.hpp

@@ -13,51 +13,28 @@ namespace roboticslab
 {
 
 /**
- *
  * @brief Abstract base for a CAN bus sharer.
- *
  */
 class ICanBusSharer
 {
 public:
-    /**
-     * Destructor.
-     */
+
     virtual ~ICanBusSharer() {}
 
     virtual unsigned int getId() = 0;
 
     virtual std::vector<unsigned int> getAdditionalIds()
     { return {}; }
 
-    /** initial configuration on pre-operational state (only SDOs allowed) */
-    virtual bool initialize() { return true; }
-
-    /** "start". Figure 5.1 Drive’s status machine. States and transitions (p68, 84/263). */
-    virtual bool start() = 0;
-
-    /** "ready to switch on", also acts as "shutdown" */
-    virtual bool readyToSwitchOn() = 0;
+    virtual bool initialize() = 0;
 
-    /** "switch on", also acts as "disable operation" */
-    virtual bool switchOn() = 0;
+    virtual bool finalize() = 0;
 
-    /** enable */
-    virtual bool enable() = 0;
-
-    /** recoverFromError */
-    virtual bool recoverFromError() = 0;
-
-    /**
-     * Interpret a can bus message.
-     * @return true/false.
-     */
     virtual bool interpretMessage(const yarp::dev::CanMessage & message) = 0;
 
     virtual bool registerSender(CanSenderDelegate * sender) = 0;
-
 };
 
-}  // namespace roboticslab
+} // namespace roboticslab
 
-#endif  //  __I_CAN_BUS_SHARER_HPP__
+#endif // __I_CAN_BUS_SHARER_HPP__

libraries/YarpPlugins/CMakeLists.txt

@@ -12,7 +12,6 @@ target_include_directories(YarpDevicesInterfaces INTERFACE $<TARGET_PROPERTY:YAR
 
 # Install interface headers.
 install(FILES IProximitySensors.h
-              ITechnosoftIpos.h
         DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
 
 # Register export set.

libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp

@@ -12,6 +12,7 @@
 
 #include <ColorDebug.h>
 
+#include "ICanBusSharer.hpp"
 #include "PositionDirectThread.hpp"
 #include "DeviceMapper.hpp"
 #include "CanRxTxThreads.hpp"
@@ -324,7 +325,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver,
     yarp::dev::PolyDriverList nodes;
 
     DeviceMapper deviceMapper;
-    std::vector<int> motorIds;
+    std::vector<ICanBusSharer *> iCanBusSharers;
 
     CanReaderThread * canReaderThread;
     CanWriterThread * canWriterThread;

libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.cpp

@@ -8,13 +8,19 @@
 
 using namespace roboticslab;
 
-CanReaderThread::CanReaderThread(const std::string & id,
-        const std::map<int, int> & _idxFromCanId,
-        const std::vector<ICanBusSharer *> & _iCanBusSharers)
-    : CanReaderWriterThread("read", id),
-      idxFromCanId(_idxFromCanId),
-      iCanBusSharers(_iCanBusSharers)
-{}
+CanReaderThread::CanReaderThread(const std::string & id, const std::vector<ICanBusSharer *> & iCanBusSharers)
+    : CanReaderWriterThread("read", id)
+{
+    for (auto p : iCanBusSharers)
+    {
+        canIdToHandle[p->getId()] = p;
+
+        for (auto id : p->getAdditionalIds())
+        {
+            canIdToHandle[id] = p;
+        }
+    }
+}
 
 void CanReaderThread::run()
 {
@@ -36,11 +42,10 @@ void CanReaderThread::run()
         for (int i = 0; i < read; i++)
         {
             const yarp::dev::CanMessage & msg = canBuffer[i];
-            int canId = msg.getId() & 0x7F;
-
-            std::map<int, int>::const_iterator idxFromCanIdFound = idxFromCanId.find(canId);
+            const int canId = msg.getId() & 0x7F;
+            auto it = canIdToHandle.find(canId);
 
-            if (idxFromCanIdFound == idxFromCanId.end())  //-- Can ID not found
+            if (it == canIdToHandle.end())  //-- Can ID not found
             {
                 //-- Intercept 700h 0 msg that just indicates presence.
                 if (msg.getId() - canId == 0x700)
@@ -51,7 +56,7 @@ void CanReaderThread::run()
                 continue;
             }
 
-            iCanBusSharers[idxFromCanIdFound->second]->interpretMessage(msg);
+            it->second->interpretMessage(msg);
         }
     }
 }

libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.hpp

@@ -14,6 +14,7 @@
 
 #include <ColorDebug.h>
 
+#include "CanSenderDelegate.hpp"
 #include "ICanBusSharer.hpp"
 
 namespace roboticslab
@@ -29,21 +30,19 @@ class CanReaderWriterThread : public yarp::os::Thread
         : iCanBus(0), iCanBufferFactory(0), type(type), id(id), bufferSize(0), period(0.0)
     {}
 
-    virtual void run() = 0;
-
-    virtual bool threadInit()
+    virtual bool threadInit() override
     { canBuffer = iCanBufferFactory->createBuffer(bufferSize); return true; }
 
-    virtual void threadRelease()
+    virtual void threadRelease() override
     { iCanBufferFactory->destroyBuffer(canBuffer); }
 
-    virtual void beforeStart()
+    virtual void beforeStart() override
     { CD_INFO("Initializing CanBusControlboard %s thread %s.\n", type.c_str(), id.c_str()); }
 
-    virtual void afterStart(bool success)
+    virtual void afterStart(bool success) override
     { CD_INFO("Configuring CanBusControlboard %s thread %s... %s\n", type.c_str(), id.c_str(), success ? "success" : "failure"); }
 
-    virtual void onStop()
+    virtual void onStop() override
     { CD_INFO("Stopping CanBusControlboard %s thread %s.\n", type.c_str(), id.c_str()); }
 
     virtual void setCanHandles(yarp::dev::ICanBus * iCanBus, yarp::dev::ICanBufferFactory * iCanBufferFactory, int bufferSize)
@@ -71,15 +70,11 @@ class CanReaderWriterThread : public yarp::os::Thread
 class CanReaderThread : public CanReaderWriterThread
 {
 public:
-    CanReaderThread(const std::string & id,
-            const std::map<int, int> & idxFromCanId,
-            const std::vector<ICanBusSharer *> & iCanBusSharers);
-
-    virtual void run();
+    CanReaderThread(const std::string & id, const std::vector<ICanBusSharer *> & iCanBusSharers);
+    virtual void run() override;
 
 private:
-    std::map<int, int> idxFromCanId;
-    std::vector<ICanBusSharer *> iCanBusSharers;
+    std::map<unsigned int, ICanBusSharer *> canIdToHandle;
 };
 
 /**
@@ -91,8 +86,8 @@ class CanWriterThread : public CanReaderWriterThread
     CanWriterThread(const std::string & id);
     ~CanWriterThread();
 
-    virtual void run();
-    virtual void setCanHandles(yarp::dev::ICanBus * iCanBus, yarp::dev::ICanBufferFactory * iCanBufferFactory, int bufferSize);
+    virtual void run() override;
+    virtual void setCanHandles(yarp::dev::ICanBus * iCanBus, yarp::dev::ICanBufferFactory * iCanBufferFactory, int bufferSize) override;
     CanSenderDelegate * getDelegate();
 
 private: