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