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/FindActions.h>
7 : #include <ros_end_effector/ParserMoveIt.h>
8 : #include <ros_end_effector/GraspingActions/ActionPrimitive.h>
9 : #include <ros_end_effector/GraspingActions/ActionTrig.h>
10 : #include <ros_end_effector/GraspingActions/ActionComposed.h>
11 : #include <ros_end_effector/GraspingActions/ActionPinchGeneric.h>
12 :
13 : namespace {
14 :
15 : class testTimedAction: public ::testing::Test {
16 :
17 :
18 : protected:
19 :
20 8 : testTimedAction() : timedAction("TestTimedAction") {
21 8 : }
22 :
23 8 : virtual ~testTimedAction() {
24 8 : }
25 :
26 8 : virtual void SetUp() override {
27 :
28 16 : std::shared_ptr <ROSEE::ParserMoveIt> parserMoveIt = std::make_shared <ROSEE::ParserMoveIt> ();
29 :
30 : //if return false, models are not found and it is useless to continue the test
31 8 : ASSERT_TRUE(parserMoveIt->init ("robot_description", false)) ;
32 :
33 16 : ROSEE::FindActions actionsFinder (parserMoveIt);
34 :
35 16 : std::string folderForActions = ROSEE::Utils::getPackagePath() + "/configs/actions/tests/" + parserMoveIt->getHandName();
36 :
37 8 : trigMap = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::Trig, folderForActions + "/primitives/") ;
38 16 : auto pinches = actionsFinder.findPinch(folderForActions + "/primitives/") ;
39 :
40 8 : pinchTightMap = pinches.first;
41 8 : pinchLooseMap = pinches.second;
42 :
43 16 : std::shared_ptr<ROSEE::ActionComposed> actionComposed = std::make_shared<ROSEE::ActionComposed> ("degrasp");
44 :
45 8 : if (trigMap.size() > 0){
46 22 : for (auto trig : trigMap) {
47 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
48 32 : std::make_shared <ROSEE::ActionTrig> ( trig.second );
49 :
50 16 : actionComposed->sumAction(pointer, 0);
51 :
52 : //different name for each trig inserted
53 16 : timedAction.insertAction(pointer, 0.54, 0.2, 0, 0.8, "trig_" + *(pointer->getKeyElements().begin()) );
54 : }
55 : }
56 :
57 : //lets add a pinch
58 8 : if (pinchTightMap.size()>0) {
59 :
60 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
61 8 : std::make_shared <ROSEE::ActionPinchTight> ( pinchTightMap.begin()->second );
62 :
63 4 : timedAction.insertAction(pointer, 0, 0.45);
64 :
65 4 : } else if (pinchLooseMap.size()>0) {
66 :
67 : std::shared_ptr <ROSEE::ActionPrimitive> pointer =
68 4 : std::make_shared <ROSEE::ActionPinchLoose> ( pinchLooseMap.begin()->second );
69 :
70 2 : timedAction.insertAction(pointer, 0, 0.45);
71 : }
72 :
73 : //now lets add a reset grasp pos (trig all zero)
74 8 : if (actionComposed->numberOfInnerActions( ) > 0) {
75 12 : std::shared_ptr <ROSEE::Action> pointer = actionComposed ;
76 6 : timedAction.insertAction(pointer);
77 : }
78 :
79 8 : ROSEE::YamlWorker yamlWorker;
80 8 : yamlWorker.createYamlFile (&timedAction, folderForActions + "/timeds/");
81 :
82 : //Parsing
83 8 : timedActionParsed = yamlWorker.parseYamlTimed (folderForActions + "/timeds/TestTimedAction.yaml");
84 :
85 : }
86 :
87 8 : virtual void TearDown() {
88 8 : }
89 :
90 : ROSEE::ActionTrig::Map trigMap;
91 : ROSEE::ActionPinchTight::Map pinchTightMap;
92 : ROSEE::ActionPinchLoose::Map pinchLooseMap;
93 :
94 : ROSEE::ActionTimed timedAction;
95 : std::shared_ptr<ROSEE::ActionTimed> timedActionParsed;
96 :
97 :
98 : };
99 :
100 20 : TEST_F ( testTimedAction, checkMembersSizeConsistency ) {
101 :
102 4 : EXPECT_EQ ( timedAction.getInnerActionsNames().size(), timedAction.getAllActionMargins().size() );
103 4 : EXPECT_EQ ( timedAction.getAllActionMargins().size(), timedAction.getAllJointCountAction().size() );
104 4 : EXPECT_EQ ( timedAction.getAllJointCountAction().size(), timedAction.getAllJointPos().size() );
105 :
106 4 : EXPECT_EQ ( timedActionParsed->getInnerActionsNames().size(), timedActionParsed->getAllActionMargins().size() );
107 4 : EXPECT_EQ ( timedActionParsed->getAllActionMargins().size(), timedActionParsed->getAllJointCountAction().size() );
108 4 : EXPECT_EQ ( timedActionParsed->getAllJointCountAction().size(), timedActionParsed->getAllJointPos().size() );
109 :
110 4 : }
111 :
112 20 : TEST_F ( testTimedAction, checkEmitParse ) {
113 :
114 4 : if (timedAction.getInnerActionsNames().size() > 0) {
115 :
116 4 : EXPECT_EQ (timedAction.getName(), timedActionParsed->getName() );
117 4 : EXPECT_EQ (timedAction.getType(), timedActionParsed->getType() );
118 4 : EXPECT_EQ (timedAction.getFingersInvolved(), timedActionParsed->getFingersInvolved() );
119 :
120 24 : for (auto jointCount : timedAction.getJointsInvolvedCount()) {
121 20 : EXPECT_EQ ( jointCount.second, timedActionParsed->getJointsInvolvedCount().at(jointCount.first) );
122 : }
123 :
124 24 : for (auto joint: timedAction.getJointPos() ) {
125 :
126 : //compare size of joint (number of dofs)
127 20 : ASSERT_EQ (joint.second.size(), timedActionParsed->getJointPos().at(joint.first).size() );
128 : //loop the eventually multiple joint pos (when dofs > 1)
129 40 : for (int j = 0; j < joint.second.size(); ++j ){
130 20 : EXPECT_DOUBLE_EQ ( joint.second.at(j), timedActionParsed->getJointPos().at(joint.first).at(j) );
131 : }
132 : }
133 :
134 4 : unsigned int k = 0;
135 18 : for (auto innerActName : timedAction.getInnerActionsNames()) {
136 14 : EXPECT_EQ ( innerActName, timedActionParsed->getInnerActionsNames().at(k) );
137 14 : k++;
138 : }
139 :
140 4 : k = 0;
141 18 : for (auto timeMargins : timedAction.getAllActionMargins() ) {
142 :
143 14 : EXPECT_DOUBLE_EQ (timeMargins.first, timedActionParsed->getAllActionMargins().at(k).first);
144 14 : EXPECT_DOUBLE_EQ (timeMargins.second, timedActionParsed->getAllActionMargins().at(k).second);
145 14 : k++;
146 : }
147 :
148 8 : auto jpvector = timedAction.getAllJointPos();
149 18 : for (int i=0; i < jpvector.size(); i++) {
150 :
151 100 : for (auto joint: jpvector.at(i) ) {
152 :
153 172 : auto otherjointPos = timedActionParsed->getAllJointPos().at(i).at(joint.first);
154 : //compare size of joint (number of dofs)
155 86 : ASSERT_EQ (joint.second.size(), otherjointPos.size() );
156 :
157 : //loop the eventually multiple joint pos (when dofs > 1)
158 172 : for (int j = 0; j < joint.second.size(); ++j ){
159 86 : EXPECT_DOUBLE_EQ ( joint.second.at(j), otherjointPos.at(j) );
160 : }
161 : }
162 : }
163 :
164 8 : auto jpcvector = timedAction.getAllJointCountAction();
165 18 : for (int i=0; i < jpcvector.size(); i++) {
166 :
167 100 : for (auto joint: jpcvector.at(i) ) {
168 :
169 86 : EXPECT_EQ ( joint.second, timedActionParsed->getAllJointCountAction().at(i).at(joint.first) );
170 :
171 : }
172 : }
173 :
174 : }
175 : }
176 :
177 : } //namespace
178 :
179 4 : int main ( int argc, char **argv ) {
180 :
181 4 : if (argc < 2 ){
182 :
183 0 : std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
184 0 : return -1;
185 : }
186 :
187 : /* Run tests on an isolated roscore */
188 4 : if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
189 : {
190 0 : perror("setenv");
191 0 : return 1;
192 : }
193 :
194 : //run roscore
195 8 : std::unique_ptr<ROSEE::TestUtils::Process> roscore;
196 4 : roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));
197 :
198 4 : if ( ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testTimedAction" ) != 0 ) {
199 :
200 0 : std::cout << "[TEST ERROR] Prepare Function failed" << std::endl;
201 0 : return -1;
202 : }
203 :
204 :
205 4 : ::testing::InitGoogleTest ( &argc, argv );
206 4 : return RUN_ALL_TESTS();
207 12 : }
|
