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/EEInterface.h>
8 :
9 : #include <ros_end_effector/RosServiceHandler.h>
10 : #include <ros_end_effector/MapActionHandler.h>
11 :
12 : #include <rosee_msg/NewGenericGraspingActionSrv.h>
13 :
14 :
15 :
16 : namespace {
17 :
18 : /**
19 : * @brief Test for the RosServiceHandler Class
20 : * It simply create the server and some clients which will test the server functions.
21 : * Hence, in this test a lot of ROSEE main class are not used like the FindAction
22 : */
23 : class testServiceHandler: public ::testing::Test {
24 :
25 :
26 : protected:
27 :
28 8 : testServiceHandler() {
29 8 : }
30 :
31 8 : virtual ~testServiceHandler() {
32 8 : }
33 :
34 8 : virtual void SetUp() override {
35 :
36 8 : if (! nh.hasParam("robot_name")) {
37 0 : std::cout << "[TEST FAIL: robot name not set on server]" << std::endl;
38 0 : return;
39 : }
40 :
41 8 : robot_name = "";
42 8 : nh.getParam("robot_name", robot_name);
43 :
44 16 : std::string handNameArg = "hand_name:=" + robot_name;
45 8 : roseeExecutor.reset(new ROSEE::TestUtils::Process({"roslaunch", "end_effector", "test_rosee_startup.launch", handNameArg}));
46 :
47 : }
48 :
49 8 : virtual void TearDown() override {
50 8 : }
51 :
52 : std::string robot_name;
53 : ros::NodeHandle nh;
54 : std::unique_ptr<ROSEE::TestUtils::Process> roseeExecutor;
55 :
56 : template <class clientType>
57 12 : bool initClient( ros::ServiceClient& rosee_client, std::string serviceName) {
58 :
59 12 : rosee_client = nh.serviceClient<clientType>(serviceName);
60 :
61 12 : rosee_client.waitForExistence();
62 :
63 12 : return true;
64 :
65 : };
66 :
67 : };
68 :
69 : /**
70 : * We call the service to include a new generic action multiple time, with both wrong and fallace requests, to see if the errors are
71 : * detected by the server, and correct action are accepted
72 : */
73 20 : TEST_F ( testServiceHandler, callNewAction ) {
74 :
75 4 : sleep(1); //without this joint state publisher crashes I do not know why (it is useless in this test, but annoying prints on traceback would appear)
76 :
77 8 : ros::ServiceClient rosee_client;
78 4 : initClient<rosee_msg::NewGenericGraspingActionSrv>(rosee_client, "/ros_end_effector/new_generic_grasping_action");
79 :
80 8 : rosee_msg::NewGenericGraspingActionSrv newActionSrv;
81 :
82 : //NOTE the server respond always with a true, and fill an error msg in the request if error happened
83 :
84 : //empty request, error
85 4 : EXPECT_TRUE( rosee_client.call(newActionSrv) );
86 4 : EXPECT_FALSE( newActionSrv.response.accepted );
87 4 : EXPECT_FALSE( newActionSrv.response.emitted );
88 4 : EXPECT_TRUE( newActionSrv.response.error_msg.size() > 0 );
89 :
90 : //empty joint pos, error
91 4 : newActionSrv.request.newAction.action_name = "newAction";
92 4 : EXPECT_TRUE( rosee_client.call(newActionSrv) );
93 4 : EXPECT_FALSE( newActionSrv.response.accepted );
94 4 : EXPECT_FALSE( newActionSrv.response.emitted );
95 4 : EXPECT_TRUE( newActionSrv.response.error_msg.size() > 0 );
96 :
97 : //correct request
98 4 : newActionSrv.request.newAction.action_motor_position.name.push_back("joint_1");
99 4 : newActionSrv.request.newAction.action_motor_position.position.push_back(1);
100 4 : EXPECT_TRUE( rosee_client.call(newActionSrv) );
101 4 : EXPECT_TRUE( newActionSrv.response.accepted );
102 4 : EXPECT_FALSE( newActionSrv.response.emitted );
103 4 : EXPECT_FALSE( newActionSrv.response.error_msg.size() > 0 );
104 :
105 : //same request, this time is must fail because a "test1" action is already present
106 4 : EXPECT_TRUE( rosee_client.call(newActionSrv) );
107 4 : EXPECT_FALSE( newActionSrv.response.accepted );
108 4 : EXPECT_FALSE( newActionSrv.response.emitted );
109 4 : EXPECT_TRUE( newActionSrv.response.error_msg.size() > 0 );
110 :
111 : // error, the joint names in motor pos and count are not the same
112 : //NOTE we use the time so if this test is run multiple time on same machine, the action will have a different name
113 4 : newActionSrv.request.newAction.action_name = "newAction_" + std::to_string(ros::Time::now().toSec());
114 4 : newActionSrv.request.newAction.action_motor_count.name.push_back("error_joint_1");
115 4 : newActionSrv.request.newAction.action_motor_count.count.push_back(1);
116 4 : EXPECT_TRUE( rosee_client.call(newActionSrv) );
117 4 : EXPECT_FALSE( newActionSrv.response.accepted );
118 4 : EXPECT_FALSE( newActionSrv.response.emitted );
119 4 : EXPECT_TRUE( newActionSrv.response.error_msg.size() > 0 );
120 :
121 : // correcting previous error, also emitting on yaml
122 4 : newActionSrv.request.newAction.action_motor_count.name.at(0) = "joint_1";
123 4 : newActionSrv.request.newAction.action_motor_count.count.at(0) = 1;
124 4 : newActionSrv.request.emitYaml = true;
125 4 : EXPECT_TRUE( rosee_client.call(newActionSrv) );
126 4 : EXPECT_TRUE( newActionSrv.response.accepted );
127 4 : EXPECT_TRUE( newActionSrv.response.emitted );
128 4 : EXPECT_FALSE( newActionSrv.response.error_msg.size() > 0 );
129 :
130 4 : }
131 :
132 : /***
133 : * Here we send some new action to newGraspingActionServer, and we see if we can retrieve them with another server, GraspingActionAvailable
134 : */
135 20 : TEST_F ( testServiceHandler, callNewActionAndRetrieve ) {
136 :
137 4 : sleep(1); //without this joint state publisher crashes I do not know why (it is useless in this test, but annoying prints on traceback would appear)
138 :
139 8 : ros::ServiceClient rosee_client_new_action, rosee_client_actions_available;
140 4 : initClient<rosee_msg::NewGenericGraspingActionSrv>(rosee_client_new_action, "/ros_end_effector/new_generic_grasping_action");
141 4 : initClient<rosee_msg::GraspingActionsAvailable>(rosee_client_actions_available, "/ros_end_effector/grasping_actions_available");
142 :
143 8 : rosee_msg::NewGenericGraspingActionSrv newActionSrv;
144 :
145 8 : std::string requestActionName = "newAction_TEST";
146 4 : newActionSrv.request.newAction.action_name = requestActionName;
147 4 : newActionSrv.request.newAction.action_motor_position.name.push_back("joint_1");
148 4 : newActionSrv.request.newAction.action_motor_position.position.push_back(1);
149 4 : newActionSrv.request.newAction.elements_involved.push_back("an_element");
150 4 : newActionSrv.request.emitYaml = false;
151 4 : EXPECT_TRUE( rosee_client_new_action.call(newActionSrv) );
152 4 : EXPECT_TRUE( newActionSrv.response.accepted );
153 4 : EXPECT_FALSE( newActionSrv.response.emitted );
154 4 : EXPECT_FALSE( newActionSrv.response.error_msg.size() > 0 );
155 :
156 :
157 8 : rosee_msg::GraspingActionsAvailable graspActionAvailable;
158 :
159 : //we ask for a not existing action, and check for the error
160 4 : graspActionAvailable.request.action_name = "newAction_TEST_NOT_EXISTENT";
161 4 : EXPECT_TRUE(rosee_client_actions_available.call(graspActionAvailable));
162 4 : EXPECT_EQ(graspActionAvailable.response.grasping_actions.size(), 0);
163 :
164 : //we ask for the requestActionName, and check if fields are the same sent by us
165 4 : graspActionAvailable.request.action_name = requestActionName;
166 : //Compulsory field, otherwise primitive is asked
167 4 : graspActionAvailable.request.action_type = 1;
168 4 : EXPECT_TRUE(rosee_client_actions_available.call(graspActionAvailable));
169 4 : EXPECT_EQ(1, graspActionAvailable.response.grasping_actions.size());
170 4 : if (graspActionAvailable.response.grasping_actions.size() > 0) {
171 8 : auto receivedAction = graspActionAvailable.response.grasping_actions.at(0);
172 4 : EXPECT_EQ(receivedAction.action_name, requestActionName);
173 4 : EXPECT_EQ(receivedAction.action_motor_positions.at(0).name.size(), newActionSrv.request.newAction.action_motor_position.name.size());
174 4 : EXPECT_EQ(receivedAction.action_motor_positions.at(0).name.at(0), newActionSrv.request.newAction.action_motor_position.name.at(0));
175 4 : EXPECT_EQ(receivedAction.action_motor_positions.at(0).position.at(0), newActionSrv.request.newAction.action_motor_position.position.at(0));
176 : //motor count was not filled here but it is filled by default by action costructor
177 4 : EXPECT_EQ(1, receivedAction.action_motor_count.name.size());
178 4 : EXPECT_EQ(receivedAction.action_motor_count.name.at(0), newActionSrv.request.newAction.action_motor_position.name.at(0));
179 4 : EXPECT_EQ(1, receivedAction.action_motor_count.count.at(0));
180 4 : EXPECT_EQ(1, receivedAction.elements_involved.size());
181 4 : if (receivedAction.elements_involved.size() > 0){
182 4 : EXPECT_EQ(receivedAction.elements_involved.at(0), newActionSrv.request.newAction.elements_involved.at(0));
183 : }
184 : }
185 :
186 :
187 :
188 :
189 4 : }
190 :
191 :
192 :
193 : } //namespace
194 :
195 :
196 :
197 4 : int main ( int argc, char **argv ) {
198 :
199 4 : if (argc < 2 ) {
200 :
201 0 : std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
202 0 : return -1;
203 : }
204 :
205 : /******************* Run tests on an isolated roscore ********************************************************/
206 : /* COMMENT ALL THIS block if you want to use roscore command by hand (useful for debugging the test) */
207 : //TODO I do not really understand why everything fails if I put this block at the beginning of prepareROSForTests function
208 4 : if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
209 : {
210 0 : perror("setenv");
211 0 : return 1;
212 : }
213 :
214 : //run roscore
215 8 : std::unique_ptr<ROSEE::TestUtils::Process> roscore;
216 4 : roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));
217 : /****************************************************************************************************/
218 :
219 4 : ros::init ( argc, argv, "testServiceHandler" );
220 :
221 4 : ros::param::set("robot_name", argv[1]);
222 :
223 4 : ::testing::InitGoogleTest ( &argc, argv );
224 4 : return RUN_ALL_TESTS();
225 12 : }
|
