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::ControlBoardRemapperTest1", "[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 YARP_REQUIRE_PLUGIN("remotecontrolboardremapper", "device");
243
244 Network::setLocalMode(true);
245
246 SECTION("Test the controlboard remapper")
247 {
248 // We first allocate three fakeMotionControl boards
249 // and their wrappers that we will remap using the remapper
250 std::vector<std::unique_ptr<PolyDriver>> fmcbs;
251 std::vector<std::unique_ptr<PolyDriver>> wrappers;
252 fmcbs.resize(3);
253 wrappers.resize(3);
254
255 std::vector<int> fmcbsSizes;
256 fmcbsSizes.push_back(2);
257 fmcbsSizes.push_back(3);
258 fmcbsSizes.push_back(4);
259
260 std::vector<std::string> fmcbsNames;
261 fmcbsNames.push_back("fakeControlBoardA");
262 fmcbsNames.push_back("fakeControlBoardB");
263 fmcbsNames.push_back("fakeControlBoardC");
264
265 std::vector<std::string> wrapperNetworks;
266 wrapperNetworks.push_back("net_a");
267 wrapperNetworks.push_back("net_b");
268 wrapperNetworks.push_back("net_c");
269
270
271 for(int i=0; i < 3; i++)
272 {
273 fmcbs[i] = std::make_unique<PolyDriver>();
274
275 Property p;
276
277 if(i==0) { p.fromConfig(fmcA_file_content); }
278 if(i==1) { p.fromConfig(fmcB_file_content); }
279 if(i==2) { p.fromConfig(fmcC_file_content); }
280
281 REQUIRE(fmcbs[i]->open(p)); // fakeMotionControlBoard open reported successful
282
283 IPositionControl *pos = nullptr;
284 REQUIRE(fmcbs[i]->view(pos)); // interface position correctly opened
285 REQUIRE(pos);
286
287 size_t axes = 0;
288 pos->getAxes(axes);
289 CHECK(axes == fmcbsSizes[i]); // fakeMotionControlBoard seems functional
290
291 // Open the wrapper
292 wrappers[i] = std::make_unique<PolyDriver>();
293
294 if(i==0) { p.fromConfig(wrapperA_file_content); }
295 if(i==1) { p.fromConfig(wrapperB_file_content); }
296 if(i==2) { p.fromConfig(wrapperC_file_content); }
297
298 REQUIRE(wrappers[i]->open(p)); // controlBoard_nws_yarp open reported successful
299
301 REQUIRE(wrappers[i]->view(iwrap)); // interface for multiple wrapper correctly opened for the controlBoard_nws_yarp
302 REQUIRE(iwrap);
303
305 pdList.push(fmcbs[i].get(), wrapperNetworks[i].c_str());
306
307 REQUIRE(iwrap->attachAll(pdList)); // controlBoard_nws_yarp attached successfully to the device
308 }
309
310 // Create a list containing all the fake controlboards
312
313 for(int i=0; i < 3; i++)
314 {
315 fmcList.push(fmcbs[i].get(),fmcbsNames[i].c_str());
316 }
317
318 // Open the remotecontrolboardremapper
321 pRemoteRemapper.put("device","remotecontrolboardremapper");
322 pRemoteRemapper.addGroup("axesNames");
323 Bottle & remoteAxesList = pRemoteRemapper.findGroup("axesNames").addList();
324 remoteAxesList.addString("axisA1");
325 remoteAxesList.addString("axisB1");
326 remoteAxesList.addString("axisC1");
327 remoteAxesList.addString("axisB3");
328 remoteAxesList.addString("axisC3");
329 remoteAxesList.addString("axisA2");
330 size_t nrOfRemappedAxes = 6;
331
334 remoteControlBoardsList.addString("/testRemapperRobot/a");
335 remoteControlBoardsList.addString("/testRemapperRobot/b");
336 remoteControlBoardsList.addString("/testRemapperRobot/c");
337 pRemoteRemapper.put("remoteControlBoards",remoteControlBoards.get(0));
338
339 pRemoteRemapper.put("localPortPrefix","/test/remoteControlBoardRemapper");
340
341 Property & opts = pRemoteRemapper.addGroup("REMOTE_CONTROLBOARD_OPTIONS");
342 opts.put("writeStrict","on");
343
344 REQUIRE(ddRemoteRemapper.open(pRemoteRemapper)); // remotecontrolboardremapper open reported successful, testing it
345
346 // Test the remotecontrolboardremapper
348
349 // Close devices
351
352 for(int i=0; i < 3; i++)
353 {
354 wrappers[i]->close();
355 }
356 for(int i=0; i < 3; i++)
357 {
358 fmcbs[i]->close();
359 }
360 }
361
362 SECTION("Test the controlboard remapper micro")
363 {
364 // We first allocate three fakeMotionControl boards
365 // and their wrappers that we will remap using the remapper
366 std::vector<std::unique_ptr<PolyDriver>> fmcbs;
367 std::vector<std::unique_ptr<PolyDriver>> wrappers;
368 fmcbs.resize(3);
369 wrappers.resize(3);
370
371 std::vector<int> fmcbsSizes;
372 fmcbsSizes.push_back(2);
373 fmcbsSizes.push_back(3);
374 fmcbsSizes.push_back(4);
375
376 std::vector<std::string> fmcbsNames;
377 fmcbsNames.push_back("fakeControlBoardAMicro");
378 fmcbsNames.push_back("fakeControlBoardBMicro");
379 fmcbsNames.push_back("fakeControlBoardCMicro");
380
381 std::vector<std::string> wrapperNetworks;
382 wrapperNetworks.push_back("net_a");
383 wrapperNetworks.push_back("net_b");
384 wrapperNetworks.push_back("net_c");
385
386
387 for(int i=0; i < 3; i++)
388 {
389 fmcbs[i] = std::make_unique<PolyDriver>();
390
391 Property p;
392
393 if(i==0) { p.fromConfig(fmcA_file_content_micro); }
394 if(i==1) { p.fromConfig(fmcB_file_content_micro); }
395 if(i==2) { p.fromConfig(fmcC_file_content_micro); }
396
397 REQUIRE(fmcbs[i]->open(p)); // fakeMotionControlBoard open reported successful
398
399 IPositionControl *pos = nullptr;
400 REQUIRE_FALSE(fmcbs[i]->view(pos)); // interface position correctly not opened
401 REQUIRE_FALSE(pos);
402
403 size_t axes = 0;
404
405 IAxisInfoRaw *axisInfo = nullptr;
406 REQUIRE(fmcbs[i]->view(axisInfo)); // interface axisInfo correctly opened
408 axisInfo->getAxes(axes);
409 CHECK(axes == fmcbsSizes[i]); // fakeMotionControlBoard seems functional
410
411 // Open the wrapper
412 wrappers[i] = std::make_unique<PolyDriver>();
413
414 if(i==0) { p.fromConfig(wrapperA_file_content); }
415 if(i==1) { p.fromConfig(wrapperB_file_content); }
416 if(i==2) { p.fromConfig(wrapperC_file_content); }
417
418 REQUIRE(wrappers[i]->open(p)); // controlBoard_nws_yarp open reported successful
419
421 REQUIRE(wrappers[i]->view(iwrap)); // interface for multiple wrapper correctly opened for the controlBoard_nws_yarp
422 REQUIRE(iwrap);
423
425 pdList.push(fmcbs[i].get(),wrapperNetworks[i].c_str());
426
427 REQUIRE(iwrap->attachAll(pdList)); // controlBoard_nws_yarp attached successfully to the device
428 }
429
430 // Create a list containing all the fake controlboards
432
433 for(int i=0; i < 3; i++)
434 {
435 fmcList.push(fmcbs[i].get(),fmcbsNames[i].c_str());
436 }
437
438 // Open the remotecontrolboardremapper
441 pRemoteRemapper.put("device","remotecontrolboardremapper");
442 pRemoteRemapper.addGroup("axesNames");
443 Bottle & remoteAxesList = pRemoteRemapper.findGroup("axesNames").addList();
444 remoteAxesList.addString("axisA1");
445 remoteAxesList.addString("axisB1");
446 remoteAxesList.addString("axisC1");
447 remoteAxesList.addString("axisB3");
448 remoteAxesList.addString("axisC3");
449 remoteAxesList.addString("axisA2");
450 size_t nrOfRemappedAxes = 6;
451
454 remoteControlBoardsList.addString("/testRemapperRobot/a");
455 remoteControlBoardsList.addString("/testRemapperRobot/b");
456 remoteControlBoardsList.addString("/testRemapperRobot/c");
457 pRemoteRemapper.put("remoteControlBoards",remoteControlBoards.get(0));
458
459 pRemoteRemapper.put("localPortPrefix","/test/remoteControlBoardRemapper");
460
461 Property & opts = pRemoteRemapper.addGroup("REMOTE_CONTROLBOARD_OPTIONS");
462 opts.put("writeStrict","on");
463
464 REQUIRE(ddRemoteRemapper.open(pRemoteRemapper)); // remotecontrolboardremapper open reported successful, testing it
465
466 // Test the remotecontrolboardremapper
468
469 // Close devices
471
472 for(int i=0; i < 3; i++)
473 {
474 wrappers[i]->close();
475 }
476 for(int i=0; i < 3; i++)
477 {
478 fmcbs[i]->close();
479 }
480 }
481
482 Network::setLocalMode(false);
483}
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:65
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:33
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)
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)
TEST_CASE("dev::ControlBoardRemapperTest1", "[yarp::dev]")
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.
Definition OSTestMsg.cpp:13