Implement MultipleAnalogSensorsClientRemapper device

src/devices/multipleanalogsensorsremapper/CMakeLists.txt

@@ -8,13 +8,22 @@ yarp_prepare_plugin(multipleanalogsensorsremapper
   DEFAULT ON
 )
 
+yarp_prepare_plugin(multipleanalogsensorsclientremapper
+  CATEGORY device
+  TYPE MultipleAnalogSensorsClientRemapper
+  INCLUDE MultipleAnalogSensorsClientRemapper.h
+  DEFAULT ON
+)
+
 if(ENABLE_multipleanalogsensorsremapper)
   yarp_add_plugin(yarp_multipleanalogsensorsremapper)
 
   target_sources(yarp_multipleanalogsensorsremapper
     PRIVATE
       MultipleAnalogSensorsRemapper.cpp
       MultipleAnalogSensorsRemapper.h
+      MultipleAnalogSensorsClientRemapper.h
+      MultipleAnalogSensorsClientRemapper.cpp
   )
 
   target_sources(yarp_multipleanalogsensorsremapper PRIVATE $<TARGET_OBJECTS:multipleAnalogSensorsSerializations>)

src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsClientRemapper.cpp

@@ -0,0 +1,155 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "MultipleAnalogSensorsClientRemapper.h"
+
+#include <yarp/os/Log.h>
+#include <yarp/os/LogComponent.h>
+#include <yarp/os/LogStream.h>
+
+#include <algorithm>
+#include <iostream>
+#include <map>
+#include <cassert>
+
+using namespace yarp::os;
+using namespace yarp::dev;
+using namespace yarp::sig;
+
+namespace {
+YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSCLIENTREMAPPER, "yarp.device.multipleanalogsensorsclientremapper")
+}
+
+
+void MultipleAnalogSensorsClientRemapper::closeAllMultipleAnalogSensorsClients()
+{
+    for(auto& device : m_multipleAnalogSensorsClientsDevices)
+    {
+        if( device != nullptr)
+        {
+            device->close();
+            delete device;
+            device = nullptr;
+        }
+    }
+
+    m_multipleAnalogSensorsClientsDevices.resize(0);
+}
+
+
+bool MultipleAnalogSensorsClientRemapper::close()
+{
+    bool ret = true;
+
+    bool ok = MultipleAnalogSensorsRemapper::detachAll();
+
+    ret = ret && ok;
+
+    ok = MultipleAnalogSensorsRemapper::close();
+
+    ret = ret && ok;
+
+    this->closeAllMultipleAnalogSensorsClients();
+
+    return ret;
+}
+
+bool MultipleAnalogSensorsClientRemapper::open(Searchable& config)
+{
+    Property prop;
+    prop.fromString(config.toString());
+
+    std::string localPortPrefix;
+    std::vector<std::string> multipleAnalogSensorsClientsPorts;
+
+    // Check if the required parameters  are found
+    if( prop.check("localPortPrefix") && prop.find("localPortPrefix").isString() )
+    {
+        localPortPrefix = prop.find("localPortPrefix").asString();
+    }
+    else
+    {
+        yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Parsing parameters: \"localPortPrefix\" should be a string.";
+        return false;
+    }
+
+    Bottle *multipleAnalogSensorsClients=prop.find("multipleAnalogSensorsClients").asList();
+    if(multipleAnalogSensorsClients==nullptr)
+    {
+        yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Parsing parameters: \"multipleAnalogSensorsClients\" should be followed by a list.";
+        return false;
+    }
+
+    multipleAnalogSensorsClientsPorts.resize(multipleAnalogSensorsClients->size());
+    for(size_t i=0; i < multipleAnalogSensorsClients->size(); i++)
+    {
+        multipleAnalogSensorsClientsPorts[i] = multipleAnalogSensorsClients->get(i).asString();
+    }
+
+    // Load the MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS, containing any additional option to pass to the multipleanalogsensorsclient
+    Property multipleAnalogSensorsClientsOptions;
+
+    Bottle & optionsGroupBot = prop.findGroup("MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS");
+    if( !(optionsGroupBot.isNull()) )
+    {
+        multipleAnalogSensorsClientsOptions.fromString(optionsGroupBot.toString());
+    }
+
+    // Parameters loaded, open all the multipleanalogsensorsclient
+
+    m_multipleAnalogSensorsClientsDevices.resize(multipleAnalogSensorsClientsPorts.size(), nullptr);
+
+    PolyDriverList multipleAnalogSensorsClientsList;
+
+    for(size_t client=0; client < multipleAnalogSensorsClientsPorts.size(); client++ )
+    {
+        std::string remote = multipleAnalogSensorsClientsPorts[client];
+        std::string local = localPortPrefix+remote;
+
+        Property options = multipleAnalogSensorsClientsOptions;
+        options.put("device", "multipleanalogsensorsclient");
+        options.put("local", local);
+        options.put("remote", remote);
+
+        m_multipleAnalogSensorsClientsDevices[client] = new PolyDriver();
+
+        bool ok = m_multipleAnalogSensorsClientsDevices[client]->open(options);
+
+        if( !ok || !(m_multipleAnalogSensorsClientsDevices[client]->isValid()) )
+        {
+            yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Opening multipleanalogsensorsclient with remote \"" << remote << "\", opening the device failed.";
+            this->closeAllMultipleAnalogSensorsClients();
+            return false;
+        }
+
+        // We use the remote name of the multipleanalogsensorsclient as the key for it, in absence of anything better
+        multipleAnalogSensorsClientsList.push((m_multipleAnalogSensorsClientsDevices[client]),
+                                              remote.c_str());
+    }
+
+    // Device opened, now we open the MultipleAnalogSensorsRemapper and then we call attachAll
+    bool ok = MultipleAnalogSensorsRemapper::open(prop);
+
+    if( !ok )
+    {
+        yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Opening the multipleanalogsensorsremapper device, opening the device failed.";
+        MultipleAnalogSensorsRemapper::close();
+        this->closeAllMultipleAnalogSensorsClients();
+        return false;
+    }
+
+    // If open went ok, we now call attachAll
+    ok = MultipleAnalogSensorsRemapper::attachAll(multipleAnalogSensorsClientsList);
+
+    if( !ok )
+    {
+        yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Calling attachAll in the multipleanalogsensorsclient device, opening the device failed.";
+        MultipleAnalogSensorsRemapper::close();
+        this->closeAllMultipleAnalogSensorsClients();
+        return false;
+    }
+
+    return true;
+}

src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsClientRemapper.h

@@ -0,0 +1,119 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef YARP_DEV_MULTIPLEANALOGSENSORSREMAPPER_MULTIPLEANALOGSENSORSCLIENTREMAPPER_H
+#define YARP_DEV_MULTIPLEANALOGSENSORSREMAPPER_MULTIPLEANALOGSENSORSCLIENTREMAPPER_H
+
+#include <yarp/dev/PolyDriver.h>
+
+#include "MultipleAnalogSensorsRemapper.h"
+
+/**
+ *  @ingroup dev_impl_network_clients
+ *
+ * @brief `MultipleAnalogSensorsClientRemapper` A device that takes a list of sensor, a list of all
+ * multiple analog sensors client and expose them as a single device exposing MultipleAnalogSensors
+ * interface.
+ *
+ * \section MultipleAnalogSensorsRemapper
+ *
+ *  Parameters required by this device are:
+ * | Parameter name | SubParameter   | Type    | Units          | Default Value | Required     | Description                                                       | Notes |
+ * |:--------------:|:--------------:|:-------:|:--------------:|:-------------:|:-----------: |:-----------------------------------------------------------------:|:-----:|
+ * | {sensorTag}Names |      -       | vector of strings  | -      |   -           | Yes     | Ordered list of name  that must belong of the remapped device. The list also defines the index that the sensor will  |  |
+ * | multipleAnalogSensorsClients |     -     | vector of strings  | -   |   -           | Yes          | List of remote prefix used by the MultipleAnalogSensorsClients.           | The element of this list are then passed as "remote" parameter to the MultipleAnalogSensorsClient device. |
+ * | localPortPrefix |     -         | string             | -   |   -           | Yes          | All ports opened by this device will start with this prefix       |       |
+ * | MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS | - | group              | -   |   -           | No           | Options that will be passed directly to the MultipleAnalogSensorsClient devices | |
+ * The sensorTag is a tag identifing the spefici sensor interface, see \ref dev_iface_multiple_analog for a list of possible sensors.
+ * The tag of each sensor interface is provided in the doxygen documentation of the specific interface.
+ *
+ *
+ * Configuration file using .ini format.
+ *
+ * \code{.unparsed}
+ *  device multipleanalogsensorsclientremapper
+ *  ThreeAxisGyroscopesNames (l_foot_ft_gyro,  r_arm_ft_gyro)
+ *  SixAxisForceTorqueSensorsNames (r_foot_ft,  r_arm_ft)
+ *  multipleAnalogSensorsClients (/icub/left_foot/imu /icub/right_arm/imu)
+ *
+ *  [MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS]
+ *  carrier udp
+ *  timeout 0.2
+ * ...
+ * \endcode
+ *
+ * Configuration of the device from C++ code.
+ * \code{.cpp}
+ *   Property options;
+ *   options.put("device","multipleanalogsensorsclientremapper");
+ *   Bottle threeAxisGyroscopesNames;
+ *   Bottle & threeAxisGyroscopesList = threeAxisGyroscopesNames.addList();
+ *   threeAxisGyroscopesList.addString("l_foot_ft_gyro");
+ *   threeAxisGyroscopesList.addString("r_arm_ft_gyro");
+ *   options.put("ThreeAxisGyroscopesNames",threeAxisGyroscopesNames.get(0))
+ *
+ *   Bottle sixAxisForceTorqueSensorsNames;
+ *   Bottle & sixAxisForceTorqueSensorsList = sixAxisForceTorqueSensorsNames.addList();
+ *   sixAxisForceTorqueSensorsList.addString("l_foot_ft_gyro");
+ *   sixAxisForceTorqueSensorsList.addString("r_arm_ft_gyro");
+ *   options.put("SixAxisForceTorqueSensorsNames",sixAxisForceTorqueSensorsNames.get(0))
+ *
+ *   Bottle multipleAnalogSensorsClients;
+ *   Bottle & multipleAnalogSensorsClientsList = multipleAnalogSensorsClients.addList();
+ *   multipleAnalogSensorsClientsList.addString("/icub/left_foot/imu");
+ *   multipleAnalogSensorsClientsList.addString("/icub/right_arm/imu");
+ *   options.put("multipleAnalogSensorsClients",multipleAnalogSensorsClients.get(0));
+ *
+ *   options.put("localPortPrefix",/test");
+ *
+ *   Property & multipleAnalogSensorsClientsOpts = options.addGroup("MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS");
+ *   multipleAnalogSensorsClientsOpts.put("carrier", "udp");
+ *   multipleAnalogSensorsClientsOpts.put("timeout", 0.2);
+ *
+ *   // Actually open the device
+ *   PolyDriver robotDevice(options);
+ *
+ *   // Use it as  you would use any controlboard device
+ *   // ...
+ * \endcode
+ *
+ */
+
+class MultipleAnalogSensorsClientRemapper : public MultipleAnalogSensorsRemapper
+{
+private:
+    /**
+     * List of MultipleAnalogSensorsClient devices opened by the MultipleAnalogSensorsClientRemapper
+     * device.
+     */
+    std::vector<yarp::dev::PolyDriver*> m_multipleAnalogSensorsClientsDevices;
+
+
+    // Close all opened  MultipleAnalogSensorsClients
+    void closeAllMultipleAnalogSensorsClients();
+
+public:
+    MultipleAnalogSensorsClientRemapper() = default;
+    MultipleAnalogSensorsClientRemapper(const MultipleAnalogSensorsClientRemapper&) = delete;
+    MultipleAnalogSensorsClientRemapper(MultipleAnalogSensorsClientRemapper&&) = delete;
+    MultipleAnalogSensorsClientRemapper& operator=(const MultipleAnalogSensorsClientRemapper&) = delete;
+    MultipleAnalogSensorsClientRemapper& operator=(MultipleAnalogSensorsClientRemapper&&) = delete;
+    ~MultipleAnalogSensorsClientRemapper() = default;
+
+   /**
+     * Open the device driver.
+     * @param prop is a Searchable object which contains the parameters.
+     * Allowed parameters are described in the class documentation.
+     */
+    bool open(yarp::os::Searchable &prop) override;
+
+    /**
+     * Close the device driver by deallocating all resources and closing ports.
+     * @return true if successful or false otherwise.
+     */
+    bool close() override;
+};
+
+#endif