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