diff --git a/libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp b/libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp index 574f13976..1cc3ec531 100644 --- a/libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp +++ b/libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "DeviceMapper.hpp" #include "SingleBusBroker.hpp" @@ -417,6 +418,8 @@ class CanBusBroker : public yarp::dev::DeviceDriver, private: DeviceMapper deviceMapper; std::vector brokers; + yarp::dev::PolyDriverList fakeBuses; + yarp::dev::PolyDriverList fakeNodes; SyncPeriodicThread * syncThread {nullptr}; }; diff --git a/libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp index 3797d0784..6c2f0888b 100644 --- a/libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp @@ -19,7 +19,7 @@ bool CanBusBroker::open(yarp::os::Searchable & config) if (!buses) { - yCError(CBB) << "Missing key \"buses\" or not a list"; + yCError(CBB) << R"(Missing key "buses" or not a list)"; return false; } @@ -59,6 +59,38 @@ bool CanBusBroker::open(yarp::os::Searchable & config) } } + if (yarp::os::Value * v; config.check("fakeBuses", v, "fake CAN buses")) + { + if (!v->isList()) + { + yCError(CBB) << R"(Key "fakeBuses" must be a list)"; + return false; + } + + const auto * buses = v->asList(); + + for (int i = 0; i < buses->size(); i++) + { + fakeBuses.push(nullptr, buses->get(i).asString().c_str()); + } + } + + if (yarp::os::Value * v; config.check("fakeNodes", v, "fake CAN nodes")) + { + if (!v->isList()) + { + yCError(CBB) << R"(Key "fakeNodes" must be a list)"; + return false; + } + + const auto * nodes = v->asList(); + + for (int i = 0; i < nodes->size(); i++) + { + fakeNodes.push(nullptr, nodes->get(i).asString().c_str()); + } + } + if (config.check("syncPeriod", "SYNC message period (s)")) { auto syncPeriod = config.find("syncPeriod").asFloat64(); diff --git a/libraries/YarpPlugins/CanBusBroker/IMultipleWrapperImpl.cpp b/libraries/YarpPlugins/CanBusBroker/IMultipleWrapperImpl.cpp index aede99b1f..9f1b4885c 100644 --- a/libraries/YarpPlugins/CanBusBroker/IMultipleWrapperImpl.cpp +++ b/libraries/YarpPlugins/CanBusBroker/IMultipleWrapperImpl.cpp @@ -197,6 +197,20 @@ bool CanBusBroker::detachAll() ok &= broker->clearFilters(); } + for (int i = 0; i < fakeBuses.size(); i++) + { + ok &= fakeBuses[i]->poly->close(); + delete fakeBuses[i]->poly; + fakeBuses[i]->poly = nullptr; + } + + for (int i = 0; i < fakeNodes.size(); i++) + { + ok &= fakeNodes[i]->poly->close(); + delete fakeNodes[i]->poly; + fakeNodes[i]->poly = nullptr; + } + return ok; }