YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
remoteControlBoardRemapper_t1_test.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#include <yarp/os/Time.h>
11
12#include <vector>
13
16#include <catch2/catch_amalgamated.hpp>
17#include <harness.h>
18
19using namespace yarp::os;
20using namespace yarp::dev;
21
22const char *fmcA_file_content = "device fakeMotionControl\n"
23 "[GENERAL]\n"
24 "Joints 2\n"
25 "\n"
26 "AxisName (\"axisA1\" \"axisA2\") \n";
27
28const char *fmcB_file_content = "device fakeMotionControl\n"
29 "[GENERAL]\n"
30 "Joints 3\n"
31 "\n"
32 "AxisName (\"axisB1\" \"axisB2\" \"axisB3\") \n";
33
34const char *fmcC_file_content = "device fakeMotionControl\n"
35 "[GENERAL]\n"
36 "Joints 4\n"
37 "\n"
38 "AxisName (\"axisC1\" \"axisC2\" \"axisC3\" \"axisC4\") \n";
39
40const char *fmcA_file_content_micro = "device fakeMotionControlMicro\n"
41 "[GENERAL]\n"
42 "Joints 2\n"
43 "\n"
44 "AxisName (\"axisA1\" \"axisA2\") \n";
45
46const char *fmcB_file_content_micro = "device fakeMotionControlMicro\n"
47 "[GENERAL]\n"
48 "Joints 3\n"
49 "\n"
50 "AxisName (\"axisB1\" \"axisB2\" \"axisB3\") \n";
51
52const char *fmcC_file_content_micro = "device fakeMotionControlMicro\n"
53 "[GENERAL]\n"
54 "Joints 4\n"
55 "\n"
56 "AxisName (\"axisC1\" \"axisC2\" \"axisC3\" \"axisC4\") \n";
57
58// NOTE: under valgrind the period of the controlBoard_nws_yarp is increased.
59// The state is streamed over tcp: if the receiver (the controlBoard_nwc_yarp)
60// cannot keep up with the sender (e.g. under valgrind, with three boards at 100Hz),
61// the messages pile up in the socket buffers and the data read by the client
62// becomes older and older (several seconds), even if it is still considered
63// valid because it has just arrived.
64// In local measurements under valgrind the receivers kept up with the three
65// boards up to ~30Hz each: 10Hz (0.1s) leaves a good margin for slower CI machines.
66#ifdef WITH_VALGRIND
67# define REMAPPER_TEST_NWS_PERIOD "0.1"
68#else
69# define REMAPPER_TEST_NWS_PERIOD "0.01"
70#endif
71
72const char *wrapperA_file_content = "device controlBoard_nws_yarp\n"
73 "name /testRemapperRobot/a\n"
74 "period " REMAPPER_TEST_NWS_PERIOD "\n";
75
76const char *wrapperB_file_content = "device controlBoard_nws_yarp\n"
77 "name /testRemapperRobot/b\n"
78 "period " REMAPPER_TEST_NWS_PERIOD "\n";
79
80const char *wrapperC_file_content = "device controlBoard_nws_yarp\n"
81 "name /testRemapperRobot/c\n"
82 "period " REMAPPER_TEST_NWS_PERIOD "\n";
83
84
86{
87 IPositionControl *pos = nullptr;
88 REQUIRE(ddRemapper.view(pos)); // interface position correctly opened
89 REQUIRE(pos);
90
91 size_t axes = 0;
92 CHECK(pos->getAxes(axes)); // getAxes returned correctly
93 CHECK((size_t) axes == nrOfRemappedAxes); // remapper seems functional
94
95 IPositionDirect *posdir = nullptr;
96 REQUIRE(ddRemapper.view(posdir)); // direct position interface correctly opened
98
99 IEncoders * encs = nullptr;
100 REQUIRE(ddRemapper.view(encs)); // encoders interface correctly opened
101 REQUIRE(encs);
102
103 IControlMode *ctrlmode = nullptr;
104 REQUIRE(ddRemapper.view(ctrlmode)); // control mode interface correctly opened
106
107 // Vector used for setting/getting data from the controlboard
108 std::vector<double> setPosition(nrOfRemappedAxes,-10),
109 setTrajSpeeds(nrOfRemappedAxes,-15),
112
113 for(size_t i=0; i < nrOfRemappedAxes; i++)
114 {
115 setPosition[i] = i*100.0+50.0 + rand;
116 setTrajSpeeds[i] = i*10.0+5 + rand;
117 readedPosition[i] = -100 + rand;
118 }
119
120 // Set the control mode in position direct
123
124 CHECK(ctrlmode->setControlModes(settedControlMode)); // setControlModes correctly called
126
127 // Check that the read control mode is actually position direct
128 // Let's try multiple times because if the remapper is using some controlBoard_nwc_yarp,
129 // it is possible that this return false if it is called before the first message
130 // has been received from the controlboard_nws_yarp
131 // The control modes are read from the streaming port of the controlBoard_nws_yarp,
132 // hence they may be not yet updated (e.g. when running under valgrind).
133 // Poll until the expected values are received (or a timeout expires).
134 bool getControlModesOk = false;
135 for(int wait=0; wait < 300; wait++)
136 {
137 getControlModesOk = ctrlmode->getControlModes(readedControlMode);
138 bool match = getControlModesOk;
139 for (size_t i = 0; match && i < nrOfRemappedAxes; i++) {
141 }
142 if (match) { break; }
144 }
145 CHECK(getControlModesOk); // getControlModes correctly called
146
147 for(size_t i=0; i < nrOfRemappedAxes; i++)
148 {
149 CHECK(settedControlMode[i] == (yarp::dev::SelectableControlModeEnum)readedControlMode[i]); // Setted control mode and readed control mode match
150 }
151
152 // Test position direct methods
153
154 // Set position
155 CHECK(posdir->setPositions(setPosition.data())); // setPositions correctly called
156
157 // Set also the speeds in the mean time, so we are sure that we don't get
158 // spurious successful set/get of position because of intermediate buffers
159 CHECK(pos->setTrajSpeeds(setTrajSpeeds.data())); // setTrajSpeeds correctly called
160
161 // Wait some time to make sure that the vector has been correctly propagated
162 // back and forth
164
165 // Read position
166 CHECK(posdir->getRefPositions(readedPosition.data())); // getRefPositions correctly called
167
168 // Check that the two vector match
169 for(size_t i=0; i < nrOfRemappedAxes; i++)
170 {
171 CHECK(setPosition[i] == readedPosition[i]); // Setted position and readed ref position match
172 }
173
174 // Do a similar test for the encoders
175 // in fakeMotionControl their value is the one setted with setPosition
176 // The encoders are read from the streaming port of the controlBoard_nws_yarp,
177 // hence they may be not yet updated (e.g. when running under valgrind).
178 // Poll until the expected values are received (or a timeout expires).
179 bool getEncodersOk = false;
180 for(int wait=0; wait < 300; wait++)
181 {
183 if (getEncodersOk && setPosition == readedEncoders) { break; }
185 }
186 CHECK(getEncodersOk); // getEncoders correctly called
187
188 // Check that the two vector match
189 for(size_t i=0; i < nrOfRemappedAxes; i++)
190 {
191 CHECK(setPosition[i] == readedEncoders[i]); // Setted position and readed encoders match
192 }
193}
194
195
197{
198 IPositionControl *pos = nullptr;
199 REQUIRE(ddRemapper.view(pos)); // interface position correctly opened
200 REQUIRE(pos);
201
202 size_t axes = 0;
203 CHECK(pos->getAxes(axes)); // getAxes returned correctly
204 CHECK((size_t) axes == nrOfRemappedAxes); // remapper seems functional
205
206 IPositionDirect *posdir = nullptr;
207 REQUIRE(ddRemapper.view(posdir)); // direct position interface correctly opened
209
210 IEncoders * encs = nullptr;
211 REQUIRE(ddRemapper.view(encs)); // encoders interface correctly opened
212 REQUIRE(encs);
213
214 IControlMode *ctrlmode = nullptr;
215 REQUIRE(ddRemapper.view(ctrlmode)); // control mode interface correctly opened
217
218 // Vector used for setting/getting data from the controlboard
219 std::vector<double> setPosition(nrOfRemappedAxes,-10),
220 setTrajSpeeds(nrOfRemappedAxes,-15),
223
224 for(size_t i=0; i < nrOfRemappedAxes; i++)
225 {
226 setPosition[i] = i*100.0+50.0 + rand;
227 setTrajSpeeds[i] = i*10.0+5 + rand;
228 readedPosition[i] = -100 + rand;
229 }
230
231 // Set the control mode in position direct
234
235 CHECK_FALSE(ctrlmode->setControlModes(settedControlMode));
236
237 // Check that the read control mode is actually position direct
238 // Let's try 10 times because if the remapper is using some controlBoard_nwc_yarp,
239 // it is possible that this return false if it is called before the first message
240 // has been received from the controlboard_nws_yarp
241 bool getControlModesOk = false;
242 for(int wait=0; wait < 20 && !getControlModesOk; wait++)
243 {
244 getControlModesOk = ctrlmode->getControlModes(readedControlMode);
246 }
248
249 // Test position direct methods
250
251 // Set also the speeds in the mean time, so we are sure that we don't get
252 // spurios successful set/get of position because of intermediate buffers
253 CHECK_FALSE(pos->setTrajSpeeds(setTrajSpeeds.data()));
254
255 // Wait some time to make sure that the vector has been correctly propagated
256 // back and forth
258
259 // Read position
260 CHECK_FALSE(posdir->getRefPositions(readedPosition.data()));
261
262 // Do a similar test for the encoders
263 // in fakeMotionControl their value is the one setted with setPosition
264 CHECK(encs->getEncoders(readedEncoders.data())); // getEncoders correctly called
265}
266
267
268TEST_CASE("dev::remotecontrolboardremapperTest", "[yarp::dev]")
269{
270 YARP_REQUIRE_PLUGIN("fakeMotionControl", "device");
271 YARP_REQUIRE_PLUGIN("fakeMotionControlMicro", "device");
272 YARP_REQUIRE_PLUGIN("controlBoard_nws_yarp", "device");
273 YARP_REQUIRE_PLUGIN("remotecontrolboardremapper", "device");
274
275 Network::setLocalMode(true);
276
277 SECTION("Test the remotecontrolboardremapper with fakeMotionControl")
278 {
279 // We first allocate three fakeMotionControl boards
280 // and their wrappers that we will remap using the remapper
281 std::vector<std::unique_ptr<PolyDriver>> fmcbs;
282 std::vector<std::unique_ptr<PolyDriver>> wrappers;
283 fmcbs.resize(3);
284 wrappers.resize(3);
285
286 std::vector<int> fmcbsSizes;
287 fmcbsSizes.push_back(2);
288 fmcbsSizes.push_back(3);
289 fmcbsSizes.push_back(4);
290
291 std::vector<std::string> fmcbsNames;
292 fmcbsNames.push_back("fakeControlBoardA");
293 fmcbsNames.push_back("fakeControlBoardB");
294 fmcbsNames.push_back("fakeControlBoardC");
295
296 std::vector<std::string> wrapperNetworks;
297 wrapperNetworks.push_back("net_a");
298 wrapperNetworks.push_back("net_b");
299 wrapperNetworks.push_back("net_c");
300
301
302 for(int i=0; i < 3; i++)
303 {
304 fmcbs[i] = std::make_unique<PolyDriver>();
305
306 Property p;
307
308 if(i==0) { p.fromConfig(fmcA_file_content); }
309 if(i==1) { p.fromConfig(fmcB_file_content); }
310 if(i==2) { p.fromConfig(fmcC_file_content); }
311
312 REQUIRE(fmcbs[i]->open(p)); // fakeMotionControlBoard open reported successful
313
314 IPositionControl *pos = nullptr;
315 REQUIRE(fmcbs[i]->view(pos)); // interface position correctly opened
316 REQUIRE(pos);
317
318 size_t axes = 0;
319 pos->getAxes(axes);
320 CHECK(axes == fmcbsSizes[i]); // fakeMotionControlBoard seems functional
321
322 // Open the wrapper
323 wrappers[i] = std::make_unique<PolyDriver>();
324
325 if(i==0) { p.fromConfig(wrapperA_file_content); }
326 if(i==1) { p.fromConfig(wrapperB_file_content); }
327 if(i==2) { p.fromConfig(wrapperC_file_content); }
328
329 REQUIRE(wrappers[i]->open(p)); // controlBoard_nws_yarp open reported successful
330
332 REQUIRE(wrappers[i]->view(iwrap)); // interface for multiple wrapper correctly opened for the controlBoard_nws_yarp
333 REQUIRE(iwrap);
334
336 pdList.push(fmcbs[i].get(), wrapperNetworks[i].c_str());
337
338 REQUIRE(iwrap->attachAll(pdList)); // controlBoard_nws_yarp attached successfully to the device
339 }
340
341 // Create a list containing all the fake controlboards
343
344 for(int i=0; i < 3; i++)
345 {
346 fmcList.push(fmcbs[i].get(),fmcbsNames[i].c_str());
347 }
348
349 // Open the remotecontrolboardremapper
352 pRemoteRemapper.put("device","remotecontrolboardremapper");
353 pRemoteRemapper.addGroup("axesNames");
354 Bottle & remoteAxesList = pRemoteRemapper.findGroup("axesNames").addList();
355 remoteAxesList.addString("axisA1");
356 remoteAxesList.addString("axisB1");
357 remoteAxesList.addString("axisC1");
358 remoteAxesList.addString("axisB3");
359 remoteAxesList.addString("axisC3");
360 remoteAxesList.addString("axisA2");
361 size_t nrOfRemappedAxes = 6;
362
365 remoteControlBoardsList.addString("/testRemapperRobot/a");
366 remoteControlBoardsList.addString("/testRemapperRobot/b");
367 remoteControlBoardsList.addString("/testRemapperRobot/c");
368 pRemoteRemapper.put("remoteControlBoards",remoteControlBoards.get(0));
369
370 pRemoteRemapper.put("localPortPrefix","/test/remoteControlBoardRemapper");
371
372 Property & opts = pRemoteRemapper.addGroup("REMOTE_CONTROLBOARD_OPTIONS");
373 opts.put("writeStrict","on");
374
375 REQUIRE(ddRemoteRemapper.open(pRemoteRemapper)); // remotecontrolboardremapper open reported successful, testing it
376
377 // Test the remotecontrolboardremapper
379
380 // Close devices
382
383 for(int i=0; i < 3; i++)
384 {
385 wrappers[i]->close();
386 }
387 for(int i=0; i < 3; i++)
388 {
389 fmcbs[i]->close();
390 }
391 }
392
393 SECTION("Test the remotecontrolboardremapper with fakeMotionControlMicro")
394 {
395 // We first allocate three fakeMotionControl boards
396 // and their wrappers that we will remap using the remapper
397 std::vector<std::unique_ptr<PolyDriver>> fmcbs;
398 std::vector<std::unique_ptr<PolyDriver>> wrappers;
399 fmcbs.resize(3);
400 wrappers.resize(3);
401
402 std::vector<int> fmcbsSizes;
403 fmcbsSizes.push_back(2);
404 fmcbsSizes.push_back(3);
405 fmcbsSizes.push_back(4);
406
407 std::vector<std::string> fmcbsNames;
408 fmcbsNames.push_back("fakeControlBoardAMicro");
409 fmcbsNames.push_back("fakeControlBoardBMicro");
410 fmcbsNames.push_back("fakeControlBoardCMicro");
411
412 std::vector<std::string> wrapperNetworks;
413 wrapperNetworks.push_back("net_a");
414 wrapperNetworks.push_back("net_b");
415 wrapperNetworks.push_back("net_c");
416
417
418 for(int i=0; i < 3; i++)
419 {
420 fmcbs[i] = std::make_unique<PolyDriver>();
421
422 Property p;
423
424 if(i==0) { p.fromConfig(fmcA_file_content_micro); }
425 if(i==1) { p.fromConfig(fmcB_file_content_micro); }
426 if(i==2) { p.fromConfig(fmcC_file_content_micro); }
427
428 REQUIRE(fmcbs[i]->open(p)); // fakeMotionControlBoard open reported successful
429
430 IPositionControl *pos = nullptr;
431 REQUIRE_FALSE(fmcbs[i]->view(pos)); // interface position correctly not opened
432 REQUIRE_FALSE(pos);
433
434 size_t axes = 0;
435
436 IAxisInfoRaw *axisInfo = nullptr;
437 REQUIRE(fmcbs[i]->view(axisInfo)); // interface axisInfo correctly opened
439 axisInfo->getAxes(axes);
440 CHECK(axes == fmcbsSizes[i]); // fakeMotionControlBoard seems functional
441
442 // Open the wrapper
443 wrappers[i] = std::make_unique<PolyDriver>();
444
445 if(i==0) { p.fromConfig(wrapperA_file_content); }
446 if(i==1) { p.fromConfig(wrapperB_file_content); }
447 if(i==2) { p.fromConfig(wrapperC_file_content); }
448
449 REQUIRE(wrappers[i]->open(p)); // controlBoard_nws_yarp open reported successful
450
452 REQUIRE(wrappers[i]->view(iwrap)); // interface for multiple wrapper correctly opened for the controlBoard_nws_yarp
453 REQUIRE(iwrap);
454
456 pdList.push(fmcbs[i].get(),wrapperNetworks[i].c_str());
457
458 REQUIRE(iwrap->attachAll(pdList)); // controlBoard_nws_yarp attached successfully to the device
459 }
460
461 // Create a list containing all the fake controlboards
463
464 for(int i=0; i < 3; i++)
465 {
466 fmcList.push(fmcbs[i].get(),fmcbsNames[i].c_str());
467 }
468
469 // Open the remotecontrolboardremapper
472 pRemoteRemapper.put("device","remotecontrolboardremapper");
473 pRemoteRemapper.addGroup("axesNames");
474 Bottle & remoteAxesList = pRemoteRemapper.findGroup("axesNames").addList();
475 remoteAxesList.addString("axisA1");
476 remoteAxesList.addString("axisB1");
477 remoteAxesList.addString("axisC1");
478 remoteAxesList.addString("axisB3");
479 remoteAxesList.addString("axisC3");
480 remoteAxesList.addString("axisA2");
481 size_t nrOfRemappedAxes = 6;
482
485 remoteControlBoardsList.addString("/testRemapperRobot/a");
486 remoteControlBoardsList.addString("/testRemapperRobot/b");
487 remoteControlBoardsList.addString("/testRemapperRobot/c");
488 pRemoteRemapper.put("remoteControlBoards",remoteControlBoards.get(0));
489
490 pRemoteRemapper.put("localPortPrefix","/test/remoteControlBoardRemapper");
491
492 Property & opts = pRemoteRemapper.addGroup("REMOTE_CONTROLBOARD_OPTIONS");
493 opts.put("writeStrict","on");
494
495 REQUIRE(ddRemoteRemapper.open(pRemoteRemapper)); // remotecontrolboardremapper open reported successful, testing it
496
497 // Test the remotecontrolboardremapper
499
500 // Close devices
502
503 for(int i=0; i < 3; i++)
504 {
505 wrappers[i]->close();
506 }
507 for(int i=0; i < 3; i++)
508 {
509 fmcbs[i]->close();
510 }
511 }
512
513 Network::setLocalMode(false);
514}
define control board standard interfaces
Interface for getting information about specific axes, if available.
Definition IAxisInfo.h:73
Interface for setting control mode in control board.
Control board, encoder interface.
Definition IEncoders.h:117
virtual yarp::dev::ReturnValue getEncoders(double *encs)=0
Read the position of all axes.
Interface for an object that can wrap/attach to to another.
Interface for a generic control board device implementing position control.
virtual yarp::dev::ReturnValue setTrajSpeeds(const double *spds)=0
Set reference speed on all joints.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
Interface for a generic control board device implementing position control.
A container for a device driver.
Definition PolyDriver.h:23
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
A class for storing options and configuration information.
Definition Property.h:32
void fromConfig(const char *txt, bool wipe=true)
Parses text in the configuration format described in fromConfigFile().
void wait_safe(const double seconds)
Definition TestUtils.cpp:17
For streams capable of holding different kinds of content, check what they actually have.
SelectableControlModeEnum
void delay(double seconds)
Wait for a certain number of seconds.
Definition Time.cpp:111
An interface to the operating system, including Port based communication.
const char * wrapperC_file_content
static void checkRemapperMicro(yarp::dev::PolyDriver &ddRemapper, int rand, size_t nrOfRemappedAxes)
const char * fmcA_file_content_micro
const char * fmcA_file_content
TEST_CASE("dev::remotecontrolboardremapperTest", "[yarp::dev]")
const char * fmcB_file_content_micro
const char * fmcB_file_content
const char * fmcC_file_content_micro
const char * fmcC_file_content
static void checkRemapper(yarp::dev::PolyDriver &ddRemapper, int rand, size_t nrOfRemappedAxes)
const char * wrapperA_file_content
const char * wrapperB_file_content