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/Parser.h>
7 : #include <ros_end_effector/ParserMoveIt.h>
8 : #include <ros_end_effector/FindActions.h>
9 : #include <ros_end_effector/EEInterface.h>
10 : #include <ros_end_effector/GraspingActions/ActionGeneric.h>
11 : #include <ros_end_effector/Utils.h>
12 : #include <ros_end_effector/YamlWorker.h>
13 :
14 : #include <rosee_msg/ROSEECommandAction.h>
15 : #include <actionlib/client/simple_action_client.h>
16 :
17 : #include <random>
18 :
19 :
20 : namespace {
21 :
22 : /**
23 : * @brief This Tests are to check if actions are sent correctly. Check each TEST_F for better explanation
24 : *
25 : * @warning @todo There are a lot of prints that seems to be not in the right order. Probably is because we use
26 : * the ROS STREAM that effectively prints only when the spin is called...
27 : * BTW it is strange that the parser prints are printed at the end, and not while setup function is running (where they
28 : * should be because we init the parser there. This does not cause problems at the test.
29 : */
30 : class testSendAction: public ::testing::Test {
31 :
32 :
33 :
34 : protected:
35 :
36 52 : testSendAction() {
37 52 : }
38 :
39 52 : virtual ~testSendAction() {
40 52 : }
41 :
42 52 : virtual void SetUp() override {
43 :
44 52 : if (! nh.hasParam("robot_name")) {
45 0 : std::cout << "[TEST FAIL: robot name not set on server]" << std::endl;
46 0 : return;
47 : }
48 :
49 104 : std::string robot_name = "";
50 52 : nh.getParam("robot_name", robot_name);
51 :
52 104 : ROSEE::Parser p ( nh );
53 52 : if (! p.init ( ROSEE::Utils::getPackagePath() + "/configs/" + robot_name + ".yaml" )) {
54 :
55 0 : std::cout << "[TEST SEND ACTIONS]parser FAIL: some config file missing]" << std::endl;
56 0 : return;
57 : }
58 :
59 52 : ee = std::make_shared<ROSEE::EEInterface>(p);
60 :
61 52 : folderForActions = p.getActionPath();
62 52 : if ( folderForActions.size() == 0 ){ //if no action path is set in the yaml file...
63 0 : std::cout << "[TEST SEND ACTIONS] parser FAIL: action_path in the config file is missing" << std::endl;
64 0 : return;
65 : }
66 :
67 : //note: test on the findActions part is done in other test files
68 52 : parserMoveIt = std::make_shared<ROSEE::ParserMoveIt>();
69 52 : if (! parserMoveIt->init ("robot_description", false) ) {
70 :
71 0 : std::cout << "[TEST SEND ACTIONS] FAILED parserMoveit Init, stopping execution" << std::endl;
72 0 : return;
73 : }
74 :
75 : //note: calls to find*** (like findTrig) are done in the specific testu
76 52 : actionsFinder = std::make_shared<ROSEE::FindActions>(parserMoveIt);
77 :
78 :
79 : }
80 :
81 52 : virtual void TearDown() override {
82 52 : }
83 :
84 : void sendAndTest(ROSEE::Action::Ptr action, double percentageWanted = 1.0);
85 :
86 : ros::NodeHandle nh;
87 : std::string folderForActions;
88 : std::unique_ptr<ROSEE::TestUtils::Process> roseeExecutor;
89 : ROSEE::EEInterface::Ptr ee;
90 : ros::Publisher sendActionPub;
91 : ros::Subscriber receiveRobStateSub;
92 : ROSEE::YamlWorker yamlWorker;
93 : std::shared_ptr <actionlib::SimpleActionClient <rosee_msg::ROSEECommandAction> > action_client;
94 : ROSEE::ParserMoveIt::Ptr parserMoveIt;
95 : std::shared_ptr<ROSEE::FindActions> actionsFinder;
96 :
97 52 : struct ClbkHelper {
98 :
99 52 : ClbkHelper() : js(), completed(false) {};
100 :
101 269 : void jointStateClbk(const sensor_msgs::JointState::ConstPtr& msg) {
102 :
103 269 : js.name = msg->name;
104 269 : js.position = msg->position;
105 269 : js.velocity = msg->velocity;
106 269 : js.effort = msg->effort;
107 269 : }
108 :
109 33 : void actionDoneClbk(const actionlib::SimpleClientGoalState& state,
110 : const rosee_msg::ROSEECommandResultConstPtr& result) {
111 :
112 33 : completed = true;
113 33 : goalState = state.state_;
114 33 : actionCompleted = result->completed_action;
115 33 : }
116 :
117 2230 : void actionFeedbackClbk(const rosee_msg::ROSEECommandFeedbackConstPtr& feedback) {
118 :
119 2230 : feedback_percentage = feedback->completation_percentage;
120 2230 : }
121 :
122 33 : void actionActiveClbk() { completed = false; }
123 :
124 : sensor_msgs::JointState js;
125 : bool completed;
126 : double feedback_percentage;
127 : rosee_msg::ROSEEActionControl actionCompleted;
128 : actionlib::SimpleClientGoalState::StateEnum goalState;
129 :
130 :
131 : };
132 :
133 : ClbkHelper clbkHelper;
134 :
135 : void setMainNode();
136 : void sendAction( ROSEE::Action::Ptr action, double percentageWanted);
137 : void testAction( ROSEE::Action::Ptr actionSent, double percentageWanted);
138 :
139 : };
140 :
141 33 : void testSendAction::setMainNode() {
142 :
143 66 : std::string handNameArg = "hand_name:=" + ee->getName();
144 33 : roseeExecutor.reset(new ROSEE::TestUtils::Process({"roslaunch", "end_effector", "test_rosee_startup.launch", handNameArg}));
145 :
146 : //TODO put a checkReady service instead of sleeping?
147 33 : sleep(5); // lets wait for test_rosee_startup to be ready
148 66 : std::string topic_name_js = "/ros_end_effector/joint_states";
149 :
150 33 : receiveRobStateSub = nh.subscribe (topic_name_js, 1, &ClbkHelper::jointStateClbk, &clbkHelper);
151 :
152 33 : action_client =
153 : std::make_shared <actionlib::SimpleActionClient <rosee_msg::ROSEECommandAction>>
154 66 : (nh, "/ros_end_effector/action_command", true); //TODO check this action command
155 :
156 33 : action_client->waitForServer();
157 :
158 33 : }
159 :
160 33 : void testSendAction::sendAction( ROSEE::Action::Ptr action, double percentageWanted) {
161 :
162 66 : rosee_msg::ROSEECommandGoal goal;
163 33 : goal.goal_action.seq = 0 ;
164 33 : goal.goal_action.stamp = ros::Time::now();
165 33 : goal.goal_action.percentage = percentageWanted;
166 33 : goal.goal_action.action_name = action->getName();
167 33 : goal.goal_action.action_type = action->getType() ;
168 :
169 33 : if (action->getType() == ROSEE::Action::Type::Primitive) {
170 28 : ROSEE::ActionPrimitive::Ptr primitivePtr = std::static_pointer_cast<ROSEE::ActionPrimitive>(action);
171 14 : goal.goal_action.actionPrimitive_type = primitivePtr->getPrimitiveType() ;
172 :
173 33 : for (auto it : primitivePtr->getKeyElements()) {
174 19 : goal.goal_action.selectable_items.push_back(it);
175 : }
176 : }
177 :
178 33 : action_client->sendGoal (goal, boost::bind(&ClbkHelper::actionDoneClbk, &clbkHelper, _1, _2),
179 : boost::bind(&ClbkHelper::actionActiveClbk, &clbkHelper), boost::bind(&ClbkHelper::actionFeedbackClbk, &clbkHelper, _1)) ;
180 :
181 33 : }
182 :
183 33 : void testSendAction::testAction(ROSEE::Action::Ptr actionSent, double percentageWanted) {
184 :
185 66 : ros::Rate r(10); // 10 hz
186 515 : while (!clbkHelper.completed) {
187 241 : ROS_INFO_STREAM_ONCE("Test Send Actions: Waiting for action completion...");
188 241 : ros::spinOnce();
189 241 : r.sleep();
190 : }
191 :
192 : //lets wait one sec more so subscriber for joint state receive the very last state
193 66 : ros::Rate r2(1);
194 33 : r2.sleep();
195 33 : ros::spinOnce();
196 :
197 :
198 : //finally, lets test if the pos set in the actions are the same of the robot when the action is completed
199 231 : for (int i=0; i < clbkHelper.js.name.size(); i++) {
200 :
201 378 : auto wantedJointsPosMap = actionSent->getJointPos();
202 :
203 198 : auto findJoint = wantedJointsPosMap.find(clbkHelper.js.name[i]);
204 :
205 198 : if (findJoint == wantedJointsPosMap.end()){
206 18 : continue; //this is not an error, in clbkHelper we have joint pos of all joints, not only actuated
207 : }
208 :
209 180 : double wantedPos = (findJoint->second.at(0))*percentageWanted;
210 180 : double realPos = clbkHelper.js.position[i];
211 :
212 180 : EXPECT_NEAR (wantedPos, realPos, 0.02); //norm of the accepted error in roseeExecutor is <0.01
213 :
214 : //the percentage must be exaclty 100 instead (apart double precisions errors, handled by the macro)
215 180 : EXPECT_DOUBLE_EQ (clbkHelper.feedback_percentage, 100);
216 180 : EXPECT_EQ (actionlib::SimpleClientGoalState::SUCCEEDED, clbkHelper.goalState);
217 :
218 : //test if the action completed is the same sent
219 180 : EXPECT_DOUBLE_EQ (percentageWanted, clbkHelper.actionCompleted.percentage);
220 180 : EXPECT_EQ (actionSent->getName(), clbkHelper.actionCompleted.action_name);
221 180 : EXPECT_EQ (actionSent->getType(), clbkHelper.actionCompleted.action_type);
222 180 : if (actionSent->getType() == ROSEE::Action::Type::Primitive) {
223 162 : ROSEE::ActionPrimitive::Ptr primitivePtr = std::static_pointer_cast<ROSEE::ActionPrimitive>(actionSent);
224 81 : EXPECT_EQ (primitivePtr->getPrimitiveType(), clbkHelper.actionCompleted.actionPrimitive_type);
225 : }
226 : }
227 33 : }
228 :
229 33 : void testSendAction::sendAndTest(ROSEE::Action::Ptr action, double percentageWanted) {
230 :
231 33 : setMainNode();
232 33 : sendAction(action, percentageWanted);
233 33 : testAction(action, percentageWanted);
234 :
235 33 : }
236 :
237 : /**
238 : * @brief These Tests create a Generic Action and see if it is sent correctly.
239 : * 1 - An ActionGeneric is created, where the actuated Joints are set
240 : *
241 : * 2 - The sendAndTest function is called:
242 : *
243 : * -[setMainNode] The ROSEE main node (which receives action commands and output specific joint pos references) is launched
244 : * such that the action created is parsed by him
245 : * -[sendAction] The action is commanded.
246 : * -[testAction] After rosee says the action is completed, some checks are done to see if the final state is the same as the
247 : * action created
248 : */
249 :
250 20 : TEST_F ( testSendAction, sendSimpleGeneric ) {
251 :
252 :
253 8 : ROSEE::JointPos jp;
254 8 : ROSEE::JointsInvolvedCount jpc;
255 :
256 8 : std::vector<std::string> actJoints = ee->getActuatedJoints();
257 :
258 4 : ASSERT_FALSE (actJoints.empty());
259 :
260 24 : for (int i = 0; i<actJoints.size(); i++) {
261 :
262 20 : int id = -1;
263 20 : ee->getInternalIdForJoint(actJoints.at(i), id);
264 :
265 : //create an action where all joints move toward their upper limit
266 40 : std::vector<double> one_dof;
267 20 : one_dof.push_back ( ee->getUpperPositionLimits()[id] );
268 20 : jp.insert ( std::make_pair(actJoints.at(i), one_dof) );
269 20 : jpc.insert (std::make_pair(actJoints.at(i), 1));
270 :
271 : }
272 :
273 : //ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionGeneric>("testAllUpperLim", jp, jpc);
274 8 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionGeneric>("AAAAAAAAAAAAAAAA", jp, jpc);
275 : //emit the yaml so roseeExecutor can find the action
276 4 : yamlWorker.createYamlFile( action.get(), folderForActions + "/generics/" );
277 :
278 4 : sendAndTest(action);
279 :
280 : }
281 :
282 20 : TEST_F ( testSendAction, sendSimpleGeneric2 ) {
283 :
284 :
285 8 : ROSEE::JointPos jp;
286 8 : ROSEE::JointsInvolvedCount jpc;
287 :
288 8 : std::vector<std::string> actJoints = ee->getActuatedJoints();
289 :
290 4 : ASSERT_FALSE (actJoints.empty());
291 :
292 24 : for (int i = 0; i<actJoints.size(); i++) {
293 :
294 20 : int id = -1;
295 20 : ee->getInternalIdForJoint(actJoints.at(i), id);
296 :
297 : //create an action where all joints move toward their lower limit
298 40 : std::vector<double> one_dof;
299 20 : one_dof.push_back ( ee->getLowerPositionLimits()[id] );
300 20 : jp.insert ( std::make_pair(actJoints.at(i), one_dof) );
301 20 : jpc.insert (std::make_pair(actJoints.at(i), 1));
302 :
303 : }
304 :
305 8 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionGeneric>("testAllLowerLim", jp, jpc);
306 : //emit the yaml so roseeExecutor can find the action
307 4 : yamlWorker.createYamlFile( action.get(), folderForActions + "/generics/" );
308 :
309 4 : sendAndTest(action);
310 :
311 : }
312 :
313 :
314 20 : TEST_F ( testSendAction, sendSimpleGeneric3 ) {
315 :
316 8 : ROSEE::JointPos jp;
317 8 : ROSEE::JointsInvolvedCount jpc;
318 :
319 8 : std::vector<std::string> actJoints = ee->getActuatedJoints();
320 :
321 4 : ASSERT_FALSE (actJoints.empty());
322 :
323 24 : for (int i = 0; i<actJoints.size(); i++) {
324 :
325 20 : int id = -1;
326 20 : ee->getInternalIdForJoint(actJoints.at(i), id);
327 :
328 : //create an action where all joints move, but make sure to stay within limits
329 40 : std::vector<double> one_dof;
330 20 : one_dof.push_back ( ee->getLowerPositionLimits()[id]*0.15 + 0.1 );
331 20 : jp.insert ( std::make_pair(actJoints.at(i), one_dof) );
332 20 : jpc.insert (std::make_pair(actJoints.at(i), 1));
333 :
334 : }
335 :
336 8 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionGeneric>("testAllUpperLim2", jp, jpc);
337 : //emit the yaml so roseeExecutor can find the action
338 4 : yamlWorker.createYamlFile( action.get(), folderForActions + "/generics/" );
339 :
340 4 : sendAndTest(action, 0.33);
341 :
342 : }
343 :
344 : /**
345 : * @brief This test take a random trig (among the one found for the ee loaded) and command it
346 : * The check is done after the action completion as before (in the sendAndTest function),
347 : * but here we also check that the final pose of the moved finger finds all the actuated joint of that finger
348 : * in the bigger bound (as it should be by definition of trig)
349 : */
350 20 : TEST_F ( testSendAction, sendTrig ) {
351 :
352 8 : ROSEE::ActionTrig::Map trigMap = actionsFinder->findTrig (ROSEE::ActionPrimitive::Type::Trig, folderForActions + "/primitives/") ;
353 :
354 4 : if (trigMap.size()>0){
355 6 : std::vector<std::string> keys = ROSEE::Utils::extract_keys(trigMap);
356 : //lets pick a random trig
357 3 : srand((unsigned int)time(NULL));
358 3 : int i = rand() % keys.size();
359 6 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionTrig>(trigMap.at(keys.at(i)));
360 :
361 3 : sendAndTest(action);
362 :
363 : //other than "default" check done in sendAndTest, we check that effectively the trig joints are gone
364 : //toward their limit (from definition of trig)
365 6 : std::vector<std::string> actJointsInvolved;
366 3 : ee->getActuatedJointsInFinger(keys.at(i), actJointsInvolved);
367 :
368 10 : for (auto jointName : actJointsInvolved) {
369 : //at O single dof joint
370 7 : double bigBound = parserMoveIt->getBiggerBoundFromZero(jointName).at(0);
371 :
372 35 : for (int k = 0; k<clbkHelper.js.name.size(); k++) {
373 :
374 31 : if (clbkHelper.js.name[i].compare(jointName) == 0 ) {
375 3 : EXPECT_NEAR(bigBound, clbkHelper.js.position[i], 0.02);
376 3 : break;
377 : }
378 : }
379 : }
380 : }
381 4 : }
382 :
383 20 : TEST_F ( testSendAction, sendTipFlex ) {
384 :
385 8 : auto tipFlexMap = actionsFinder->findTrig (ROSEE::ActionPrimitive::Type::TipFlex, folderForActions + "/primitives/");
386 :
387 4 : if (tipFlexMap.size()>0) {
388 6 : std::vector<std::string> keys = ROSEE::Utils::extract_keys(tipFlexMap);
389 : //lets pick a random
390 3 : srand((unsigned int)time(NULL));
391 3 : int i = rand() % keys.size();
392 6 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionTrig>(tipFlexMap.at(keys.at(i)));
393 :
394 3 : sendAndTest(action);
395 :
396 : //other than "default" check done in sendAndTest, we check that effectively the joint is gone to the limit
397 :
398 : //Workaround to take the single joint used by this action
399 6 : auto jic = action->getJointsInvolvedCount();
400 6 : std::string jointInvolved;
401 22 : for (auto it : jic) {
402 19 : if (it.second == 1 ){
403 3 : jointInvolved = it.first;
404 : }
405 : }
406 :
407 3 : EXPECT_TRUE(jointInvolved.size() > 0);
408 :
409 : //at O single dof joint
410 3 : double bigBound = parserMoveIt->getBiggerBoundFromZero(jointInvolved).at(0);
411 :
412 22 : for (int k = 0; k<clbkHelper.js.name.size(); k++) {
413 :
414 19 : if (clbkHelper.js.name[i].compare(jointInvolved) == 0 ) {
415 0 : EXPECT_NEAR(bigBound, clbkHelper.js.position[i], 0.02);
416 0 : break;
417 :
418 : }
419 : }
420 : }
421 4 : }
422 :
423 20 : TEST_F ( testSendAction, sendFingFlex ) {
424 :
425 8 : auto fingFlexMap = actionsFinder->findTrig (ROSEE::ActionPrimitive::Type::FingFlex, folderForActions + "/primitives/");
426 :
427 4 : if (fingFlexMap.size()>0) {
428 6 : std::vector<std::string> keys = ROSEE::Utils::extract_keys(fingFlexMap);
429 : //lets pick a random
430 3 : srand((unsigned int)time(NULL));
431 3 : int i = rand() % keys.size();
432 6 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionTrig>(fingFlexMap.at(keys.at(i)));
433 :
434 3 : sendAndTest(action);
435 :
436 : //other than "default" check done in sendAndTest, we check that effectively the joint is gone to the limit
437 :
438 : //Workaround to take the single joint used by this action
439 6 : auto jic = action->getJointsInvolvedCount();
440 6 : std::string jointInvolved;
441 22 : for (auto it : jic) {
442 19 : if (it.second == 1 ){
443 3 : jointInvolved = it.first;
444 : }
445 : }
446 :
447 3 : EXPECT_TRUE(jointInvolved.size() > 0);
448 :
449 : //at O single dof joint
450 3 : double bigBound = parserMoveIt->getBiggerBoundFromZero(jointInvolved).at(0);
451 :
452 3 : for (int k = 0; k<clbkHelper.js.name.size(); k++) {
453 :
454 3 : if (clbkHelper.js.name[i].compare(jointInvolved) == 0 ) {
455 3 : EXPECT_NEAR(bigBound, clbkHelper.js.position[i], 0.02);
456 3 : break;
457 :
458 : }
459 : }
460 : }
461 4 : }
462 :
463 :
464 20 : TEST_F ( testSendAction, sendPinches ) {
465 :
466 8 : auto pinchTightMap = actionsFinder->findPinch(folderForActions + "/primitives/").first;
467 :
468 4 : if (pinchTightMap.size()>0){
469 4 : std::vector<std::pair<std::string,std::string>> keys = ROSEE::Utils::extract_keys(pinchTightMap);
470 : //lets pick a random
471 2 : srand((unsigned int)time(NULL));
472 2 : int i = rand() % keys.size();
473 4 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionPinchTight>(pinchTightMap.at(keys.at(i)));
474 :
475 2 : sendAndTest(action);
476 :
477 : }
478 4 : }
479 :
480 20 : TEST_F ( testSendAction, sendPinchLoose ) {
481 :
482 8 : auto pinchLooseMap = actionsFinder->findPinch(folderForActions + "/primitives/").second;
483 :
484 4 : if (pinchLooseMap.size()>0){
485 2 : std::vector<std::pair<std::string,std::string>> keys = ROSEE::Utils::extract_keys(pinchLooseMap);
486 : //lets pick a random
487 1 : srand((unsigned int)time(NULL));
488 1 : int i = rand() % keys.size();
489 2 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionPinchLoose>(pinchLooseMap.at(keys.at(i)));
490 :
491 1 : sendAndTest(action);
492 :
493 : }
494 4 : }
495 :
496 20 : TEST_F (testSendAction, sendMultiplePinchStrict ) {
497 :
498 :
499 8 : std::vector < ROSEE::ActionMultiplePinchTight::Map > multiplePinchMapsStrict;
500 6 : for (int i = 3; i <= ee->getFingersNumber(); i++) {
501 4 : auto multiplePinchMapStrict = actionsFinder->findMultiplePinch(i, folderForActions + "/primitives/", true );
502 :
503 : //keep only if it is not empty
504 2 : if (multiplePinchMapStrict.size() > 0 ) {
505 0 : multiplePinchMapsStrict.push_back (multiplePinchMapStrict);
506 : }
507 : }
508 :
509 4 : if ( multiplePinchMapsStrict.size() > 0 ) {
510 :
511 0 : srand((unsigned int)time(NULL));
512 0 : int j = rand() % multiplePinchMapsStrict.size();
513 :
514 0 : std::vector<std::set<std::string>> keys = ROSEE::Utils::extract_keys( multiplePinchMapsStrict.at(j) );
515 : //lets pick a random trig
516 0 : srand((unsigned int)time(NULL));
517 0 : int i = rand() % keys.size();
518 0 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionMultiplePinchTight>(multiplePinchMapsStrict.at(j).at(keys.at(i)));
519 :
520 0 : sendAndTest(action);
521 :
522 : }
523 :
524 4 : }
525 :
526 20 : TEST_F (testSendAction, sendMultiplePinchNoStrict ) {
527 :
528 :
529 8 : std::vector < ROSEE::ActionMultiplePinchTight::Map > multiplePinchMapsNOStrict;
530 6 : for (int i = 3; i <= ee->getFingersNumber(); i++) {
531 4 : auto multiplePinchMapNOStrict = actionsFinder->findMultiplePinch(i, folderForActions + "/primitives/", false );
532 :
533 : //keep only if it is not empty
534 2 : if (multiplePinchMapNOStrict.size() > 0 ) {
535 1 : multiplePinchMapsNOStrict.push_back (multiplePinchMapNOStrict);
536 : }
537 :
538 : }
539 :
540 4 : if ( multiplePinchMapsNOStrict.size() > 0 ) {
541 :
542 1 : srand((unsigned int)time(NULL));
543 1 : int j = rand() % multiplePinchMapsNOStrict.size();
544 :
545 2 : std::vector<std::set<std::string>> keys = ROSEE::Utils::extract_keys( multiplePinchMapsNOStrict.at(j) );
546 : //lets pick a random trig
547 1 : srand((unsigned int)time(NULL));
548 1 : int i = rand() % keys.size();
549 2 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionMultiplePinchTight>(multiplePinchMapsNOStrict.at(j).at(keys.at(i)));
550 :
551 1 : sendAndTest(action);
552 :
553 : }
554 :
555 4 : }
556 :
557 20 : TEST_F (testSendAction, sendSingleJointMultipleTips ) {
558 :
559 8 : std::vector < ROSEE::ActionSingleJointMultipleTips::Map > singleJointMultipleTipsMaps;
560 :
561 14 : for (int i = 1; i<=ee->getFingersNumber(); i++) {
562 :
563 : std::map < std::string, ROSEE::ActionSingleJointMultipleTips> singleJointMultipleTipsMap =
564 20 : actionsFinder->findSingleJointMultipleTips (i, folderForActions + "/primitives/") ;
565 :
566 10 : if (singleJointMultipleTipsMap.size()>0) {
567 1 : singleJointMultipleTipsMaps.push_back(singleJointMultipleTipsMap);
568 : }
569 :
570 : }
571 :
572 4 : if ( singleJointMultipleTipsMaps.size() > 0 ) {
573 :
574 1 : srand((unsigned int)time(NULL));
575 1 : int j = rand() % singleJointMultipleTipsMaps.size();
576 :
577 2 : std::vector<std::string> keys = ROSEE::Utils::extract_keys( singleJointMultipleTipsMaps.at(j) );
578 : //lets pick a random trig
579 1 : srand((unsigned int)time(NULL));
580 1 : int i = rand() % keys.size();
581 2 : ROSEE::Action::Ptr action = std::make_shared<ROSEE::ActionSingleJointMultipleTips>(singleJointMultipleTipsMaps.at(j).at(keys.at(i)));
582 :
583 1 : sendAndTest(action);
584 :
585 : }
586 4 : }
587 :
588 :
589 : /***
590 : * @brief Here we create a randomic action composed by some primitives
591 : */
592 20 : TEST_F (testSendAction, sendComposedAction ) {
593 :
594 8 : ROSEE::ActionTrig::Map trigMap = actionsFinder->findTrig (ROSEE::ActionPrimitive::Type::Trig, folderForActions + "/primitives/") ;
595 8 : auto fingFlexMap = actionsFinder->findTrig (ROSEE::ActionPrimitive::Type::FingFlex, folderForActions + "/primitives/");
596 8 : auto pinchTightMap = actionsFinder->findPinch(folderForActions + "/primitives/").first;
597 :
598 8 : ROSEE::ActionComposed actionComposed ( "TestComposed", false) ;
599 :
600 12 : for (auto trig : trigMap) {
601 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
602 16 : std::make_shared <ROSEE::ActionTrig> ( trig.second );
603 :
604 8 : double lower_bound = 0;
605 8 : double upper_bound = 0.8; //low, we do not want to go out of joint limits
606 8 : std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
607 8 : std::default_random_engine re;
608 8 : double random = unif(re);
609 8 : EXPECT_TRUE(actionComposed.sumAction ( pointer, random ));
610 : }
611 :
612 12 : for (auto trig : fingFlexMap) {
613 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
614 16 : std::make_shared <ROSEE::ActionTrig> ( trig.second );
615 :
616 8 : double lower_bound = 0;
617 8 : double upper_bound = 0.7; //low, we do not want to go out of joint limits
618 8 : std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
619 8 : std::default_random_engine re;
620 8 : double random = unif(re);
621 8 : EXPECT_TRUE(actionComposed.sumAction ( pointer, random ));
622 : }
623 :
624 8 : for (auto pinch : pinchTightMap) {
625 :
626 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
627 8 : std::make_shared <ROSEE::ActionPinchTight> ( pinch.second );
628 :
629 4 : double lower_bound = 0;
630 4 : double upper_bound = 0.29; //low, we do not want to go out of joint limits
631 4 : std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
632 4 : std::default_random_engine re;
633 4 : double random = unif(re);
634 4 : EXPECT_TRUE(actionComposed.sumAction ( pointer, random, 2 ));
635 : }
636 :
637 4 : if (actionComposed.numberOfInnerActions() > 0) {
638 6 : ROSEE::Action::Ptr actionPtr = std::make_shared<ROSEE::ActionGeneric>(actionComposed);
639 :
640 : //do not forget to emit the file, in sendAndTest the rosee main node need to parse it
641 3 : yamlWorker.createYamlFile( actionPtr.get(), folderForActions + "/generics/" );
642 :
643 3 : sendAndTest(actionPtr, 0.95);
644 : }
645 :
646 4 : }
647 :
648 :
649 20 : TEST_F (testSendAction, sendTimedAction ) {
650 :
651 8 : ROSEE::ActionTrig::Map trigMap = actionsFinder->findTrig (ROSEE::ActionPrimitive::Type::Trig, folderForActions + "/primitives/") ;
652 8 : auto tipFlexMap = actionsFinder->findTrig (ROSEE::ActionPrimitive::Type::TipFlex, folderForActions + "/primitives/");
653 8 : auto pinchMaps = actionsFinder->findPinch(folderForActions + "/primitives/");
654 :
655 8 : ROSEE::ActionTimed actionTimed ( "TestTimed" ) ;
656 :
657 4 : if (trigMap.size()>0) {
658 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
659 6 : std::make_shared <ROSEE::ActionTrig> ( trigMap.begin()->second );
660 :
661 3 : double lower_bound = 0;
662 3 : double upper_bound = 0.8; //low, we do not want to go out of joint limits
663 3 : std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
664 3 : std::default_random_engine re;
665 3 : double random = unif(re);
666 3 : EXPECT_TRUE(actionTimed.insertAction(pointer, 0, 0.7, 0, random));
667 : }
668 :
669 4 : if (tipFlexMap.size() > 0) {
670 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
671 6 : std::make_shared <ROSEE::ActionTrig> ( tipFlexMap.rbegin()->second); //rbegin is the last element
672 :
673 3 : double lower_bound = 0;
674 3 : double upper_bound = 0.95;
675 3 : std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
676 3 : std::default_random_engine re;
677 3 : double random = unif(re);
678 3 : EXPECT_TRUE(actionTimed.insertAction(pointer, 0.2, 0.32, 0, random));
679 : }
680 :
681 4 : if (! pinchMaps.first.empty()) {
682 :
683 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
684 4 : std::make_shared <ROSEE::ActionPinchTight> ( pinchMaps.first.begin()->second );
685 :
686 2 : double lower_bound = 0.6;
687 2 : double upper_bound = 1;
688 2 : std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
689 2 : std::default_random_engine re;
690 2 : double random = unif(re);
691 2 : EXPECT_TRUE(actionTimed.insertAction(pointer, 0.2, 0.32, 1, random));
692 : }
693 :
694 4 : if (! pinchMaps.second.empty()) {
695 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
696 2 : std::make_shared <ROSEE::ActionPinchLoose> ( pinchMaps.second.rbegin()->second );
697 :
698 1 : double lower_bound = 0;
699 1 : double upper_bound = 1;
700 1 : std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
701 1 : std::default_random_engine re;
702 1 : double random = unif(re);
703 1 : EXPECT_TRUE(actionTimed.insertAction(pointer, 0, 0, 0, random));
704 : }
705 :
706 4 : if (actionTimed.getInnerActionsNames().size() > 0) {
707 8 : ROSEE::Action::Ptr actionPtr = std::make_shared<ROSEE::ActionTimed>(actionTimed);
708 :
709 : //do not forget to emit the file, in sendAndTest the rosee main node need to parse it
710 4 : yamlWorker.createYamlFile( actionPtr.get(), folderForActions + "/timeds/" );
711 :
712 4 : sendAndTest(actionPtr);
713 : }
714 :
715 4 : }
716 :
717 :
718 :
719 : } //namespace
720 :
721 4 : int main ( int argc, char **argv ) {
722 :
723 4 : if (argc < 2 ){
724 :
725 0 : std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
726 0 : return -1;
727 : }
728 :
729 : /******************* Run tests on an isolated roscore ********************************************************/
730 : /* COMMENT ALL THIS block if you want to use roscore command by hand (useful for debugging the test) */
731 : //TODO I do not really understand why everything fails if I put this block at the beginning of prepareROSForTests function
732 4 : if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
733 : {
734 0 : perror("setenv");
735 0 : return 1;
736 : }
737 :
738 : //run roscore
739 8 : std::unique_ptr<ROSEE::TestUtils::Process> roscore;
740 4 : roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));
741 : /****************************************************************************************************/
742 :
743 4 : if ( ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testSendAction" ) != 0 ) {
744 :
745 0 : std::cout << "[TEST ERROR] Prepare Function failed" << std::endl;
746 0 : return -1;
747 : }
748 :
749 4 : ::testing::InitGoogleTest ( &argc, argv );
750 4 : return RUN_ALL_TESTS();
751 12 : }
|
