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