Skip to content

Commit 1151dcb

Browse files
GiulioRomualdirandaz81
GiulioRomualdi
authored and
randaz81
committed
Implement MultipleAnalogSensorsClientRemapper device
1 parent a454a3b commit 1151dcb

File tree

3 files changed

+283
-0
lines changed

3 files changed

+283
-0
lines changed

‎src/devices/multipleanalogsensorsremapper/CMakeLists.txt‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,13 +8,22 @@ yarp_prepare_plugin(multipleanalogsensorsremapper
88
DEFAULT ON
99
)
1010

11+
yarp_prepare_plugin(multipleanalogsensorsclientremapper
12+
CATEGORY device
13+
TYPE MultipleAnalogSensorsClientRemapper
14+
INCLUDE MultipleAnalogSensorsClientRemapper.h
15+
DEFAULT ON
16+
)
17+
1118
if(ENABLE_multipleanalogsensorsremapper)
1219
yarp_add_plugin(yarp_multipleanalogsensorsremapper)
1320

1421
target_sources(yarp_multipleanalogsensorsremapper
1522
PRIVATE
1623
MultipleAnalogSensorsRemapper.cpp
1724
MultipleAnalogSensorsRemapper.h
25+
MultipleAnalogSensorsClientRemapper.h
26+
MultipleAnalogSensorsClientRemapper.cpp
1827
)
1928

2029
target_sources(yarp_multipleanalogsensorsremapper PRIVATE $<TARGET_OBJECTS:multipleAnalogSensorsSerializations>)

‎src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsClientRemapper.cpp‎

Lines changed: 155 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
1+
/*
2+
* SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
*/
5+
6+
#include "MultipleAnalogSensorsClientRemapper.h"
7+
8+
#include <yarp/os/Log.h>
9+
#include <yarp/os/LogComponent.h>
10+
#include <yarp/os/LogStream.h>
11+
12+
#include <algorithm>
13+
#include <iostream>
14+
#include <map>
15+
#include <cassert>
16+
17+
using namespace yarp::os;
18+
using namespace yarp::dev;
19+
using namespace yarp::sig;
20+
21+
namespace {
22+
YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSCLIENTREMAPPER, "yarp.device.multipleanalogsensorsclientremapper")
23+
}
24+
25+
26+
void MultipleAnalogSensorsClientRemapper::closeAllMultipleAnalogSensorsClients()
27+
{
28+
for(auto& device : m_multipleAnalogSensorsClientsDevices)
29+
{
30+
if( device != nullptr)
31+
{
32+
device->close();
33+
delete device;
34+
device = nullptr;
35+
}
36+
}
37+
38+
m_multipleAnalogSensorsClientsDevices.resize(0);
39+
}
40+
41+
42+
bool MultipleAnalogSensorsClientRemapper::close()
43+
{
44+
bool ret = true;
45+
46+
bool ok = MultipleAnalogSensorsRemapper::detachAll();
47+
48+
ret = ret && ok;
49+
50+
ok = MultipleAnalogSensorsRemapper::close();
51+
52+
ret = ret && ok;
53+
54+
this->closeAllMultipleAnalogSensorsClients();
55+
56+
return ret;
57+
}
58+
59+
bool MultipleAnalogSensorsClientRemapper::open(Searchable& config)
60+
{
61+
Property prop;
62+
prop.fromString(config.toString());
63+
64+
std::string localPortPrefix;
65+
std::vector<std::string> multipleAnalogSensorsClientsPorts;
66+
67+
// Check if the required parameters are found
68+
if( prop.check("localPortPrefix") && prop.find("localPortPrefix").isString() )
69+
{
70+
localPortPrefix = prop.find("localPortPrefix").asString();
71+
}
72+
else
73+
{
74+
yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Parsing parameters: \"localPortPrefix\" should be a string.";
75+
return false;
76+
}
77+
78+
Bottle *multipleAnalogSensorsClients=prop.find("multipleAnalogSensorsClients").asList();
79+
if(multipleAnalogSensorsClients==nullptr)
80+
{
81+
yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Parsing parameters: \"multipleAnalogSensorsClients\" should be followed by a list.";
82+
return false;
83+
}
84+
85+
multipleAnalogSensorsClientsPorts.resize(multipleAnalogSensorsClients->size());
86+
for(size_t i=0; i < multipleAnalogSensorsClients->size(); i++)
87+
{
88+
multipleAnalogSensorsClientsPorts[i] = multipleAnalogSensorsClients->get(i).asString();
89+
}
90+
91+
// Load the MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS, containing any additional option to pass to the multipleanalogsensorsclient
92+
Property multipleAnalogSensorsClientsOptions;
93+
94+
Bottle & optionsGroupBot = prop.findGroup("MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS");
95+
if( !(optionsGroupBot.isNull()) )
96+
{
97+
multipleAnalogSensorsClientsOptions.fromString(optionsGroupBot.toString());
98+
}
99+
100+
// Parameters loaded, open all the multipleanalogsensorsclient
101+
102+
m_multipleAnalogSensorsClientsDevices.resize(multipleAnalogSensorsClientsPorts.size(), nullptr);
103+
104+
PolyDriverList multipleAnalogSensorsClientsList;
105+
106+
for(size_t client=0; client < multipleAnalogSensorsClientsPorts.size(); client++ )
107+
{
108+
std::string remote = multipleAnalogSensorsClientsPorts[client];
109+
std::string local = localPortPrefix+remote;
110+
111+
Property options = multipleAnalogSensorsClientsOptions;
112+
options.put("device", "multipleanalogsensorsclient");
113+
options.put("local", local);
114+
options.put("remote", remote);
115+
116+
m_multipleAnalogSensorsClientsDevices[client] = new PolyDriver();
117+
118+
bool ok = m_multipleAnalogSensorsClientsDevices[client]->open(options);
119+
120+
if( !ok || !(m_multipleAnalogSensorsClientsDevices[client]->isValid()) )
121+
{
122+
yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Opening multipleanalogsensorsclient with remote \"" << remote << "\", opening the device failed.";
123+
this->closeAllMultipleAnalogSensorsClients();
124+
return false;
125+
}
126+
127+
// We use the remote name of the multipleanalogsensorsclient as the key for it, in absence of anything better
128+
multipleAnalogSensorsClientsList.push((m_multipleAnalogSensorsClientsDevices[client]),
129+
remote.c_str());
130+
}
131+
132+
// Device opened, now we open the MultipleAnalogSensorsRemapper and then we call attachAll
133+
bool ok = MultipleAnalogSensorsRemapper::open(prop);
134+
135+
if( !ok )
136+
{
137+
yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Opening the multipleanalogsensorsremapper device, opening the device failed.";
138+
MultipleAnalogSensorsRemapper::close();
139+
this->closeAllMultipleAnalogSensorsClients();
140+
return false;
141+
}
142+
143+
// If open went ok, we now call attachAll
144+
ok = MultipleAnalogSensorsRemapper::attachAll(multipleAnalogSensorsClientsList);
145+
146+
if( !ok )
147+
{
148+
yCError(MULTIPLEANALOGSENSORSCLIENTREMAPPER) << "Calling attachAll in the multipleanalogsensorsclient device, opening the device failed.";
149+
MultipleAnalogSensorsRemapper::close();
150+
this->closeAllMultipleAnalogSensorsClients();
151+
return false;
152+
}
153+
154+
return true;
155+
}

‎src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsClientRemapper.h‎

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
/*
2+
* SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
*/
5+
6+
#ifndef YARP_DEV_MULTIPLEANALOGSENSORSREMAPPER_MULTIPLEANALOGSENSORSCLIENTREMAPPER_H
7+
#define YARP_DEV_MULTIPLEANALOGSENSORSREMAPPER_MULTIPLEANALOGSENSORSCLIENTREMAPPER_H
8+
9+
#include <yarp/dev/PolyDriver.h>
10+
11+
#include "MultipleAnalogSensorsRemapper.h"
12+
13+
/**
14+
* @ingroup dev_impl_network_clients
15+
*
16+
* @brief `MultipleAnalogSensorsClientRemapper` A device that takes a list of sensor, a list of all
17+
* multiple analog sensors client and expose them as a single device exposing MultipleAnalogSensors
18+
* interface.
19+
*
20+
* \section MultipleAnalogSensorsRemapper
21+
*
22+
* Parameters required by this device are:
23+
* | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes |
24+
* |:--------------:|:--------------:|:-------:|:--------------:|:-------------:|:-----------: |:-----------------------------------------------------------------:|:-----:|
25+
* | {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 | |
26+
* | 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. |
27+
* | localPortPrefix | - | string | - | - | Yes | All ports opened by this device will start with this prefix | |
28+
* | MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS | - | group | - | - | No | Options that will be passed directly to the MultipleAnalogSensorsClient devices | |
29+
* The sensorTag is a tag identifing the spefici sensor interface, see \ref dev_iface_multiple_analog for a list of possible sensors.
30+
* The tag of each sensor interface is provided in the doxygen documentation of the specific interface.
31+
*
32+
*
33+
* Configuration file using .ini format.
34+
*
35+
* \code{.unparsed}
36+
* device multipleanalogsensorsclientremapper
37+
* ThreeAxisGyroscopesNames (l_foot_ft_gyro, r_arm_ft_gyro)
38+
* SixAxisForceTorqueSensorsNames (r_foot_ft, r_arm_ft)
39+
* multipleAnalogSensorsClients (/icub/left_foot/imu /icub/right_arm/imu)
40+
*
41+
* [MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS]
42+
* carrier udp
43+
* timeout 0.2
44+
* ...
45+
* \endcode
46+
*
47+
* Configuration of the device from C++ code.
48+
* \code{.cpp}
49+
* Property options;
50+
* options.put("device","multipleanalogsensorsclientremapper");
51+
* Bottle threeAxisGyroscopesNames;
52+
* Bottle & threeAxisGyroscopesList = threeAxisGyroscopesNames.addList();
53+
* threeAxisGyroscopesList.addString("l_foot_ft_gyro");
54+
* threeAxisGyroscopesList.addString("r_arm_ft_gyro");
55+
* options.put("ThreeAxisGyroscopesNames",threeAxisGyroscopesNames.get(0))
56+
*
57+
* Bottle sixAxisForceTorqueSensorsNames;
58+
* Bottle & sixAxisForceTorqueSensorsList = sixAxisForceTorqueSensorsNames.addList();
59+
* sixAxisForceTorqueSensorsList.addString("l_foot_ft_gyro");
60+
* sixAxisForceTorqueSensorsList.addString("r_arm_ft_gyro");
61+
* options.put("SixAxisForceTorqueSensorsNames",sixAxisForceTorqueSensorsNames.get(0))
62+
*
63+
* Bottle multipleAnalogSensorsClients;
64+
* Bottle & multipleAnalogSensorsClientsList = multipleAnalogSensorsClients.addList();
65+
* multipleAnalogSensorsClientsList.addString("/icub/left_foot/imu");
66+
* multipleAnalogSensorsClientsList.addString("/icub/right_arm/imu");
67+
* options.put("multipleAnalogSensorsClients",multipleAnalogSensorsClients.get(0));
68+
*
69+
* options.put("localPortPrefix",/test");
70+
*
71+
* Property & multipleAnalogSensorsClientsOpts = options.addGroup("MULTIPLE_ANALOG_SENSORS_CLIENTS_OPTIONS");
72+
* multipleAnalogSensorsClientsOpts.put("carrier", "udp");
73+
* multipleAnalogSensorsClientsOpts.put("timeout", 0.2);
74+
*
75+
* // Actually open the device
76+
* PolyDriver robotDevice(options);
77+
*
78+
* // Use it as you would use any controlboard device
79+
* // ...
80+
* \endcode
81+
*
82+
*/
83+
84+
class MultipleAnalogSensorsClientRemapper : public MultipleAnalogSensorsRemapper
85+
{
86+
private:
87+
/**
88+
* List of MultipleAnalogSensorsClient devices opened by the MultipleAnalogSensorsClientRemapper
89+
* device.
90+
*/
91+
std::vector<yarp::dev::PolyDriver*> m_multipleAnalogSensorsClientsDevices;
92+
93+
94+
// Close all opened MultipleAnalogSensorsClients
95+
void closeAllMultipleAnalogSensorsClients();
96+
97+
public:
98+
MultipleAnalogSensorsClientRemapper() = default;
99+
MultipleAnalogSensorsClientRemapper(const MultipleAnalogSensorsClientRemapper&) = delete;
100+
MultipleAnalogSensorsClientRemapper(MultipleAnalogSensorsClientRemapper&&) = delete;
101+
MultipleAnalogSensorsClientRemapper& operator=(const MultipleAnalogSensorsClientRemapper&) = delete;
102+
MultipleAnalogSensorsClientRemapper& operator=(MultipleAnalogSensorsClientRemapper&&) = delete;
103+
~MultipleAnalogSensorsClientRemapper() = default;
104+
105+
/**
106+
* Open the device driver.
107+
* @param prop is a Searchable object which contains the parameters.
108+
* Allowed parameters are described in the class documentation.
109+
*/
110+
bool open(yarp::os::Searchable &prop) override;
111+
112+
/**
113+
* Close the device driver by deallocating all resources and closing ports.
114+
* @return true if successful or false otherwise.
115+
*/
116+
bool close() override;
117+
};
118+
119+
#endif

0 commit comments

Comments
 (0)