test_find_trigs.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include "testUtils.h"
3 
4 #include <ros/ros.h>
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  testFindTrigs() {
20  }
21 
22  virtual ~testFindTrigs() {
23  }
24 
25  virtual void SetUp() override {
26 
27 
28  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  ASSERT_TRUE(parserMoveIt->init ("robot_description", false)) ;
31  ROSEE::FindActions actionsFinder (parserMoveIt);
32 
33  std::string folderForActions = "ROSEE/actions/" + parserMoveIt->getHandName();
34 
35  ROSEE::YamlWorker yamlWorker;
36 
37  auto trig = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::Trig, folderForActions + "/primitives/");
38  if (trig.size() > 0) {
39 
40  trigMap.push_back( trig );
41  trigParsedMap.push_back( yamlWorker.parseYamlPrimitive ( folderForActions + "/primitives/" + "trig.yaml" ) );
42  }
43 
44  auto tipFlex = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::TipFlex, folderForActions + "/primitives/");
45  if (tipFlex.size() > 0) {
46 
47  trigMap.push_back( tipFlex );
48  trigParsedMap.push_back( yamlWorker.parseYamlPrimitive ( folderForActions + "/primitives/" + "tipFlex.yaml" ) );
49  }
50 
51  auto fingFlex = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::FingFlex, folderForActions + "/primitives/");
52  if (fingFlex.size() > 0) {
53 
54  trigMap.push_back( fingFlex );
55  trigParsedMap.push_back( yamlWorker.parseYamlPrimitive ( folderForActions + "/primitives/" + "fingFlex.yaml" ) );
56 
57  }
58  }
59 
60  virtual void TearDown() override {
61  }
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 TEST_F ( testFindTrigs, checkNumberLinks ) {
69 
70  for (int k = 0; k< trigMap.size(); ++k) {
71  for (auto &mapEl: trigMap.at(k) ) {
72 
73  //the .first has always dimension 1
74  EXPECT_EQ (1, mapEl.second.getFingersInvolved().size() ); //the names inside the action
75  EXPECT_EQ (1, mapEl.second.getnFingersInvolved() ); //the int nLinkInvolved member of action
76  }
77 
78  for (auto &mapEl: trigParsedMap.at(k) ) {
79 
80  EXPECT_EQ (1, mapEl.first.size()); // the key
81  EXPECT_EQ (1, mapEl.second->getFingersInvolved().size()); //the names inside the action
82  EXPECT_EQ (1, mapEl.second->getnFingersInvolved()); //the int nLinkInvolved member of action
83  }
84  }
85 }
86 
87 TEST_F ( testFindTrigs, checkSizeStatesInfoSet ) {
88 
89  for (int k = 0; k< trigMap.size(); ++k) {
90 
91  for (auto &mapEl: trigMap.at(k) ) {
92 
93  //get the member which is set in costructor
94  unsigned int size = mapEl.second.getMaxStoredActionStates();
95 
96  //it must be equal to the real size of the statesInfoSet
97  EXPECT_EQ ( size, mapEl.second.getAllJointPos().size() );
98  }
99 
100  for (auto &mapEl: trigParsedMap.at(k) ) {
101 
102  //get the member which is set in costructor
103  unsigned int size = mapEl.second->getMaxStoredActionStates();
104 
105  //it must be equal to the real size of the statesInfoSet
106  EXPECT_EQ (size, mapEl.second->getAllJointPos().size());
107  }
108  }
109 }
110 
111 TEST_F ( testFindTrigs, checkNameTypeConsistency ) {
112 
113  for (int k = 0; k < trigMap.size(); ++k) {
114 
115  ROSEE::ActionPrimitive::Type actionType = trigMap.at(k).begin()->second.getPrimitiveType();
116 
117  for (auto &mapEl: trigMap.at(k) ) {
118 
119  EXPECT_EQ (actionType, mapEl.second.getPrimitiveType() ); //in the map all el must be of same ActionType
120 
121  switch (mapEl.second.getPrimitiveType()) {
122  case ROSEE::ActionPrimitive::Type::Trig :
123  EXPECT_EQ (mapEl.second.getName(), "trig");
124  break;
125  case ROSEE::ActionPrimitive::Type::TipFlex :
126  EXPECT_EQ (mapEl.second.getName(), "tipFlex");
127  break;
128  case ROSEE::ActionPrimitive::Type::FingFlex :
129  EXPECT_EQ (mapEl.second.getName(), "fingFlex");
130  break;
131  default:
132  FAIL() << mapEl.second.getPrimitiveType() << " not a know type" << std::endl ;
133  }
134  }
135 
136  actionType = trigParsedMap.at(k).begin()->second->getPrimitiveType();
137  for (auto &mapEl: trigParsedMap.at(k) ) {
138 
139  EXPECT_EQ (actionType, mapEl.second->getPrimitiveType() ); //in the map all el must be of same ActionType
140 
141  switch (mapEl.second->getPrimitiveType()) {
142  case ROSEE::ActionPrimitive::Type::Trig :
143  EXPECT_EQ (mapEl.second->getName(), "trig");
144  break;
145  case ROSEE::ActionPrimitive::Type::TipFlex :
146  EXPECT_EQ (mapEl.second->getName(), "tipFlex");
147  break;
148  case ROSEE::ActionPrimitive::Type::FingFlex :
149  EXPECT_EQ (mapEl.second->getName(), "fingFlex");
150  break;
151  default:
152  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 TEST_F ( testFindTrigs, checkEmitParse ) {
160 
161  for (int k = 0; k< trigMap.size(); ++k) {
162 
163  ASSERT_EQ (trigMap.at(k).size(), trigParsedMap.at(k).size() );
164 
165  for (auto &mapEl: trigParsedMap.at(k) ) {
166 
167  std::shared_ptr <ROSEE::ActionTrig> trigCasted =
168  std::dynamic_pointer_cast < ROSEE::ActionTrig > (mapEl.second);
169 
170  ASSERT_FALSE (trigCasted == nullptr);
171  ASSERT_EQ (1, mapEl.first.size() );
172  std::string key;
173  std::set<std::string>::iterator it = mapEl.first.begin();
174  key = *it;
175 
176  //std::string is ok to compare with _EQ
177  EXPECT_EQ (trigCasted->getName(), trigMap.at(k).at(key).getName() );
178  EXPECT_EQ (trigCasted->getType(), trigMap.at(k).at(key).getType() );
179  EXPECT_EQ (trigCasted->getnFingersInvolved(), trigMap.at(k).at(key).getnFingersInvolved() );
180  EXPECT_EQ (trigCasted->getMaxStoredActionStates(), trigMap.at(k).at(key).getMaxStoredActionStates());
181  EXPECT_EQ (trigCasted->getPrimitiveType(), trigMap.at(k).at(key).getPrimitiveType() );
182  EXPECT_EQ (trigCasted->getFingersInvolved(), trigMap.at(k).at(key).getFingersInvolved());
183  EXPECT_EQ (trigCasted->getJointsInvolvedCount(), trigMap.at(k).at(key).getJointsInvolvedCount());
184 
185  for (auto jointStates: trigCasted->getAllJointPos() ) {
186 
187  //loop the map "jointStates"
188  for (auto joint : jointStates) {
189  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  for (int j=0; j<joint.second.size(); ++j){
192  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 
208 TEST_F ( testFindTrigs, checkJointPosTipAndFing ) {
209 
210  if (trigMap.size() != 0 &&
211  trigMap.at(0).size() != 0 &&
212  trigMap.at(1).size() != 0 &&
213  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  ASSERT_EQ ( trigMap.at(0).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::Trig);
218  ASSERT_EQ ( trigMap.at(1).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::TipFlex);
219  ASSERT_EQ ( trigMap.at(2).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::FingFlex);
220 
221  //compare tip and fing flex
222  for (auto &mapTipEl: trigMap.at(1) ) {
223 
224  ROSEE::JointPos tipJs = mapTipEl.second.getJointPos();
225 
226  for (auto &mapFingEl : trigMap.at(2) ) {
227 
228  ROSEE::JointPos fingJs = mapFingEl.second.getJointPos();
229  ASSERT_EQ ( tipJs.size(), fingJs.size() );
230 
231  for (auto tipJoint: tipJs) {
232 
233  //at(0): 1dof joint
234  if (tipJoint.second.at(0) != 0.0) {
235  //if so, it is the setted joint, and the correspondent of fingerAction must be zero
236  EXPECT_EQ ( fingJs.at(tipJoint.first).at(0), 0.0);
237  }
238  }
239  }
240  }
241  }
242 }
243 
258 TEST_F ( testFindTrigs, checkJointPosFlexsAndTrig ) {
259 
260 
261  if (trigMap.size() != 0 &&
262  trigMap.at(0).size() != 0 &&
263  trigMap.at(1).size() != 0 &&
264  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  ASSERT_EQ ( trigMap.at(0).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::Trig);
269  ASSERT_EQ ( trigMap.at(1).begin()->second.getPrimitiveType(), ROSEE::ActionPrimitive::Type::TipFlex);
270  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  for (auto &mapTipEl: trigMap.at(1) ) {
275  for ( auto &tipJs : mapTipEl.second.getJointPos() ) {
276  if (tipJs.second.at(0) != 0 ) {
277  //if a tipFlex exist for a tip, also a trig for that tip exist
278  EXPECT_TRUE (trigMap.at(0).find ( mapTipEl.first ) != trigMap.at(0).end());
279  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  for (auto &mapFingEl: trigMap.at(2) ) {
288  for ( auto &fingJs : mapFingEl.second.getJointPos() ) {
289  if (fingJs.second.at(0) != 0 ) {
290  //if a fingFlex exist for a tip, also a trig for that tip exist
291  EXPECT_TRUE ( trigMap.at(0).find ( mapFingEl.first ) != trigMap.at(0).end() );
292  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  for (auto &mapTrigEl: trigMap.at(0) ) {
301  for ( auto &trigJs : mapTrigEl.second.getJointPos() ) {
302  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  if (trigMap.at(1).find ( mapTrigEl.first ) != trigMap.at(1).end()) {
306  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  if (trigMap.at(2).find ( mapTrigEl.first ) != trigMap.at(2).end()) {
312  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 
324 TEST_F ( testFindTrigs, checkFlexsSingleJoint ) {
325 
326  for (int k = 0; k< trigMap.size(); ++k) {
327 
328  if ( trigMap.at(k).begin()->second.getPrimitiveType() == ROSEE::ActionPrimitive::Type::Trig ) {
329  continue;
330  }
331 
332  for (auto &mapFlexEl: trigMap.at(k) ) {
333 
334  unsigned int nJSet = 0;
335 
336  for ( auto &flexJs : mapFlexEl.second.getJointPos() ) { //iteration over the jointstates map
337 
338  if ( flexJs.second.at(0) != 0.0 ) {
339  nJSet++;
340  }
341  }
342  EXPECT_EQ (1, nJSet);
343 
344  //test also the jointInvolvedBool vector
345  unsigned int jointsInvolvedSum = 0;
346  for ( auto flexJs : mapFlexEl.second.getJointsInvolvedCount() ) { //iteration over the jointstates map
347 
348  jointsInvolvedSum += flexJs.second;
349  }
350  EXPECT_EQ (1, jointsInvolvedSum);
351 
352  }
353  }
354 }
355 
356 
357 } //namespace
358 
359 int main ( int argc, char **argv ) {
360 
361  if (argc < 2 ){
362 
363  std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
364  return -1;
365  }
366 
367  /* Run tests on an isolated roscore */
368  if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
369  {
370  perror("setenv");
371  return 1;
372  }
373 
374  //run roscore
375  std::unique_ptr<ROSEE::TestUtils::Process> roscore;
376  roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));
377 
378  if ( ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testFindTrigs" ) != 0 ) {
379 
380  std::cout << "[TEST ERROR] Prepare Funcion failed" << std::endl;
381  return -1;
382  }
383 
384 
385  ::testing::InitGoogleTest ( &argc, argv );
386  return RUN_ALL_TESTS();
387 }
ROSEE::YamlWorker::parseYamlPrimitive
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr > parseYamlPrimitive(std::string fileWithPath)
Parse a yaml file and return the map with all the actions present in the file. For the moment...
Definition: YamlWorker.cpp:144
ROSEE::JointPos
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints. The key is the name of the string...
Definition: Action.h:40
size
std::size_t size(custom_string const &s)
EXPECT_EQ
#define EXPECT_EQ(a, b)
ROSEE::ActionPrimitive::Type
Type
Enum useful to discriminate each primitive action when, for example, we want to parse a file if you ...
Definition: ActionPrimitive.h:60
main
int main(int argc, char **argv)
Definition: test_find_trigs.cpp:359
ROSEE::ActionTrig
The action of moving some joints (see later) of a single finger in a full clousure position towards t...
Definition: ActionTrig.h:64
ROSEE::TestUtils::prepareROSForTests
int prepareROSForTests(int argc, char **argv, std::string testName)
Function to be called in the main of each test, it runs roscore and fill parameter server with robot ...
Definition: testUtils.h:95
ROSEE::FindActions
Class to check which fingertips collide (for the pinch action at the moment)
Definition: FindActions.h:36
EXPECT_TRUE
#define EXPECT_TRUE(args)
TEST_F
TEST_F(TestRunner, threaded_test)

end-effector
Author(s): Luca Muratore , Davide Torielli , Liana Bertoni
autogenerated on Tue Apr 5 2022 02:57:53