diff --git a/libraries/YarpPlugins/CanBusBroker/CMakeLists.txt b/libraries/YarpPlugins/CanBusBroker/CMakeLists.txt index eb704f221..a47e88786 100644 --- a/libraries/YarpPlugins/CanBusBroker/CMakeLists.txt +++ b/libraries/YarpPlugins/CanBusBroker/CMakeLists.txt @@ -10,6 +10,7 @@ if(NOT SKIP_CanBusBroker) yarp_add_plugin(CanBusBroker CanBusBroker.hpp DeviceDriverImpl.cpp + IMultipleWrapperImpl.cpp # control board interfaces motor/IAmplifierControlImpl.cpp motor/IAxisInfoImpl.cpp diff --git a/libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp b/libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp index fad8bac10..574f13976 100644 --- a/libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp +++ b/libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp @@ -5,6 +5,7 @@ #include +#include #include #include @@ -25,7 +26,7 @@ namespace roboticslab /** * @ingroup CanBusBroker - * @brief CAN-oriented control board that implements all YARP motor interfaces. + * @brief CAN-oriented control board that implements all YARP motor and MAS interfaces. * * This control board wrapper subdevice exposes motor commands to CAN nodes * modelled as wrapped motor raw subdevices (i.e. devices which implement the @@ -38,6 +39,7 @@ namespace roboticslab * instructions. */ class CanBusBroker : public yarp::dev::DeviceDriver, + public yarp::dev::IMultipleWrapper, // control board interfaces public yarp::dev::IAmplifierControl, public yarp::dev::IAxisInfo, @@ -79,6 +81,11 @@ class CanBusBroker : public yarp::dev::DeviceDriver, bool open(yarp::os::Searchable & config) override; bool close() override; + // -------- IMultipleWrapper declarations. Implementation in IMultipleWrapperImpl.cpp -------- + + bool attachAll(const yarp::dev::PolyDriverList & drivers) override; + bool detachAll() override; + // ---------- CONTROL BOARD INTERFACES ---------- // --------- IAmplifierControl declarations. Implementation in IAmplifierControlImpl.cpp --------- @@ -409,11 +416,7 @@ class CanBusBroker : public yarp::dev::DeviceDriver, private: DeviceMapper deviceMapper; - - std::vector busDevices; - std::vector nodeDevices; std::vector brokers; - SyncPeriodicThread * syncThread {nullptr}; }; diff --git a/libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp index b3c6e3ced..3797d0784 100644 --- a/libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp @@ -3,9 +3,9 @@ #include "CanBusBroker.hpp" #include -#include #include +#include "FutureTask.hpp" #include "ICanBusSharer.hpp" #include "LogComponent.hpp" @@ -15,188 +15,50 @@ using namespace roboticslab; bool CanBusBroker::open(yarp::os::Searchable & config) { - if (!config.check("robotConfig") || !config.find("robotConfig").isBlob()) - { - yCError(CBB) << "Missing \"robotConfig\" property or not a blob"; - return false; - } + const auto * buses = config.find("buses").asList(); - const auto * robotConfig = *reinterpret_cast(config.find("robotConfig").asBlob()); - - const auto * canBuses = config.find("buses").asList(); - - if (!canBuses) + if (!buses) { yCError(CBB) << "Missing key \"buses\" or not a list"; return false; } - for (int i = 0; i < canBuses->size(); i++) + for (int i = 0; i < buses->size(); i++) { - auto canBus = canBuses->get(i).asString(); + auto bus = buses->get(i).asString(); - if (!config.check(canBus)) + if (!config.check(bus)) { - yCError(CBB) << "Missing CAN bus key:" << canBus; + yCError(CBB) << "Missing CAN bus key:" << bus; return false; } - const auto & nodesVal = config.find(canBus); - - if (!nodesVal.isList()) + if (!config.find(bus).isList()) { - yCError(CBB) << "Key" << canBus << "must be a list"; + yCError(CBB) << "Key" << bus << "must be a list"; return false; } - const auto * nodes = nodesVal.asList(); - auto isFakeBus = canBus.rfind("fake-", 0) == 0; // starts with "fake-"? - std::vector busSharers; - yarp::os::Bottle nodeIds; + const auto * nodes = config.find(bus).asList(); + + std::vector names; for (int j = 0; j < nodes->size(); j++) { auto node = nodes->get(j).asString(); - auto isFakeNode = node.rfind("fake-", 0) == 0; // starts with "fake-"? - - yarp::os::Property nodeOptions; - nodeOptions.setMonitor(config.getMonitor(), node.c_str()); - - if (!isFakeNode) - { - const auto & nodeGroup = robotConfig->findGroup(node); - - if (nodeGroup.isNull()) - { - yCError(CBB) << "Missing CAN node device group" << node; - return false; - } - - nodeOptions.fromString(nodeGroup.toString()); - nodeOptions.put("robotConfig", config.find("robotConfig")); - - if (config.check("syncPeriod")) - { - nodeOptions.put("syncPeriod", config.find("syncPeriod")); - } - } - else - { - nodeOptions.put("device", "FakeJoint"); - nodeOptions.put("jointName", node.substr(5)); - } - - auto * device = new yarp::dev::PolyDriver; - nodeDevices.push_back(device); - - if (!device->open(nodeOptions)) - { - yCError(CBB) << "CAN node device" << node << "configuration failure"; - return false; - } - - if (!deviceMapper.registerDevice(device)) - { - yCError(CBB) << "Unable to register CAN node device" << node; - return false; - } - - if (!isFakeBus && !isFakeNode) - { - ICanBusSharer * iCanBusSharer; - - if (!device->view(iCanBusSharer)) - { - yCError(CBB) << "Unable to view ICanBusSharer in" << node; - return false; - } - - busSharers.push_back(iCanBusSharer); - nodeIds.addInt32(iCanBusSharer->getId()); - - for (auto id : iCanBusSharer->getAdditionalIds()) - { - nodeIds.addInt32(id); - } - } + names.push_back(node); } - if (!isFakeBus) - { - const auto & canBusGroup = robotConfig->findGroup(canBus); - - if (canBusGroup.isNull()) - { - yCError(CBB) << "Missing CAN bus device group" << canBus; - return false; - } - - yarp::os::Property canBusOptions; - canBusOptions.setMonitor(config.getMonitor(), canBus.c_str()); - canBusOptions.fromString(canBusGroup.toString()); - canBusOptions.put("robotConfig", config.find("robotConfig")); - canBusOptions.put("blockingMode", false); // enforce non-blocking mode - canBusOptions.put("allowPermissive", false); // always check usage requirements - - auto enableAcceptanceFilters = canBusOptions.check("enableAcceptanceFilters", yarp::os::Value(false), - "enable CAN acceptance filters").asBool(); - - if (enableAcceptanceFilters && nodeIds.size() != 0) - { - canBusOptions.put("filteredIds", yarp::os::Value::makeList(nodeIds.toString().c_str())); - } - - auto * canBusDevice = new yarp::dev::PolyDriver; - busDevices.push_back(canBusDevice); - - if (!canBusDevice->open(canBusOptions)) - { - yCError(CBB) << "canBusDevice instantiation failed:" << canBus; - return false; - } - - auto * broker = new SingleBusBroker(canBus); - brokers.push_back(broker); - - if (!broker->configure(canBusOptions)) - { - yCError(CBB) << "Unable to configure broker of CAN bus device" << canBus; - return false; - } - - if (!broker->registerDevice(canBusDevice)) - { - yCError(CBB) << "Unable to register CAN bus device" << canBus; - return false; - } - - for (auto * iCanBusSharer : busSharers) - { - broker->getReader()->registerHandle(iCanBusSharer); - iCanBusSharer->registerSender(broker->getWriter()->getDelegate()); - } - } - } + auto * broker = new SingleBusBroker(bus, names); + brokers.push_back(broker); - for (auto * broker : brokers) - { - if (!broker->startThreads()) + if (!broker->configure(config)) { - yCError(CBB) << "Unable to start CAN threads in" << broker->getName(); + yCError(CBB) << "Unable to configure broker of CAN bus device" << bus; return false; } } - for (const auto & rawDevice : deviceMapper.getDevices()) - { - auto * iCanBusSharer = rawDevice->castToType(); - - if (iCanBusSharer && !iCanBusSharer->initialize()) - { - yCError(CBB) << "Node device id" << iCanBusSharer->getId() << "could not initialize CAN comms"; - } - } - if (config.check("syncPeriod", "SYNC message period (s)")) { auto syncPeriod = config.find("syncPeriod").asFloat64(); @@ -209,11 +71,11 @@ bool CanBusBroker::open(yarp::os::Searchable & config) FutureTaskFactory * taskFactory = nullptr; - if (busDevices.size() > 1) + if (brokers.size() > 1) { - taskFactory = new ParallelTaskFactory(busDevices.size()); + taskFactory = new ParallelTaskFactory(brokers.size()); } - else if (busDevices.size() == 1) + else if (brokers.size() == 1) { taskFactory = new SequentialTaskFactory; } @@ -243,62 +105,21 @@ bool CanBusBroker::open(yarp::os::Searchable & config) yCWarning(CBB) << "Synchronization thread not enabled, use --syncPeriod"; } - return !syncThread || syncThread->start(); + return true; } // ----------------------------------------------------------------------------- bool CanBusBroker::close() { - bool ok = true; - - if (syncThread && syncThread->isRunning()) - { - syncThread->stop(); - } - - delete syncThread; - syncThread = nullptr; - - for (const auto & rawDevice : deviceMapper.getDevices()) - { - auto * iCanBusSharer = rawDevice->castToType(); - - if (iCanBusSharer && !iCanBusSharer->finalize()) - { - yCWarning(CBB) << "Node device id" << iCanBusSharer->getId() << "could not finalize CAN comms"; - ok = false; - } - } - - deviceMapper.clear(); + bool ok = detachAll(); for (auto * broker : brokers) { - ok &= broker->stopThreads(); - ok &= broker->clearFilters(); delete broker; } brokers.clear(); - - for (auto * device : nodeDevices) - { - // CAN read threads must not live beyond this point. - ok &= device->close(); - delete device; - } - - nodeDevices.clear(); - - for (auto * device : busDevices) - { - ok &= device->close(); - delete device; - } - - busDevices.clear(); - return ok; } diff --git a/libraries/YarpPlugins/CanBusBroker/IMultipleWrapperImpl.cpp b/libraries/YarpPlugins/CanBusBroker/IMultipleWrapperImpl.cpp new file mode 100644 index 000000000..aede99b1f --- /dev/null +++ b/libraries/YarpPlugins/CanBusBroker/IMultipleWrapperImpl.cpp @@ -0,0 +1,203 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "CanBusBroker.hpp" + +#include // std::any_of, std::find_if, std::for_each +#include // std::distance + +#include + +#include "ICanBusSharer.hpp" +#include "LogComponent.hpp" + +using namespace roboticslab; + +// ----------------------------------------------------------------------------- + +bool CanBusBroker::attachAll(const yarp::dev::PolyDriverList & drivers) +{ + std::vector nodeNames; // flattened broker[i]->getNodeNames() + + std::for_each(brokers.begin(), brokers.end(), [&nodeNames](const auto * broker) + { nodeNames.insert(nodeNames.end(), + broker->getNodeNames().begin(), + broker->getNodeNames().end()); }); + + std::vector buses(brokers.size()); + std::vector nodes(nodeNames.size()); + + bool hasBusDevices = false; + bool hasNodeDevices = false; + + for (int i = 0; i < drivers.size(); i++) + { + const auto * driver = drivers[i]; + + if (auto bus = std::find_if(brokers.begin(), brokers.end(), [driver](const auto * broker) + { return broker->getBusName() == driver->key; }); + bus != brokers.end()) + { + int index = std::distance(brokers.begin(), bus); + + if (buses[index] != nullptr) + { + yCError(CBB) << "Bus device" << driver->key << "already attached"; + return false; + } + + buses[index] = driver; + hasBusDevices = true; + } + else if (auto node = std::find_if(nodeNames.begin(), nodeNames.end(), [driver](const auto & name) + { return name == driver->key; }); + node != nodeNames.end()) + { + int index = std::distance(nodeNames.begin(), node); + + if (nodes[index] != nullptr) + { + yCError(CBB) << "Node device" << driver->key << "already attached"; + return false; + } + + nodes[index] = driver; + hasNodeDevices = true; + } + else + { + yCError(CBB) << "Unknown device:" << driver->key; + return false; + } + } + + if (hasBusDevices && std::any_of(buses.begin(), buses.end(), [](const auto * bus) { return bus == nullptr; })) + { + std::vector names; + + for (int i = 0; i < buses.size(); i++) + { + if (buses[i] == nullptr) + { + names.push_back(brokers[i]->getBusName()); + } + } + + yCError(CBB) << "Some bus devices are missing:" << names; + return false; + } + + if (hasNodeDevices && std::any_of(nodes.begin(), nodes.end(), [](const auto * node) { return node == nullptr; })) + { + std::vector names; + + for (int i = 0; i < nodes.size(); i++) + { + if (nodes[i] == nullptr) + { + names.push_back(nodeNames[i]); + } + } + + yCError(CBB) << "Some node devices are missing:" << names; + return false; + } + + if (hasNodeDevices && std::any_of(brokers.begin(), brokers.end(), [](const auto * broker) { return !broker->isRegistered(); })) + { + yCError(CBB) << "Bus devices need to be attached prior to node devices"; + return false; + } + + for (int i = 0; i < buses.size(); i++) + { + if (!brokers[i]->registerDevice(buses[i]->poly)) + { + yCError(CBB) << "Unable to register bus device" << buses[i]->key; + return false; + } + + if (!brokers[i]->startThreads()) + { + yCError(CBB) << "Unable to start CAN threads in" << brokers[i]->getBusName(); + return false; + } + } + + for (int i = 0; i < nodes.size(); i++) + { + const auto * node = nodes[i]; + + if (!deviceMapper.registerDevice(node->poly)) + { + yCError(CBB) << "Unable to register node device" << node->key; + return false; + } + + ICanBusSharer * iCanBusSharer; + + if (!node->poly->view(iCanBusSharer)) + { + yCError(CBB) << "Unable to view ICanBusSharer in" << node->key; + return false; + } + + auto it = std::find_if(brokers.begin(), brokers.end(), [node](const auto * broker) + { const auto & names = broker->getNodeNames(); + return std::find(names.begin(), names.end(), node->key) != names.end(); }); + + auto * broker = *it; + + broker->getReader()->registerHandle(iCanBusSharer); + iCanBusSharer->registerSender(broker->getWriter()->getDelegate()); + } + + for (const auto & rawDevice : deviceMapper.getDevices()) + { + auto * iCanBusSharer = rawDevice->castToType(); + + if (iCanBusSharer && !iCanBusSharer->initialize()) + { + yCError(CBB) << "Node device id" << iCanBusSharer->getId() << "could not initialize CAN comms"; + } + } + + return !syncThread || syncThread->start(); +} + +// ----------------------------------------------------------------------------- + +bool CanBusBroker::detachAll() +{ + bool ok = true; + + if (syncThread && syncThread->isRunning()) + { + syncThread->stop(); + } + + delete syncThread; + syncThread = nullptr; + + for (const auto & rawDevice : deviceMapper.getDevices()) + { + auto * iCanBusSharer = rawDevice->castToType(); + + if (iCanBusSharer && !iCanBusSharer->finalize()) + { + yCWarning(CBB) << "Node device id" << iCanBusSharer->getId() << "could not finalize CAN comms"; + ok = false; + } + } + + deviceMapper.clear(); + + for (auto * broker : brokers) + { + ok &= broker->stopThreads(); + ok &= broker->clearFilters(); + } + + return ok; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CanBusBroker/SingleBusBroker.cpp b/libraries/YarpPlugins/CanBusBroker/SingleBusBroker.cpp index 84701747e..1b7b269b1 100644 --- a/libraries/YarpPlugins/CanBusBroker/SingleBusBroker.cpp +++ b/libraries/YarpPlugins/CanBusBroker/SingleBusBroker.cpp @@ -13,17 +13,6 @@ using namespace roboticslab; // ----------------------------------------------------------------------------- -SingleBusBroker::SingleBusBroker(const std::string & _name) - : name(_name), - readerThread(nullptr), - writerThread(nullptr), - iCanBus(nullptr), - iCanBufferFactory(nullptr), - busLoadMonitor(nullptr) -{ } - -// ----------------------------------------------------------------------------- - SingleBusBroker::~SingleBusBroker() { stopThreads(); @@ -58,8 +47,8 @@ bool SingleBusBroker::configure(const yarp::os::Searchable & config) return false; } - readerThread = new CanReaderThread(name, rxDelay, rxBufferSize); - writerThread = new CanWriterThread(name, txDelay, txBufferSize); + readerThread = new CanReaderThread(busName, rxDelay, rxBufferSize); + writerThread = new CanWriterThread(busName, txDelay, txBufferSize); if (config.check("name", "YARP port prefix for remote CAN interface")) { @@ -121,6 +110,7 @@ bool SingleBusBroker::registerDevice(yarp::dev::PolyDriver * driver) writerThread->setCanHandles(iCanBus, iCanBufferFactory); } + registered = true; return true; } @@ -205,7 +195,7 @@ bool SingleBusBroker::clearFilters() // Clear CAN acceptance filters ('0' = all IDs that were previously set by canIdAdd). if (!iCanBus->canIdDelete(0)) { - yCWarning(CBB) << "CAN filters on bus" << name << "may be preserved on the next run"; + yCWarning(CBB) << "CAN filters on bus" << busName << "may be preserved on the next run"; return false; } diff --git a/libraries/YarpPlugins/CanBusBroker/SingleBusBroker.hpp b/libraries/YarpPlugins/CanBusBroker/SingleBusBroker.hpp index 1786d797d..c63d99d85 100644 --- a/libraries/YarpPlugins/CanBusBroker/SingleBusBroker.hpp +++ b/libraries/YarpPlugins/CanBusBroker/SingleBusBroker.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include @@ -39,8 +40,10 @@ namespace roboticslab class SingleBusBroker final : public yarp::os::TypedReaderCallback { public: - //! Constructor, passes string identifier of the CAN bus. - SingleBusBroker(const std::string & name); + //! Constructor, accepts the string identifiers of the CAN bus and its nodes. + SingleBusBroker(const std::string & _busName, const std::vector & _nodeNames) + : busName(_busName), nodeNames(_nodeNames) + { } //! Destructor. ~SingleBusBroker() override; @@ -60,17 +63,25 @@ class SingleBusBroker final : public yarp::os::TypedReaderCallback & getNodeNames() const + { return nodeNames; } //! Callback on incoming remote CAN commands. void onRead(yarp::os::Bottle & b) override; @@ -79,13 +90,14 @@ class SingleBusBroker final : public yarp::os::TypedReaderCallback nodeNames; - CanReaderThread * readerThread; - CanWriterThread * writerThread; + CanReaderThread * readerThread {nullptr}; + CanWriterThread * writerThread {nullptr}; - yarp::dev::ICanBus * iCanBus; - yarp::dev::ICanBufferFactory * iCanBufferFactory; + yarp::dev::ICanBus * iCanBus {nullptr}; + yarp::dev::ICanBufferFactory * iCanBufferFactory {nullptr}; yarp::os::Port dumpPort; yarp::os::PortWriterBuffer dumpWriter; @@ -98,7 +110,9 @@ class SingleBusBroker final : public yarp::os::TypedReaderCallback