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/ActionTrig.h>
10 :
11 :
12 : namespace {
13 :
14 : class testFindTrigs: public ::testing::Test {
15 :
16 :
17 : protected:
18 :
19 28 : testFindTrigs() {
20 28 : }
21 :
22 28 : virtual ~testFindTrigs() {
23 28 : }
24 :
25 28 : virtual void SetUp() override {
26 :
27 :
28 56 : 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 28 : ASSERT_TRUE(parserMoveIt->init ("robot_description", false)) ;
31 56 : ROSEE::FindActions actionsFinder (parserMoveIt);
32 :
33 56 : std::string folderForActions = ROSEE::Utils::getPackagePath() + "/configs/actions/tests/" + parserMoveIt->getHandName();
34 :
35 28 : ROSEE::YamlWorker yamlWorker;
36 :
37 56 : auto trig = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::Trig, folderForActions + "/primitives/");
38 28 : if (trig.size() > 0) {
39 :
40 21 : trigMap.push_back( trig );
41 21 : trigParsedMap.push_back( yamlWorker.parseYamlPrimitive ( folderForActions + "/primitives/" + "trig.yaml" ) );
42 : }
43 :
44 56 : auto tipFlex = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::TipFlex, folderForActions + "/primitives/");
45 28 : if (tipFlex.size() > 0) {
46 :
47 21 : trigMap.push_back( tipFlex );
48 21 : trigParsedMap.push_back( yamlWorker.parseYamlPrimitive ( folderForActions + "/primitives/" + "tipFlex.yaml" ) );
49 : }
50 :
51 56 : auto fingFlex = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::FingFlex, folderForActions + "/primitives/");
52 28 : if (fingFlex.size() > 0) {
53 :
54 21 : trigMap.push_back( fingFlex );
55 21 : trigParsedMap.push_back( yamlWorker.parseYamlPrimitive ( folderForActions + "/primitives/" + "fingFlex.yaml" ) );
56 :
57 : }
58 : }
59 :
60 28 : virtual void TearDown() override {
61 28 : }
62 :
63 : std::vector < std::map < std::string , ROSEE::ActionTrig > > trigMap;
64 : std::vector < std::map < std::set < std::string >, std::shared_ptr<ROSEE::ActionPrimitive> > > trigParsedMap;
65 : };
66 :
67 :
68 20 : TEST_F ( testFindTrigs, checkNumberLinks ) {
69 :
70 13 : for (int k = 0; k< trigMap.size(); ++k) {
71 33 : for (auto &mapEl: trigMap.at(k) ) {
72 :
73 : //the .first has always dimension 1
74 24 : EXPECT_EQ (1, mapEl.second.getFingersInvolved().size() ); //the names inside the action
75 24 : EXPECT_EQ (1, mapEl.second.getnFingersInvolved() ); //the int nLinkInvolved member of action
76 : }
77 :
78 33 : for (auto &mapEl: trigParsedMap.at(k) ) {
79 :
80 24 : EXPECT_EQ (1, mapEl.first.size()); // the key
81 24 : EXPECT_EQ (1, mapEl.second->getFingersInvolved().size()); //the names inside the action
82 24 : EXPECT_EQ (1, mapEl.second->getnFingersInvolved()); //the int nLinkInvolved member of action
83 : }
84 : }
85 4 : }
86 :
87 20 : TEST_F ( testFindTrigs, checkSizeStatesInfoSet ) {
88 :
89 13 : for (int k = 0; k< trigMap.size(); ++k) {
90 :
91 33 : for (auto &mapEl: trigMap.at(k) ) {
92 :
93 : //get the member which is set in costructor
94 24 : unsigned int size = mapEl.second.getMaxStoredActionStates();
95 :
96 : //it must be equal to the real size of the statesInfoSet
97 24 : EXPECT_EQ ( size, mapEl.second.getAllJointPos().size() );
98 : }
99 :
100 33 : for (auto &mapEl: trigParsedMap.at(k) ) {
101 :
102 : //get the member which is set in costructor
103 24 : unsigned int size = mapEl.second->getMaxStoredActionStates();
104 :
105 : //it must be equal to the real size of the statesInfoSet
106 24 : EXPECT_EQ (size, mapEl.second->getAllJointPos().size());
107 : }
108 : }
109 4 : }
110 :
111 20 : TEST_F ( testFindTrigs, checkNameTypeConsistency ) {
112 :
113 13 : for (int k = 0; k < trigMap.size(); ++k) {
114 :
115 9 : ROSEE::ActionPrimitive::Type actionType = trigMap.at(k).begin()->second.getPrimitiveType();
116 :
117 33 : for (auto &mapEl: trigMap.at(k) ) {
118 :
119 24 : EXPECT_EQ (actionType, mapEl.second.getPrimitiveType() ); //in the map all el must be of same ActionType
120 :
121 24 : switch (mapEl.second.getPrimitiveType()) {
122 8 : case ROSEE::ActionPrimitive::Type::Trig :
123 8 : EXPECT_EQ (mapEl.second.getName(), "trig");
124 8 : break;
125 8 : case ROSEE::ActionPrimitive::Type::TipFlex :
126 8 : EXPECT_EQ (mapEl.second.getName(), "tipFlex");
127 8 : break;
128 8 : case ROSEE::ActionPrimitive::Type::FingFlex :
129 8 : EXPECT_EQ (mapEl.second.getName(), "fingFlex");
130 8 : break;
131 0 : default:
132 0 : FAIL() << mapEl.second.getPrimitiveType() << " not a know type" << std::endl ;
133 : }
134 : }
135 :
136 18 : actionType = trigParsedMap.at(k).begin()->second->getPrimitiveType();
137 33 : for (auto &mapEl: trigParsedMap.at(k) ) {
138 :
139 24 : EXPECT_EQ (actionType, mapEl.second->getPrimitiveType() ); //in the map all el must be of same ActionType
140 :
141 24 : switch (mapEl.second->getPrimitiveType()) {
142 8 : case ROSEE::ActionPrimitive::Type::Trig :
143 8 : EXPECT_EQ (mapEl.second->getName(), "trig");
144 8 : break;
145 8 : case ROSEE::ActionPrimitive::Type::TipFlex :
146 8 : EXPECT_EQ (mapEl.second->getName(), "tipFlex");
147 8 : break;
148 8 : case ROSEE::ActionPrimitive::Type::FingFlex :
149 8 : EXPECT_EQ (mapEl.second->getName(), "fingFlex");
150 8 : break;
151 0 : default:
152 0 : FAIL() << mapEl.second->getPrimitiveType() << " not a know type" << std::endl ;
153 : }
154 : }
155 : }
156 : }
157 :
158 : // to check if the found map is the same map that is emitted in the file and then parsed
159 20 : TEST_F ( testFindTrigs, checkEmitParse ) {
160 :
161 13 : for (int k = 0; k< trigMap.size(); ++k) {
162 :
163 9 : ASSERT_EQ (trigMap.at(k).size(), trigParsedMap.at(k).size() );
164 :
165 33 : for (auto &mapEl: trigParsedMap.at(k) ) {
166 :
167 : std::shared_ptr <ROSEE::ActionTrig> trigCasted =
168 48 : std::dynamic_pointer_cast < ROSEE::ActionTrig > (mapEl.second);
169 :
170 24 : ASSERT_FALSE (trigCasted == nullptr);
171 24 : ASSERT_EQ (1, mapEl.first.size() );
172 48 : std::string key;
173 24 : std::set<std::string>::iterator it = mapEl.first.begin();
174 24 : key = *it;
175 :
176 : //std::string is ok to compare with _EQ
177 24 : EXPECT_EQ (trigCasted->getName(), trigMap.at(k).at(key).getName() );
178 24 : EXPECT_EQ (trigCasted->getType(), trigMap.at(k).at(key).getType() );
179 24 : EXPECT_EQ (trigCasted->getnFingersInvolved(), trigMap.at(k).at(key).getnFingersInvolved() );
180 24 : EXPECT_EQ (trigCasted->getMaxStoredActionStates(), trigMap.at(k).at(key).getMaxStoredActionStates());
181 24 : EXPECT_EQ (trigCasted->getPrimitiveType(), trigMap.at(k).at(key).getPrimitiveType() );
182 24 : EXPECT_EQ (trigCasted->getFingersInvolved(), trigMap.at(k).at(key).getFingersInvolved());
183 24 : EXPECT_EQ (trigCasted->getJointsInvolvedCount(), trigMap.at(k).at(key).getJointsInvolvedCount());
184 :
185 48 : for (auto jointStates: trigCasted->getAllJointPos() ) {
186 :
187 : //loop the map "jointStates"
188 183 : for (auto joint : jointStates) {
189 159 : ASSERT_EQ ( joint.second.size(), trigMap.at(k).at(key).getJointPos().at(joint.first).size() );
190 : //loop the eventually multiple joint pos (when dofs > 1)
191 318 : for (int j=0; j<joint.second.size(); ++j){
192 159 : EXPECT_DOUBLE_EQ ( joint.second.at(j),
193 : trigMap.at(k).at(key).getJointPos().at(joint.first).at(j) );
194 : }
195 : }
196 :
197 : }
198 : }
199 : }
200 : }
201 :
202 : /** TipFlex and FingerFlex, for definition, must have the unique setted joints that are different joints
203 : *
204 : * @NOTE : These consideration are valid because we send the joint in the biggest bound, so a 0 position always
205 : * means that is a "not setted" joint.
206 : * @WARNING what happens if the define in findAction.h DEFAULT_JOINT_POS 0.0 changes
207 : */
208 20 : TEST_F ( testFindTrigs, checkJointPosTipAndFing ) {
209 :
210 11 : if (trigMap.size() != 0 &&
211 6 : trigMap.at(0).size() != 0 &&
212 10 : trigMap.at(1).size() != 0 &&
213 3 : trigMap.at(2).size() != 0 )
214 : {
215 : // we assume the order in trigmap : 0 = trig, 1 = tipflex, 2 = fingflex
216 : // otherwise we have to check which one is what that is useless
217 3 : ASSERT_EQ ( trigMap.at(0).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::Trig);
218 3 : ASSERT_EQ ( trigMap.at(1).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::TipFlex);
219 3 : ASSERT_EQ ( trigMap.at(2).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::FingFlex);
220 :
221 : //compare tip and fing flex
222 11 : for (auto &mapTipEl: trigMap.at(1) ) {
223 :
224 16 : ROSEE::JointPos tipJs = mapTipEl.second.getJointPos();
225 :
226 30 : for (auto &mapFingEl : trigMap.at(2) ) {
227 :
228 44 : ROSEE::JointPos fingJs = mapFingEl.second.getJointPos();
229 22 : ASSERT_EQ ( tipJs.size(), fingJs.size() );
230 :
231 173 : for (auto tipJoint: tipJs) {
232 :
233 : //at(0): 1dof joint
234 151 : if (tipJoint.second.at(0) != 0.0) {
235 : //if so, it is the setted joint, and the correspondent of fingerAction must be zero
236 22 : EXPECT_EQ ( fingJs.at(tipJoint.first).at(0), 0.0);
237 : }
238 : }
239 : }
240 : }
241 : }
242 : }
243 :
244 : /** If a tipFlex is present, the unique setted joint must be also setted in the trig action (taking the same
245 : * fingertip), because with trig we move ALL the joint on the finger. (so the opposite can be not true).
246 : *
247 : * (third for loop) If some joint is not setted in the trig, it must be also non setted in
248 : * the tipAction of the same fingertip
249 : *
250 : * Similar consideration exists for fingFlex
251 : *
252 : * @NOTE : These consideration are valid because we send the joint in the biggest bound, so a 0 position always
253 : * means that is a "not setted" joint.
254 : * @WARNING what happens if the define in findAction.h DEFAULT_JOINT_POS 0.0 changes
255 : * @WARNING the third for must be changed if for trig another key (like the finger group name) is used instead
256 : * of the tip
257 : */
258 20 : TEST_F ( testFindTrigs, checkJointPosFlexsAndTrig ) {
259 :
260 :
261 11 : if (trigMap.size() != 0 &&
262 6 : trigMap.at(0).size() != 0 &&
263 10 : trigMap.at(1).size() != 0 &&
264 3 : trigMap.at(2).size() != 0 )
265 : {
266 : // we assume the order in trigmap : 0 = trig, 1 = tipflex, 2 = fingflex
267 : // otherwise we have to check which one is what that is useless
268 3 : ASSERT_EQ ( trigMap.at(0).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::Trig);
269 3 : ASSERT_EQ ( trigMap.at(1).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::TipFlex);
270 3 : ASSERT_EQ ( trigMap.at(2).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::FingFlex);
271 :
272 :
273 : // If a tipFlex is present, the unique setted joint must be also setted (equal pos) in the trig action
274 11 : for (auto &mapTipEl: trigMap.at(1) ) {
275 61 : for ( auto &tipJs : mapTipEl.second.getJointPos() ) {
276 53 : if (tipJs.second.at(0) != 0 ) {
277 : //if a tipFlex exist for a tip, also a trig for that tip exist
278 8 : EXPECT_TRUE (trigMap.at(0).find ( mapTipEl.first ) != trigMap.at(0).end());
279 8 : EXPECT_DOUBLE_EQ ( tipJs.second.at(0),
280 : trigMap.at(0).at(mapTipEl.first).getJointPos().at(tipJs.first).at(0) );
281 : }
282 :
283 : }
284 : }
285 :
286 : // If a FingFlex is present, the unique setted joint must be also setted (equal pos) in the trig action
287 11 : for (auto &mapFingEl: trigMap.at(2) ) {
288 61 : for ( auto &fingJs : mapFingEl.second.getJointPos() ) {
289 53 : if (fingJs.second.at(0) != 0 ) {
290 : //if a fingFlex exist for a tip, also a trig for that tip exist
291 8 : EXPECT_TRUE ( trigMap.at(0).find ( mapFingEl.first ) != trigMap.at(0).end() );
292 8 : EXPECT_DOUBLE_EQ ( fingJs.second.at(0),
293 : trigMap.at(0).at(mapFingEl.first).getJointPos().at(fingJs.first).at(0) );
294 : }
295 : }
296 : }
297 :
298 : // If some joint is not set in the trig, it must be also non setted in
299 : // the tipAction of the same fingertip
300 11 : for (auto &mapTrigEl: trigMap.at(0) ) {
301 61 : for ( auto &trigJs : mapTrigEl.second.getJointPos() ) {
302 53 : if (trigJs.second.at(0) == 0.0 ) {
303 :
304 : // if a trig exist, it is not assured that a tip flex exist for that tip
305 34 : if (trigMap.at(1).find ( mapTrigEl.first ) != trigMap.at(1).end()) {
306 34 : EXPECT_EQ ( 0.0,
307 : trigMap.at(1).at(mapTrigEl.first).getJointPos().at(trigJs.first).at(0) );
308 : }
309 :
310 : // if a trig exist, it is not assured that a fing flex exist for that tip
311 34 : if (trigMap.at(2).find ( mapTrigEl.first ) != trigMap.at(2).end()) {
312 34 : EXPECT_EQ ( 0.0,
313 : trigMap.at(2).at(mapTrigEl.first).getJointPos().at(trigJs.first).at(0) );
314 : }
315 : }
316 : }
317 : }
318 : }
319 : }
320 :
321 :
322 : /** For the tip and fing flex action, for definition, only one joint must be set (ie position != 0).
323 : */
324 20 : TEST_F ( testFindTrigs, checkFlexsSingleJoint ) {
325 :
326 13 : for (int k = 0; k< trigMap.size(); ++k) {
327 :
328 9 : if ( trigMap.at(k).begin()->second.getPrimitiveType() == ROSEE::ActionPrimitive::Type::Trig ) {
329 3 : continue;
330 : }
331 :
332 22 : for (auto &mapFlexEl: trigMap.at(k) ) {
333 :
334 16 : unsigned int nJSet = 0;
335 :
336 122 : for ( auto &flexJs : mapFlexEl.second.getJointPos() ) { //iteration over the jointstates map
337 :
338 106 : if ( flexJs.second.at(0) != 0.0 ) {
339 16 : nJSet++;
340 : }
341 : }
342 16 : EXPECT_EQ (1, nJSet);
343 :
344 : //test also the jointInvolvedBool vector
345 16 : unsigned int jointsInvolvedSum = 0;
346 122 : for ( auto flexJs : mapFlexEl.second.getJointsInvolvedCount() ) { //iteration over the jointstates map
347 :
348 106 : jointsInvolvedSum += flexJs.second;
349 : }
350 16 : EXPECT_EQ (1, jointsInvolvedSum);
351 :
352 : }
353 : }
354 4 : }
355 :
356 :
357 : } //namespace
358 :
359 4 : int main ( int argc, char **argv ) {
360 :
361 4 : if (argc < 2 ){
362 :
363 0 : std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
364 0 : return -1;
365 : }
366 :
367 : /* Run tests on an isolated roscore */
368 4 : if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
369 : {
370 0 : perror("setenv");
371 0 : return 1;
372 : }
373 :
374 : //run roscore
375 8 : std::unique_ptr<ROSEE::TestUtils::Process> roscore;
376 4 : roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));
377 :
378 4 : if ( ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testFindTrigs" ) != 0 ) {
379 :
380 0 : std::cout << "[TEST ERROR] Prepare Funcion failed" << std::endl;
381 0 : return -1;
382 : }
383 :
384 :
385 4 : ::testing::InitGoogleTest ( &argc, argv );
386 4 : return RUN_ALL_TESTS();
387 12 : }
|
