[WIP]

roboticslab-uc3m · Jan 7, 2024 · 1a4f36a · 1a4f36a
1 parent 06d22f2
commit 1a4f36a
Show file tree

Hide file tree

Showing 6 changed files with 516 additions and 229 deletions.
diff --git 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
@@ -3,10 +3,15 @@
 #ifndef __CAN_BUS_BROKER_HPP__
 #define __CAN_BUS_BROKER_HPP__
 
+#include <map>
+#include <string>
+#include <tuple>
 #include <vector>
 
+#include <yarp/dev/IMultipleWrapper.h>
 #include <yarp/dev/ControlBoardInterfaces.h>
 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
+#include <yarp/dev/PolyDriverList.h>
 
 #include "DeviceMapper.hpp"
 #include "SingleBusBroker.hpp"
@@ -25,7 +30,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 +43,7 @@ namespace roboticslab
  * <a href="https://github.com/roboticslab-uc3m/yarp-devices/issues/241#issuecomment-569112698">instructions</a>.
  */
 class CanBusBroker : public yarp::dev::DeviceDriver,
+                     public yarp::dev::IMultipleWrapper,
                      // control board interfaces
                      public yarp::dev::IAmplifierControl,
                      public yarp::dev::IAxisInfo,
@@ -79,6 +85,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 ---------
@@ -408,10 +419,22 @@ class CanBusBroker : public yarp::dev::DeviceDriver,
     bool getThreeAxisMagnetometerMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
 
 private:
+    bool attachBusDevice(const yarp::dev::PolyDriverDescriptor * device, int index);
+    bool attachNodeDevice(const yarp::dev::PolyDriverDescriptor * device);
+
     DeviceMapper deviceMapper;
 
-    std::vector<yarp::dev::PolyDriver *> busDevices;
-    std::vector<yarp::dev::PolyDriver *> nodeDevices;
+    std::map<std::string, std::vector<std::string>> deviceKeys;
+
+    std::vector<std::string> busNames;
+    // std::map<std::string, std::pair<std::string, int>>
+    std::vector<std::tuple<std::string, std::string, int>> deviceKeys2;
+
+    yarp::dev::PolyDriverList busDevices;
+    yarp::dev::PolyDriverList nodeDevices;
+    // std::vector<yarp::dev::PolyDriver *> busDevices;
+    // std::vector<yarp::dev::PolyDriver *> nodeDevices;
+
     std::vector<SingleBusBroker *> brokers;
 
     SyncPeriodicThread * syncThread {nullptr};