test_service_handler.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include "testUtils.h"
3 
4 #include <ros/ros.h>
5 
6 #include <end_effector/Parser.h>
7 #include <end_effector/EEInterface.h>
8 
9 #include <end_effector/RosServiceHandler.h>
10 #include <end_effector/MapActionHandler.h>
11 
12 #include <rosee_msg/NewGenericGraspingActionSrv.h>
13 
14 
15 
16 namespace {
17 
23 class testServiceHandler: public ::testing::Test {
24 
25 
26 protected:
27 
28  testServiceHandler() {
29  }
30 
31  virtual ~testServiceHandler() {
32  }
33 
34  virtual void SetUp() override {
35 
36  if (! nh.hasParam("robot_name")) {
37  std::cout << "[TEST FAIL: robot name not set on server]" << std::endl;
38  return;
39  }
40 
41  robot_name = "";
42  nh.getParam("robot_name", robot_name);
43 
44  std::string handNameArg = "hand_name:=" + robot_name;
45  roseeExecutor.reset(new ROSEE::TestUtils::Process({"roslaunch", "end_effector", "test_rosee_startup.launch", handNameArg}));
46 
47  }
48 
49  virtual void TearDown() override {
50  }
51 
52  std::string robot_name;
53  ros::NodeHandle nh;
54  std::unique_ptr<ROSEE::TestUtils::Process> roseeExecutor;
55 
56  template <class clientType>
57  bool initClient( ros::ServiceClient& rosee_client, std::string serviceName) {
58 
59  rosee_client = nh.serviceClient<clientType>(serviceName);
60 
61  rosee_client.waitForExistence();
62 
63  return true;
64 
65  };
66 
67 };
68 
73 TEST_F ( testServiceHandler, callNewAction ) {
74 
75  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  ros::ServiceClient rosee_client;
78  initClient<rosee_msg::NewGenericGraspingActionSrv>(rosee_client, "/ros_end_effector/new_generic_grasping_action");
79 
80  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  EXPECT_TRUE( rosee_client.call(newActionSrv) );
86  EXPECT_FALSE( newActionSrv.response.accepted );
87  EXPECT_FALSE( newActionSrv.response.emitted );
88  EXPECT_TRUE( newActionSrv.response.error_msg.size() > 0 );
89 
90  //empty joint pos, error
91  newActionSrv.request.newAction.action_name = "newAction";
92  EXPECT_TRUE( rosee_client.call(newActionSrv) );
93  EXPECT_FALSE( newActionSrv.response.accepted );
94  EXPECT_FALSE( newActionSrv.response.emitted );
95  EXPECT_TRUE( newActionSrv.response.error_msg.size() > 0 );
96 
97  //correct request
98  newActionSrv.request.newAction.action_motor_position.name.push_back("joint_1");
99  newActionSrv.request.newAction.action_motor_position.position.push_back(1);
100  EXPECT_TRUE( rosee_client.call(newActionSrv) );
101  EXPECT_TRUE( newActionSrv.response.accepted );
102  EXPECT_FALSE( newActionSrv.response.emitted );
103  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  EXPECT_TRUE( rosee_client.call(newActionSrv) );
107  EXPECT_FALSE( newActionSrv.response.accepted );
108  EXPECT_FALSE( newActionSrv.response.emitted );
109  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  newActionSrv.request.newAction.action_name = "newAction_" + std::to_string(ros::Time::now().toSec());
114  newActionSrv.request.newAction.action_motor_count.name.push_back("error_joint_1");
115  newActionSrv.request.newAction.action_motor_count.count.push_back(1);
116  EXPECT_TRUE( rosee_client.call(newActionSrv) );
117  EXPECT_FALSE( newActionSrv.response.accepted );
118  EXPECT_FALSE( newActionSrv.response.emitted );
119  EXPECT_TRUE( newActionSrv.response.error_msg.size() > 0 );
120 
121  // correcting previous error, also emitting on yaml
122  newActionSrv.request.newAction.action_motor_count.name.at(0) = "joint_1";
123  newActionSrv.request.newAction.action_motor_count.count.at(0) = 1;
124  newActionSrv.request.emitYaml = true;
125  EXPECT_TRUE( rosee_client.call(newActionSrv) );
126  EXPECT_TRUE( newActionSrv.response.accepted );
127  EXPECT_TRUE( newActionSrv.response.emitted );
128  EXPECT_FALSE( newActionSrv.response.error_msg.size() > 0 );
129 
130 }
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 TEST_F ( testServiceHandler, callNewActionAndRetrieve ) {
136 
137  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  ros::ServiceClient rosee_client_new_action, rosee_client_actions_available;
140  initClient<rosee_msg::NewGenericGraspingActionSrv>(rosee_client_new_action, "/ros_end_effector/new_generic_grasping_action");
141  initClient<rosee_msg::GraspingActionsAvailable>(rosee_client_actions_available, "/ros_end_effector/grasping_actions_available");
142 
143  rosee_msg::NewGenericGraspingActionSrv newActionSrv;
144 
145  std::string requestActionName = "newAction_TEST";
146  newActionSrv.request.newAction.action_name = requestActionName;
147  newActionSrv.request.newAction.action_motor_position.name.push_back("joint_1");
148  newActionSrv.request.newAction.action_motor_position.position.push_back(1);
149  newActionSrv.request.newAction.elements_involved.push_back("an_element");
150  newActionSrv.request.emitYaml = false;
151  EXPECT_TRUE( rosee_client_new_action.call(newActionSrv) );
152  EXPECT_TRUE( newActionSrv.response.accepted );
153  EXPECT_FALSE( newActionSrv.response.emitted );
154  EXPECT_FALSE( newActionSrv.response.error_msg.size() > 0 );
155 
156 
157  rosee_msg::GraspingActionsAvailable graspActionAvailable;
158 
159  //we ask for a not existing action, and check for the error
160  graspActionAvailable.request.action_name = "newAction_TEST_NOT_EXISTENT";
161  EXPECT_TRUE(rosee_client_actions_available.call(graspActionAvailable));
162  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  graspActionAvailable.request.action_name = requestActionName;
166  //Compulsory field, otherwise primitive is asked
167  graspActionAvailable.request.action_type = 1;
168  EXPECT_TRUE(rosee_client_actions_available.call(graspActionAvailable));
169  EXPECT_EQ(1, graspActionAvailable.response.grasping_actions.size());
170  if (graspActionAvailable.response.grasping_actions.size() > 0) {
171  auto receivedAction = graspActionAvailable.response.grasping_actions.at(0);
172  EXPECT_EQ(receivedAction.action_name, requestActionName);
173  EXPECT_EQ(receivedAction.action_motor_positions.at(0).name.size(), newActionSrv.request.newAction.action_motor_position.name.size());
174  EXPECT_EQ(receivedAction.action_motor_positions.at(0).name.at(0), newActionSrv.request.newAction.action_motor_position.name.at(0));
175  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  EXPECT_EQ(1, receivedAction.action_motor_count.name.size());
178  EXPECT_EQ(receivedAction.action_motor_count.name.at(0), newActionSrv.request.newAction.action_motor_position.name.at(0));
179  EXPECT_EQ(1, receivedAction.action_motor_count.count.at(0));
180  EXPECT_EQ(1, receivedAction.elements_involved.size());
181  if (receivedAction.elements_involved.size() > 0){
182  EXPECT_EQ(receivedAction.elements_involved.at(0), newActionSrv.request.newAction.elements_involved.at(0));
183  }
184  }
185 
186 
187 
188 
189 }
190 
191 
192 
193 } //namespace
194 
195 
196 
197 int main ( int argc, char **argv ) {
198 
199  if (argc < 2 ) {
200 
201  std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
202  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  if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
209  {
210  perror("setenv");
211  return 1;
212  }
213 
214  //run roscore
215  std::unique_ptr<ROSEE::TestUtils::Process> roscore;
216  roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));
217  /****************************************************************************************************/
218 
219  ros::init ( argc, argv, "testServiceHandler" );
220 
221  ros::param::set("robot_name", argv[1]);
222 
223  ::testing::InitGoogleTest ( &argc, argv );
224  return RUN_ALL_TESTS();
225 }
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::NodeHandle
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
main
int main(int argc, char **argv)
Definition: test_service_handler.cpp:197
EXPECT_FALSE
#define EXPECT_FALSE(args)
ros::param::set
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
EXPECT_EQ
#define EXPECT_EQ(a, b)
ros::ServiceClient::waitForExistence
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
ros::Time::now
static Time now()
EXPECT_TRUE
#define EXPECT_TRUE(args)
TEST_F
TEST_F(TestRunner, threaded_test)

end-effector
Author(s): Luca Muratore , Davide Torielli , Liana Bertoni
autogenerated on Tue Apr 5 2022 02:57:53