Line data Source code
1 : #include <gtest/gtest.h>
2 : #include "testUtils.h"
3 :
4 : #include <rclcpp/rclcpp.hpp>
5 :
6 : #include <end_effector/FindActions.h>
7 : #include <end_effector/ParserMoveIt.h>
8 : #include <end_effector/GraspingActions/ActionPrimitive.h>
9 : #include <end_effector/GraspingActions/ActionPinchTight.h>
10 : #include <end_effector/GraspingActions/ActionPinchLoose.h>
11 :
12 :
13 : namespace {
14 :
15 : class testFindPinches: public ::testing::Test {
16 :
17 : protected:
18 :
19 44 : testFindPinches() {
20 44 : }
21 :
22 44 : virtual ~testFindPinches() {
23 44 : }
24 :
25 44 : virtual void SetUp() override {
26 :
27 44 : }
28 :
29 44 : virtual void SetUp(int argc, char **argv) {
30 :
31 44 : node = ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testFindPinches");
32 :
33 44 : ASSERT_NE(node, nullptr);
34 :
35 44 : std::shared_ptr <ROSEE::ParserMoveIt> parserMoveIt = std::make_shared <ROSEE::ParserMoveIt> (node);
36 :
37 : //if return false, models are not found and it is useless to continue the test
38 44 : ASSERT_TRUE(parserMoveIt->init ("robot_description", false)) ;
39 :
40 88 : ROSEE::FindActions actionsFinder (parserMoveIt);
41 :
42 88 : std::string folderForActions = "ROSEE/actions/" + parserMoveIt->getHandName();
43 :
44 88 : auto theTwoMaps = actionsFinder.findPinch(folderForActions + "/primitives/");
45 :
46 44 : pinchMap = theTwoMaps.first;
47 44 : pinchLooseMap = theTwoMaps.second;
48 :
49 44 : ROSEE::YamlWorker yamlWorker;
50 :
51 44 : if (pinchMap.size() > 0 ) {
52 22 : pinchParsedMap = yamlWorker.parseYamlPrimitive(folderForActions + "/primitives/" + "pinchTight.yaml" );
53 : }
54 :
55 44 : if (pinchLooseMap.size() > 0 ) {
56 11 : pinchLooseParsedMap = yamlWorker.parseYamlPrimitive(folderForActions + "/primitives/" + "pinchLoose.yaml");
57 : }
58 :
59 : }
60 :
61 44 : virtual void TearDown() {
62 44 : }
63 :
64 : rclcpp::Node::SharedPtr node;
65 :
66 :
67 : std::map < std::pair < std::string, std::string >, ROSEE::ActionPinchTight > pinchMap;
68 : std::map < std::set < std::string >, std::shared_ptr<ROSEE::ActionPrimitive> > pinchParsedMap;
69 :
70 : std::map < std::pair < std::string, std::string >, ROSEE::ActionPinchLoose > pinchLooseMap;
71 : std::map < std::set < std::string >, std::shared_ptr<ROSEE::ActionPrimitive> > pinchLooseParsedMap;
72 : };
73 :
74 :
75 8 : TEST_F ( testFindPinches, checkNumberLinks ) {
76 :
77 4 : SetUp(argc_g, argv_g);
78 :
79 8 : for (auto &mapEl: pinchMap ) {
80 :
81 : //being a pair the .first has always dimension 2
82 4 : EXPECT_EQ (2, mapEl.second.getFingersInvolved().size() ); //the names inside the action
83 4 : EXPECT_EQ (2, mapEl.second.getnFingersInvolved() ); //the int nLinkInvolved member of action
84 : }
85 :
86 8 : for (auto &mapEl: pinchParsedMap ) {
87 :
88 4 : EXPECT_EQ (2, mapEl.first.size()); // the key
89 4 : EXPECT_EQ (2, mapEl.second->getFingersInvolved().size()); //the names inside the action
90 4 : EXPECT_EQ (2, mapEl.second->getnFingersInvolved()); //the int nLinkInvolved member of action
91 : }
92 4 : }
93 :
94 8 : TEST_F ( testFindPinches, checkSizeStatesInfoSet ) {
95 :
96 4 : SetUp(argc_g, argv_g);
97 :
98 8 : for (auto &mapEl: pinchMap ) {
99 :
100 : //get the member which is set in costructor
101 4 : unsigned int size = mapEl.second.getMaxStoredActionStates();
102 :
103 : //it must be equal to the real size of the actionState set
104 4 : EXPECT_EQ ( size, mapEl.second.getActionStates().size() );
105 4 : EXPECT_EQ ( size, mapEl.second.getAllJointPos().size() );
106 : }
107 :
108 8 : for (auto &mapEl: pinchParsedMap ) {
109 :
110 : //get the member which is set in costructor
111 4 : unsigned int size = mapEl.second->getMaxStoredActionStates();
112 :
113 4 : EXPECT_EQ (size, mapEl.second->getAllJointPos().size());
114 : }
115 4 : }
116 :
117 8 : TEST_F ( testFindPinches, checkName ) {
118 :
119 4 : SetUp(argc_g, argv_g);
120 :
121 8 : for (auto &mapEl: pinchMap ) {
122 :
123 4 : EXPECT_TRUE (mapEl.second.getName().compare("pinchTight") == 0);
124 4 : EXPECT_EQ (ROSEE::ActionPrimitive::Type::PinchTight, mapEl.second.getPrimitiveType() );
125 : }
126 :
127 8 : for (auto &mapEl: pinchParsedMap ) {
128 :
129 4 : EXPECT_TRUE (mapEl.second->getName().compare("pinchTight") == 0);
130 4 : EXPECT_EQ (ROSEE::ActionPrimitive::Type::PinchTight, mapEl.second->getPrimitiveType() );
131 : }
132 4 : }
133 :
134 : //this is an important test: check if the order of statesInfo in right according to depth
135 8 : TEST_F ( testFindPinches, checkOrderStatesInfoSet ) {
136 :
137 4 : SetUp(argc_g, argv_g);
138 :
139 8 : for (auto &mapEl: pinchMap ) {
140 :
141 : std::vector < ROSEE::ActionPinchTight::StateWithContact> statesInfo =
142 8 : mapEl.second.getActionStates();
143 :
144 4 : double oldDepth = std::numeric_limits<double>::infinity();
145 :
146 16 : for (auto &setEl : statesInfo) {
147 12 : EXPECT_LE (std::abs(setEl.second.depth), std::abs(oldDepth) ); //lesser or equal
148 12 : oldDepth = setEl.second.depth;
149 : }
150 : }
151 :
152 :
153 8 : for (auto &mapEl: pinchParsedMap ) {
154 :
155 : std::shared_ptr <ROSEE::ActionPinchTight> pinchCasted =
156 4 : std::dynamic_pointer_cast < ROSEE::ActionPinchTight > (mapEl.second);
157 :
158 4 : ASSERT_FALSE (pinchCasted == nullptr);
159 : std::vector < ROSEE::ActionPinchTight::StateWithContact> statesInfo =
160 8 : pinchCasted->getActionStates();
161 :
162 4 : double oldDepth = std::numeric_limits<double>::infinity();
163 :
164 16 : for (auto &setEl : statesInfo) {
165 12 : EXPECT_LE (std::abs(setEl.second.depth), std::abs(oldDepth) ); //lesser or equal
166 12 : oldDepth = setEl.second.depth;
167 : }
168 : }
169 : }
170 :
171 : // to check if the found map is the same map that is emitted in the file and then parsed
172 8 : TEST_F ( testFindPinches, checkEmitParse ) {
173 :
174 4 : SetUp(argc_g, argv_g);
175 :
176 4 : ASSERT_EQ (pinchMap.size(), pinchParsedMap.size() );
177 :
178 8 : for (auto &mapEl: pinchParsedMap ) {
179 :
180 : std::shared_ptr <ROSEE::ActionPinchTight> pinchCasted =
181 4 : std::dynamic_pointer_cast < ROSEE::ActionPinchTight > (mapEl.second);
182 :
183 4 : ASSERT_FALSE (pinchCasted == nullptr);
184 4 : ASSERT_EQ (2, mapEl.first.size() );
185 4 : std::pair <std::string, std::string> keyPair;
186 4 : std::set<std::string>::iterator it = mapEl.first.begin();
187 4 : keyPair.first = *it;
188 4 : std::advance ( it, 1 );
189 4 : keyPair.second = *it;
190 :
191 : //std::string is ok to compare with _EQ
192 8 : EXPECT_EQ (pinchCasted->getName(), pinchMap.at(keyPair).getName() );
193 4 : EXPECT_EQ (pinchCasted->getType(), pinchMap.at(keyPair).getType() );
194 4 : EXPECT_EQ (pinchCasted->getnFingersInvolved(), pinchMap.at(keyPair).getnFingersInvolved() );
195 4 : EXPECT_EQ (pinchCasted->getMaxStoredActionStates(), pinchMap.at(keyPair).getMaxStoredActionStates());
196 4 : EXPECT_EQ (pinchCasted->getPrimitiveType(), pinchMap.at(keyPair).getPrimitiveType() );
197 8 : EXPECT_EQ (pinchCasted->getFingersInvolved(), pinchMap.at(keyPair).getFingersInvolved());
198 :
199 4 : unsigned int i = 0;
200 16 : for (auto as: pinchCasted->getActionStates() ) {
201 :
202 : //check equality of joint states (as.first)
203 105 : for (auto joint : as.first) {
204 93 : ASSERT_EQ ( joint.second.size(),
205 : pinchMap.at(keyPair).getAllJointPos().at(i).at(joint.first).size() );
206 : //loop the eventually multiple joint pos (when dofs > 1)
207 186 : for (int j=0; j<joint.second.size(); ++j){
208 186 : EXPECT_DOUBLE_EQ ( joint.second.at(j),
209 : pinchMap.at(keyPair).getAllJointPos().at(i).at(joint.first).at(j) );
210 : }
211 : }
212 :
213 : //check equality of contact (as.second)
214 24 : collision_detection::Contact thisCont = as.second;
215 : collision_detection::Contact otherCont =
216 12 : pinchMap.at(keyPair).getActionStates().at(i).second;
217 : // Tricky here, body colliding names can be swapped.
218 : // BUT it seems that depth, normal and pos refer to a fixed something, so they must
219 : // not be swapped (I don't see that EXPECT fails if I don't swap them when names are swapped)
220 12 : if (thisCont.body_name_1 > thisCont.body_name_2) {
221 3 : std::swap(thisCont.body_name_1, thisCont.body_name_2);
222 3 : std::swap(thisCont.body_type_1, thisCont.body_type_2);
223 : }
224 12 : if (otherCont.body_name_1 > otherCont.body_name_2) {
225 3 : std::swap(otherCont.body_name_1, otherCont.body_name_2);
226 3 : std::swap(otherCont.body_type_1, otherCont.body_type_2);
227 :
228 : }
229 12 : EXPECT_EQ (thisCont.body_name_1, otherCont.body_name_1 );
230 12 : EXPECT_EQ (thisCont.body_name_2, otherCont.body_name_2 );
231 12 : EXPECT_EQ (thisCont.body_type_1, otherCont.body_type_1 );
232 12 : EXPECT_EQ (thisCont.body_type_2, otherCont.body_type_2 );
233 12 : EXPECT_NEAR (thisCont.depth, otherCont.depth, 0.00001);
234 12 : EXPECT_NEAR (thisCont.pos.x(), otherCont.pos.x(), 0.00001);
235 12 : EXPECT_NEAR (thisCont.pos.y(), otherCont.pos.y(), 0.00001);
236 12 : EXPECT_NEAR (thisCont.pos.z(), otherCont.pos.z(), 0.00001);
237 12 : EXPECT_NEAR (thisCont.normal.x(), otherCont.normal.x(), 0.00001);
238 12 : EXPECT_NEAR (thisCont.normal.y(), otherCont.normal.y(), 0.00001);
239 12 : EXPECT_NEAR (thisCont.normal.z(), otherCont.normal.z(), 0.00001);
240 :
241 : // TODO HOW TO print this once and only if any of the expect at this loop iteration fails?
242 12 : if ( (std::abs(thisCont.depth - otherCont.depth)) > 0.00001) {
243 0 : std::cout << "EXPECT equal depths fails: error is on the actionState_" << i << std::endl;
244 0 : pinchCasted->print();
245 0 : pinchMap.at(keyPair).print();
246 0 : std::cout << std::endl;
247 : }
248 :
249 12 : i++;
250 : }
251 : }
252 : }
253 :
254 :
255 : ////********************************* LOOSE PINCH TESTS *********************************************************
256 8 : TEST_F ( testFindPinches, checkNumberLinksLoose ) {
257 :
258 4 : SetUp(argc_g, argv_g);
259 :
260 5 : for (auto &mapEl: pinchLooseMap ) {
261 :
262 : //being a pair the .first has always dimension 2
263 1 : EXPECT_EQ (2, mapEl.second.getFingersInvolved().size() ); //the names inside the action
264 1 : EXPECT_EQ (2, mapEl.second.getnFingersInvolved() ); //the int nLinkInvolved member of action
265 : }
266 :
267 5 : for (auto &mapEl: pinchLooseParsedMap ) {
268 :
269 1 : EXPECT_EQ (2, mapEl.first.size()); // the key
270 1 : EXPECT_EQ (2, mapEl.second->getFingersInvolved().size()); //the names inside the action
271 1 : EXPECT_EQ (2, mapEl.second->getnFingersInvolved()); //the int nLinkInvolved member of action
272 : }
273 4 : }
274 :
275 8 : TEST_F ( testFindPinches, checkSizeStatesInfoSetLoose ) {
276 :
277 4 : SetUp(argc_g, argv_g);
278 :
279 5 : for (auto &mapEl: pinchLooseMap ) {
280 :
281 : //get the member which is set in costructor
282 1 : unsigned int size = mapEl.second.getMaxStoredActionStates();
283 :
284 : //it must be equal to the real size of the action state set
285 1 : EXPECT_EQ ( size, mapEl.second.getActionStates().size() );
286 1 : EXPECT_EQ ( size, mapEl.second.getAllJointPos().size() );
287 : }
288 :
289 5 : for (auto &mapEl: pinchLooseParsedMap ) {
290 :
291 : //get the member which is set in costructor
292 1 : unsigned int size = mapEl.second->getMaxStoredActionStates();
293 :
294 1 : EXPECT_EQ (size, mapEl.second->getAllJointPos().size());
295 : }
296 4 : }
297 :
298 8 : TEST_F ( testFindPinches, checkNameLoose ) {
299 :
300 4 : SetUp(argc_g, argv_g);
301 :
302 5 : for (auto &mapEl: pinchLooseMap ) {
303 :
304 1 : EXPECT_TRUE (mapEl.second.getName().compare("pinchLoose") == 0);
305 1 : EXPECT_EQ (ROSEE::ActionPrimitive::Type::PinchLoose, mapEl.second.getPrimitiveType() );
306 : }
307 :
308 5 : for (auto &mapEl: pinchLooseParsedMap ) {
309 :
310 1 : EXPECT_TRUE (mapEl.second->getName().compare("pinchLoose") == 0);
311 1 : EXPECT_EQ (ROSEE::ActionPrimitive::Type::PinchLoose, mapEl.second->getPrimitiveType() );
312 : }
313 4 : }
314 :
315 : //this is an important test: check if the order of statesInfo in right according to distance
316 8 : TEST_F ( testFindPinches, checkOrderStatesInfoSetLoose ) {
317 :
318 4 : SetUp(argc_g, argv_g);
319 :
320 5 : for (auto &mapEl: pinchLooseMap ) {
321 :
322 : std::vector < ROSEE::ActionPinchLoose::StateWithDistance> statesInfo =
323 2 : mapEl.second.getActionStates();
324 :
325 1 : double oldDist = 0;
326 4 : for (auto &setEl : statesInfo) {
327 3 : EXPECT_GE ( setEl.second, oldDist ); //greater or equal
328 3 : oldDist = setEl.second;
329 : }
330 : }
331 :
332 :
333 5 : for (auto &mapEl: pinchLooseParsedMap ) {
334 :
335 : std::shared_ptr <ROSEE::ActionPinchLoose> pinchLooseCasted =
336 1 : std::dynamic_pointer_cast < ROSEE::ActionPinchLoose > (mapEl.second);
337 :
338 1 : ASSERT_FALSE (pinchLooseCasted == nullptr);
339 : std::vector < ROSEE::ActionPinchLoose::StateWithDistance> statesInfo =
340 2 : pinchLooseCasted->getActionStates();
341 :
342 1 : double oldDist = 0;
343 4 : for (auto &setEl : statesInfo) {
344 3 : EXPECT_GE ( setEl.second, oldDist ); //greater or equal
345 3 : oldDist = setEl.second;
346 : }
347 : }
348 : }
349 :
350 : // to check if the found map is the same map that is emitted in the file and then parsed
351 8 : TEST_F ( testFindPinches, checkEmitParseLoose ) {
352 :
353 4 : SetUp(argc_g, argv_g);
354 :
355 4 : ASSERT_EQ (pinchLooseMap.size(), pinchLooseParsedMap.size() );
356 :
357 5 : for (auto &mapEl: pinchLooseParsedMap ) {
358 :
359 : std::shared_ptr <ROSEE::ActionPinchLoose> pinchCasted =
360 1 : std::dynamic_pointer_cast < ROSEE::ActionPinchLoose > (mapEl.second);
361 :
362 1 : ASSERT_FALSE (pinchCasted == nullptr);
363 1 : ASSERT_EQ (2, mapEl.first.size() ); // the key set must have dim 2 (the tip pair)
364 1 : std::pair <std::string, std::string> keyPair;
365 1 : std::set<std::string>::iterator it = mapEl.first.begin();
366 1 : keyPair.first = *it;
367 1 : std::advance ( it, 1 );
368 1 : keyPair.second = *it;
369 :
370 : //std::string is ok to compare with _EQ
371 2 : EXPECT_EQ (pinchCasted->getName(), pinchLooseMap.at(keyPair).getName() );
372 1 : EXPECT_EQ (pinchCasted->getnFingersInvolved(), pinchLooseMap.at(keyPair).getnFingersInvolved() );
373 1 : EXPECT_EQ (pinchCasted->getMaxStoredActionStates(), pinchLooseMap.at(keyPair).getMaxStoredActionStates());
374 1 : EXPECT_EQ (pinchCasted->getPrimitiveType(), pinchLooseMap.at(keyPair).getPrimitiveType() );
375 2 : EXPECT_EQ (pinchCasted->getFingersInvolved(), pinchLooseMap.at(keyPair).getFingersInvolved());
376 :
377 1 : unsigned int i = 0;
378 4 : for (auto as: pinchCasted->getActionStates() ) {
379 :
380 : //check equality of joint states (as.first)
381 6 : for (auto joint : as.first) {
382 3 : ASSERT_EQ ( joint.second.size(),
383 : pinchLooseMap.at(keyPair).getAllJointPos().at(i).at(joint.first).size() );
384 : //loop the eventually multiple joint pos (when dofs > 1)
385 6 : for (int j=0; j<joint.second.size(); ++j){
386 6 : EXPECT_DOUBLE_EQ ( joint.second.at(j),
387 : pinchLooseMap.at(keyPair).getAllJointPos().at(i).at(joint.first).at(j) );
388 : }
389 : }
390 :
391 : //check equality of distance (as.second)
392 6 : EXPECT_DOUBLE_EQ (as.second, pinchLooseMap.at(keyPair).getActionStates().at(i).second);
393 :
394 3 : i++;
395 : }
396 : }
397 : }
398 :
399 :
400 : //A tight pinch cant be a loose pinch and viceversa
401 : //here we check if all entries of one map are not present in the other (and viceversa)
402 : //we check only the not parsed maps, other tests are done for correctness of parsing (checkEmitParse)
403 8 : TEST_F ( testFindPinches, checkTightLooseExclusion ) {
404 :
405 4 : SetUp(argc_g, argv_g);
406 :
407 8 : for (auto mapEl : pinchMap ) {
408 4 : EXPECT_EQ (0, pinchLooseMap.count(mapEl.first)) << mapEl.first.first << ", " << mapEl.first.second
409 0 : << " " << " is also present in Loose Pinches" << std::endl;
410 : }
411 :
412 5 : for (auto mapEl : pinchLooseMap ) {
413 1 : EXPECT_EQ (0, pinchMap.count(mapEl.first)) << mapEl.first.first << ", " << mapEl.first.second
414 0 : << " " << " is also present in Tight Pinches" << std::endl;
415 : }
416 :
417 4 : }
418 :
419 :
420 :
421 : } //namespace
422 :
423 4 : int main ( int argc, char **argv ) {
424 :
425 4 : if (argc < 2 ){
426 :
427 0 : std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
428 0 : return -1;
429 : }
430 :
431 4 : rclcpp::init ( argc, argv );
432 :
433 4 : ::testing::InitGoogleTest ( &argc, argv );
434 4 : ::testing::AddGlobalTestEnvironment(new MyTestEnvironment(argc, argv));
435 :
436 4 : return RUN_ALL_TESTS();
437 : }
|
