YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
controlBoardRemapper_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
58const char *wrapperA_file_content = "device controlBoard_nws_yarp\n"
59 "name /testRemapperRobot/a\n"
60 "period 0.01\n";
61
62const char *wrapperB_file_content = "device controlBoard_nws_yarp\n"
63 "name /testRemapperRobot/b\n"
64 "period 0.01\n";
65
66const char *wrapperC_file_content = "device controlBoard_nws_yarp\n"
67 "name /testRemapperRobot/c\n"
68 "period 0.01\n";
69
70
72{
73 IPositionControl *pos = nullptr;
74 REQUIRE(ddRemapper.view(pos)); // interface position correctly opened
75 REQUIRE(pos);
76
77 size_t axes = 0;
78 CHECK(pos->getAxes(axes)); // getAxes returned correctly
79 CHECK((size_t) axes == nrOfRemappedAxes); // remapper seems functional
80
81 IPositionDirect *posdir = nullptr;
82 REQUIRE(ddRemapper.view(posdir)); // direct position interface correctly opened
84
85 IEncoders * encs = nullptr;
86 REQUIRE(ddRemapper.view(encs)); // encoders interface correctly opened
87 REQUIRE(encs);
88
89 IControlMode *ctrlmode = nullptr;
90 REQUIRE(ddRemapper.view(ctrlmode)); // control mode interface correctly opened
92
93 // Vector used for setting/getting data from the controlboard
94 std::vector<double> setPosition(nrOfRemappedAxes,-10),
95 setTrajSpeeds(nrOfRemappedAxes,-15),
98
99 for(size_t i=0; i < nrOfRemappedAxes; i++)
100 {
101 setPosition[i] = i*100.0+50.0 + rand;
102 setTrajSpeeds[i] = i*10.0+5 + rand;
103 readedPosition[i] = -100 + rand;
104 }
105
106 // Set the control mode in position direct
109
110 CHECK(ctrlmode->setControlModes(settedControlMode)); // setControlModes correctly called
112
113 // Check that the read control mode is actually position direct
114 // Let's try 10 times because if the remapper is using some controlBoard_nwc_yarp,
115 // it is possible that this return false if it is called before the first message
116 // has been received from the controlboard_nws_yarp
117 bool getControlModesOk = false;
118 for(int wait=0; wait < 20 && !getControlModesOk; wait++)
119 {
120 getControlModesOk = ctrlmode->getControlModes(readedControlMode);
122 }
123 CHECK(getControlModesOk); // getControlModes correctly called
124
125 for(size_t i=0; i < nrOfRemappedAxes; i++)
126 {
127 CHECK(settedControlMode[i] == (yarp::dev::SelectableControlModeEnum)readedControlMode[i]); // Setted control mode and readed control mode match
128 }
129
130 // Test position direct methods
131
132 // Set position
133 CHECK(posdir->setPositions(setPosition.data())); // setPositions correctly called
134
135 // Set also the speeds in the mean time, so we are sure that we don't get
136 // spurious successful set/get of position because of intermediate buffers
137 CHECK(pos->setTrajSpeeds(setTrajSpeeds.data())); // setTrajSpeeds correctly called
138
139 // Wait some time to make sure that the vector has been correctly propagated
140 // back and forth
142
143 // Read position
144 CHECK(posdir->getRefPositions(readedPosition.data())); // getRefPositions correctly called
145
146 // Check that the two vector match
147 for(size_t i=0; i < nrOfRemappedAxes; i++)
148 {
149 CHECK(setPosition[i] == readedPosition[i]); // Setted position and readed ref position match
150 }
151
152 // Do a similar test for the encoders
153 // in fakeMotionControl their value is the one setted with setPosition
154 CHECK(encs->getEncoders(readedEncoders.data())); // getEncoders correctly called
155
156 // Check that the two vector match
157 for(size_t i=0; i < nrOfRemappedAxes; i++)
158 {
159 CHECK(setPosition[i] == readedEncoders[i]); // Setted position and readed encoders match
160 }
161}
162
163
165{
166 IPositionControl *pos = nullptr;
167 REQUIRE(ddRemapper.view(pos)); // interface position correctly opened
168 REQUIRE(pos);
169
170 size_t axes = 0;
171 CHECK(pos->getAxes(axes)); // getAxes returned correctly
172 CHECK((size_t) axes == nrOfRemappedAxes); // remapper seems functional
173
174 IPositionDirect *posdir = nullptr;
175 REQUIRE(ddRemapper.view(posdir)); // direct position interface correctly opened
177
178 IEncoders * encs = nullptr;
179 REQUIRE(ddRemapper.view(encs)); // encoders interface correctly opened
180 REQUIRE(encs);
181
182 IControlMode *ctrlmode = nullptr;
183 REQUIRE(ddRemapper.view(ctrlmode)); // control mode interface correctly opened
185
186 // Vector used for setting/getting data from the controlboard
187 std::vector<double> setPosition(nrOfRemappedAxes,-10),
188 setTrajSpeeds(nrOfRemappedAxes,-15),
191
192 for(size_t i=0; i < nrOfRemappedAxes; i++)
193 {
194 setPosition[i] = i*100.0+50.0 + rand;
195 setTrajSpeeds[i] = i*10.0+5 + rand;
196 readedPosition[i] = -100 + rand;
197 }
198
199 // Set the control mode in position direct
202
203 CHECK_FALSE(ctrlmode->setControlModes(settedControlMode));
204
205 // Check that the read control mode is actually position direct
206 // Let's try 10 times because if the remapper is using some controlBoard_nwc_yarp,
207 // it is possible that this return false if it is called before the first message
208 // has been received from the controlboard_nws_yarp
209 bool getControlModesOk = false;
210 for(int wait=0; wait < 20 && !getControlModesOk; wait++)
211 {
212 getControlModesOk = ctrlmode->getControlModes(readedControlMode);
214 }
216
217 // Test position direct methods
218
219 // Set also the speeds in the mean time, so we are sure that we don't get
220 // spurios successful set/get of position because of intermediate buffers
221 CHECK_FALSE(pos->setTrajSpeeds(setTrajSpeeds.data()));
222
223 // Wait some time to make sure that the vector has been correctly propagated
224 // back and forth
226
227 // Read position
228 CHECK_FALSE(posdir->getRefPositions(readedPosition.data()));
229
230 // Do a similar test for the encoders
231 // in fakeMotionControl their value is the one setted with setPosition
232 CHECK(encs->getEncoders(readedEncoders.data())); // getEncoders correctly called
233}
234
235
236TEST_CASE("dev::ControlBoardRemapperTest", "[yarp::dev]")
237{
238 YARP_REQUIRE_PLUGIN("fakeMotionControl", "device");
239 YARP_REQUIRE_PLUGIN("fakeMotionControlMicro", "device");
240 YARP_REQUIRE_PLUGIN("controlboardremapper", "device");
241 YARP_REQUIRE_PLUGIN("controlBoard_nws_yarp", "device");
242
243 Network::setLocalMode(true);
244
245 SECTION("Test the controlboard remapper")
246 {
247 // We first allocate three fakeMotionControl boards
248 // and their wrappers that we will remap using the remapper
249 std::vector<std::unique_ptr<PolyDriver>> fmcbs;
250 std::vector<std::unique_ptr<PolyDriver>> wrappers;
251 fmcbs.resize(3);
252 wrappers.resize(3);
253
254 std::vector<int> fmcbsSizes;
255 fmcbsSizes.push_back(2);
256 fmcbsSizes.push_back(3);
257 fmcbsSizes.push_back(4);
258
259 std::vector<std::string> fmcbsNames;
260 fmcbsNames.push_back("fakeControlBoardA");
261 fmcbsNames.push_back("fakeControlBoardB");
262 fmcbsNames.push_back("fakeControlBoardC");
263
264 std::vector<std::string> wrapperNetworks;
265 wrapperNetworks.push_back("net_a");
266 wrapperNetworks.push_back("net_b");
267 wrapperNetworks.push_back("net_c");
268
269
270 for(int i=0; i < 3; i++)
271 {
272 fmcbs[i] = std::make_unique<PolyDriver>();
273
274 Property p;
275
276 if(i==0) { p.fromConfig(fmcA_file_content); }
277 if(i==1) { p.fromConfig(fmcB_file_content); }
278 if(i==2) { p.fromConfig(fmcC_file_content); }
279
280 REQUIRE(fmcbs[i]->open(p)); // fakeMotionControlBoard open reported successful
281
282 IPositionControl *pos = nullptr;
283 REQUIRE(fmcbs[i]->view(pos)); // interface position correctly opened
284 REQUIRE(pos);
285
286 size_t axes = 0;
287 pos->getAxes(axes);
288 CHECK(axes == fmcbsSizes[i]); // fakeMotionControlBoard seems functional
289
290 // Open the wrapper
291 wrappers[i] = std::make_unique<PolyDriver>();
292
293 if(i==0) { p.fromConfig(wrapperA_file_content); }
294 if(i==1) { p.fromConfig(wrapperB_file_content); }
295 if(i==2) { p.fromConfig(wrapperC_file_content); }
296
297 REQUIRE(wrappers[i]->open(p)); // controlBoard_nws_yarp open reported successful
298
300 REQUIRE(wrappers[i]->view(iwrap)); // interface for multiple wrapper correctly opened for the controlBoard_nws_yarp
301 REQUIRE(iwrap);
302
304 pdList.push(fmcbs[i].get(), wrapperNetworks[i].c_str());
305
306 REQUIRE(iwrap->attachAll(pdList)); // controlBoard_nws_yarp attached successfully to the device
307 }
308
309 // Create a list containing all the fake controlboards
311
312 for(int i=0; i < 3; i++)
313 {
314 fmcList.push(fmcbs[i].get(),fmcbsNames[i].c_str());
315 }
316
317 // Open a controlboardremapper with the wrong axisName,
318 // and make sure that if fails during attachAll
321 pRemapperWN.put("device","controlboardremapper");
322 pRemapperWN.addGroup("axesNames");
323 Bottle & axesListWN = pRemapperWN.findGroup("axesNames").addList();
324 axesListWN.addString("axisA1");
325 axesListWN.addString("axisB1");
326 axesListWN.addString("axisC1");
327 axesListWN.addString("axisB3");
328 axesListWN.addString("axisC3");
329 axesListWN.addString("axisA2");
330 axesListWN.addString("thisIsAnAxisNameThatDoNotExist");
331
332 REQUIRE(ddRemapperWN.open(pRemapperWN)); // controlboardremapper with wrong names open reported successful
333
335 REQUIRE(ddRemapperWN.view(imultwrapWN)); // interface for multiple wrapper with wrong names correctly opened
337
338 REQUIRE_FALSE(imultwrapWN->attachAll(fmcList)); // attachAll for controlboardremapper with wrong names successful
339
340 // Make sure that a controlboard in which attachAll is not successfull
341 // closes correctly
342 CHECK(ddRemapperWN.close()); // controlboardremapper with wrong names close was successful
343
344 // Open the controlboardremapper
347 pRemapper.put("device","controlboardremapper");
348 pRemapper.addGroup("axesNames");
349 Bottle & axesList = pRemapper.findGroup("axesNames").addList();
350 axesList.addString("axisA1");
351 axesList.addString("axisB1");
352 axesList.addString("axisC1");
353 axesList.addString("axisB3");
354 axesList.addString("axisC3");
355 axesList.addString("axisA2");
356 size_t nrOfRemappedAxes = 6;
357
358 REQUIRE(ddRemapper.open(pRemapper)); // controlboardremapper open reported successful
359
361 REQUIRE(ddRemapper.view(imultwrap)); // interface for multiple wrapper correctly opened
363
364 REQUIRE(imultwrap->attachAll(fmcList)); // attachAll for controlboardremapper successful
365
366
367 // Test the controlboardremapper
369
370 // Close devices
371 imultwrap->detachAll();
373
374 for(int i=0; i < 3; i++)
375 {
376 wrappers[i]->close();
377 }
378 for(int i=0; i < 3; i++)
379 {
380 fmcbs[i]->close();
381 }
382 }
383
384 SECTION("Test the controlboard remapper micro")
385 {
386 // We first allocate three fakeMotionControl boards
387 // and their wrappers that we will remap using the remapper
388 std::vector<std::unique_ptr<PolyDriver>> fmcbs;
389 std::vector<std::unique_ptr<PolyDriver>> wrappers;
390 fmcbs.resize(3);
391 wrappers.resize(3);
392
393 std::vector<int> fmcbsSizes;
394 fmcbsSizes.push_back(2);
395 fmcbsSizes.push_back(3);
396 fmcbsSizes.push_back(4);
397
398 std::vector<std::string> fmcbsNames;
399 fmcbsNames.push_back("fakeControlBoardAMicro");
400 fmcbsNames.push_back("fakeControlBoardBMicro");
401 fmcbsNames.push_back("fakeControlBoardCMicro");
402
403 std::vector<std::string> wrapperNetworks;
404 wrapperNetworks.push_back("net_a");
405 wrapperNetworks.push_back("net_b");
406 wrapperNetworks.push_back("net_c");
407
408
409 for(int i=0; i < 3; i++)
410 {
411 fmcbs[i] = std::make_unique<PolyDriver>();
412
413 Property p;
414
415 if(i==0) { p.fromConfig(fmcA_file_content_micro); }
416 if(i==1) { p.fromConfig(fmcB_file_content_micro); }
417 if(i==2) { p.fromConfig(fmcC_file_content_micro); }
418
419 REQUIRE(fmcbs[i]->open(p)); // fakeMotionControlBoard open reported successful
420
421 IPositionControl *pos = nullptr;
422 REQUIRE_FALSE(fmcbs[i]->view(pos)); // interface position correctly not opened
423 REQUIRE_FALSE(pos);
424
425 size_t axes = 0;
426
427 IAxisInfoRaw *axisInfo = nullptr;
428 REQUIRE(fmcbs[i]->view(axisInfo)); // interface axisInfo correctly opened
430 axisInfo->getAxes(axes);
431 CHECK(axes == fmcbsSizes[i]); // fakeMotionControlBoard seems functional
432
433 // Open the wrapper
434 wrappers[i] = std::make_unique<PolyDriver>();
435
436 if(i==0) { p.fromConfig(wrapperA_file_content); }
437 if(i==1) { p.fromConfig(wrapperB_file_content); }
438 if(i==2) { p.fromConfig(wrapperC_file_content); }
439
440 REQUIRE(wrappers[i]->open(p)); // controlBoard_nws_yarp open reported successful
441
443 REQUIRE(wrappers[i]->view(iwrap)); // interface for multiple wrapper correctly opened for the controlBoard_nws_yarp
444 REQUIRE(iwrap);
445
447 pdList.push(fmcbs[i].get(),wrapperNetworks[i].c_str());
448
449 REQUIRE(iwrap->attachAll(pdList)); // controlBoard_nws_yarp attached successfully to the device
450 }
451
452 // Create a list containing all the fake controlboards
454
455 for(int i=0; i < 3; i++)
456 {
457 fmcList.push(fmcbs[i].get(),fmcbsNames[i].c_str());
458 }
459
460 // Open a controlboardremapper with the wrong axisName,
461 // and make sure that if fails during attachAll
464 pRemapperWN.put("device","controlboardremapper");
465 pRemapperWN.addGroup("axesNames");
466 Bottle & axesListWN = pRemapperWN.findGroup("axesNames").addList();
467 axesListWN.addString("axisA1");
468 axesListWN.addString("axisB1");
469 axesListWN.addString("axisC1");
470 axesListWN.addString("axisB3");
471 axesListWN.addString("axisC3");
472 axesListWN.addString("axisA2");
473 axesListWN.addString("thisIsAnAxisNameThatDoNotExist");
474
475 REQUIRE(ddRemapperWN.open(pRemapperWN)); // controlboardremapper with wrong names open reported successful
476
478 REQUIRE(ddRemapperWN.view(imultwrapWN)); // interface for multiple wrapper with wrong names correctly opened
480
481 REQUIRE_FALSE(imultwrapWN->attachAll(fmcList)); // attachAll for controlboardremapper with wrong names successful
482
483 // Make sure that a controlboard in which attachAll is not successfull
484 // closes correctly
485 CHECK(ddRemapperWN.close()); // controlboardremapper with wrong names close was successful
486
487 // Open the controlboardremapper
490 pRemapper.put("device","controlboardremapper");
491 pRemapper.addGroup("axesNames");
492 Bottle & axesList = pRemapper.findGroup("axesNames").addList();
493 axesList.addString("axisA1");
494 axesList.addString("axisB1");
495 axesList.addString("axisC1");
496 axesList.addString("axisB3");
497 axesList.addString("axisC3");
498 axesList.addString("axisA2");
499 size_t nrOfRemappedAxes = 6;
500
501 REQUIRE(ddRemapper.open(pRemapper)); // controlboardremapper open reported successful
502
504 REQUIRE(ddRemapper.view(imultwrap)); // interface for multiple wrapper correctly opened
506
507 REQUIRE(imultwrap->attachAll(fmcList)); // attachAll for controlboardremapper successful
508
509
510 // Test the controlboardremapper
512
513 // Close devices
514 imultwrap->detachAll();
516
517 for(int i=0; i < 3; i++)
518 {
519 wrappers[i]->close();
520 }
521 for(int i=0; i < 3; i++)
522 {
523 fmcbs[i]->close();
524 }
525 }
526
527 Network::setLocalMode(false);
528}
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().
const char * wrapperC_file_content
static void checkRemapperMicro(yarp::dev::PolyDriver &ddRemapper, int rand, size_t nrOfRemappedAxes)
TEST_CASE("dev::ControlBoardRemapperTest", "[yarp::dev]")
const char * fmcA_file_content_micro
const char * fmcA_file_content
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
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.