diff --git a/src/devices/multipleanalogsensorsremapper/CMakeLists.txt b/src/devices/multipleanalogsensorsremapper/CMakeLists.txt index baa8bd6be6..e6e6087dc1 100644 --- a/src/devices/multipleanalogsensorsremapper/CMakeLists.txt +++ b/src/devices/multipleanalogsensorsremapper/CMakeLists.txt @@ -8,6 +8,13 @@ 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) @@ -15,6 +22,8 @@ if(ENABLE_multipleanalogsensorsremapper) PRIVATE MultipleAnalogSensorsRemapper.cpp MultipleAnalogSensorsRemapper.h + MultipleAnalogSensorsClientRemapper.h + MultipleAnalogSensorsClientRemapper.cpp ) target_sources(yarp_multipleanalogsensorsremapper PRIVATE $) diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsClientRemapper.cpp b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsClientRemapper.cpp new file mode 100644 index 0000000000..a3fb0b00c3 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include + +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 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; +} diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsClientRemapper.h b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsClientRemapper.h new file mode 100644 index 0000000000..60ffafcd83 --- /dev/null +++ b/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 + +#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 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