LCOV - code coverage report
Current view: top level - test - test_send_action.cpp (source / functions) Hit Total Coverage
Test: main_coverage.info Lines: 317 366 86.6 %
Date: 2022-06-06 13:34:00 Functions: 39 41 95.1 %

          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             : }

Generated by: LCOV version 1.14