Add the possibility to attach the polydriver list in BipedalLocomotio…

…n::RobotInterface::constructMultipleAnalogSensorsRemapper

src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpHelper.h

@@ -16,6 +16,7 @@
 #include <yarp/dev/PolyDriver.h>
 
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <yarp/dev/PolyDriverList.h>
 
 namespace BipedalLocomotion
 {
@@ -117,6 +118,46 @@ PolyDriverDescriptor constructMultipleAnalogSensorsClient(
 PolyDriverDescriptor constructMultipleAnalogSensorsRemapper(
  std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
 
+/**
+ * Helper function that can be used to build a `MultipleAnalogSensorsRemapper` device.
+ * @param handler pointer to a parameter handler interface.
+ * @param polydriverList a list of the polydriver that will be attached to the multiple analog sensor remapper
+ * @note The following parameters are taken into consideration
+ * | Parameter Name | Type | Description | Mandatory |
+ * |:---------------------------------------:|:----------------:|:---------------------------------------------------------------------------------------:|:---------:|
+ * | `three_axis_gyroscopes_names` | `vector<string>` | Vector containing the names of the gyroscopes (Default empty vector) | No |
+ * | `three_axis_linear_accelerometers_names`| `vector<string>` | Vector containing the names of the accelerometers (Default empty vector) | No |
+ * | `three_axis_magnetometers_names` | `vector<string>` | Vector containing the names of the magnetometers (Default empty vector) | No |
+ * | `orientation_sensors_names` | `vector<string>` | Vector containing the names of the orientation sensors (Default empty vector) | No |
+ * | `six_axis_force_torque_sensors_names` | `vector<string>` | Vector containing the names of the FT sensors (Default empty vector) | No |
+ * | `temperature_sensors_names` | `vector<string>` | Vector containing the names of the temperature sensors (Default empty vector) | No |
+ * @note The `MultipleAnalogSensorsRemapper` device is implement in [yarp](https://www.yarp.it/git-master/classMultipleAnalogSensorsRemapper.html).
+ * @return A PolyDriverDescriptor. In case of error an invalid `PolyDriverDescriptor` is returned.
+ */
+PolyDriverDescriptor constructMultipleAnalogSensorsRemapper(
+ std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
+ const std::vector<PolyDriverDescriptor>& polydriverList);
+
+/**
+ * Helper function that can be used to build a `MultipleAnalogSensorsRemapper` device.
+ * @param handler pointer to a parameter handler interface.
+ * @param polydriverList a list of the polydriver that will be attached to the multiple analog sensor remapper.
+ * @note The following parameters are taken into consideration
+ * | Parameter Name | Type | Description | Mandatory |
+ * |:---------------------------------------:|:----------------:|:---------------------------------------------------------------------------------------:|:---------:|
+ * | `three_axis_gyroscopes_names` | `vector<string>` | Vector containing the names of the gyroscopes (Default empty vector) | No |
+ * | `three_axis_linear_accelerometers_names`| `vector<string>` | Vector containing the names of the accelerometers (Default empty vector) | No |
+ * | `three_axis_magnetometers_names` | `vector<string>` | Vector containing the names of the magnetometers (Default empty vector) | No |
+ * | `orientation_sensors_names` | `vector<string>` | Vector containing the names of the orientation sensors (Default empty vector) | No |
+ * | `six_axis_force_torque_sensors_names` | `vector<string>` | Vector containing the names of the FT sensors (Default empty vector) | No |
+ * | `temperature_sensors_names` | `vector<string>` | Vector containing the names of the temperature sensors (Default empty vector) | No |
+ * @note The `MultipleAnalogSensorsRemapper` device is implement in [yarp](https://www.yarp.it/git-master/classMultipleAnalogSensorsRemapper.html).
+ * @return A PolyDriverDescriptor. In case of error an invalid `PolyDriverDescriptor` is returned.
+ */
+PolyDriverDescriptor constructMultipleAnalogSensorsRemapper(
+ std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
+ const yarp::dev::PolyDriverList& polydriverList);
+
 } // namespace RobotInterface
 } // namespace BipedalLocomotion
 

src/RobotInterface/YarpImplementation/src/YarpHelper.cpp

@@ -8,6 +8,9 @@
 #include <BipedalLocomotion/RobotInterface/YarpHelper.h>
 #include <BipedalLocomotion/TextLogging/Logger.h>
 
+#include <yarp/dev/IMultipleWrapper.h>
+#include <yarp/dev/PolyDriverList.h>
+
 using namespace BipedalLocomotion::RobotInterface;
 
 PolyDriverDescriptor::PolyDriverDescriptor(const std::string& key,
@@ -265,3 +268,47 @@ PolyDriverDescriptor BipedalLocomotion::RobotInterface::constructMultipleAnalogS
 
  return device;
 }
+
+PolyDriverDescriptor BipedalLocomotion::RobotInterface::constructMultipleAnalogSensorsRemapper(
+ std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
+ const std::vector<PolyDriverDescriptor>& polydriverList)
+{
+ // create the yarp::dev::PolyDriverList
+ yarp::dev::PolyDriverList list;
+ for (const auto& driver : polydriverList)
+ {
+ list.push(driver.poly.get(), driver.key.c_str());
+ }
+
+ return constructMultipleAnalogSensorsRemapper(handler, list);
+}
+
+PolyDriverDescriptor BipedalLocomotion::RobotInterface::constructMultipleAnalogSensorsRemapper(
+ std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
+ const yarp::dev::PolyDriverList& polydriverList)
+{
+ constexpr auto errorPrefix = "[RobotInterface::constructMultipleAnalogsensorsRemapper]";
+
+ auto device = constructMultipleAnalogSensorsRemapper(handler);
+
+ if (!device.isValid())
+ {
+ return device;
+ }
+
+ // attach the the interface
+ yarp::dev::IMultipleWrapper* multipleWrapper = nullptr;
+ if (!device.poly->view(multipleWrapper) || multipleWrapper == nullptr)
+ {
+ log()->error("{} Could not view the IMultipleWrapper interface.", errorPrefix);
+ return device;
+ }
+
+ if (!multipleWrapper->attachAll(polydriverList))
+ {
+ log()->error("{} Could not attach the polydriver list.", errorPrefix);
+ return PolyDriverDescriptor();
+ }
+
+ return device;
+}