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/EEInterface.h>
8 :
9 : #include <end_effector/RosServiceHandler.h>
10 : #include <end_effector/MapActionHandler.h>
11 :
12 : #include <rosee_msg/srv/new_generic_grasping_action_srv.hpp>
13 :
14 : #include <chrono>
15 : using namespace std::literals::chrono_literals;
16 :
17 :
18 : namespace {
19 :
20 : /**
21 : * @brief Test for the RosServiceHandler Class
22 : * It simply create the server and some clients which will test the server functions.
23 : * Hence, in this test a lot of ROSEE main class are not used like the FindAction
24 : */
25 : class testServiceHandler: public ::testing::Test {
26 :
27 :
28 : protected:
29 :
30 8 : testServiceHandler() {
31 8 : }
32 :
33 8 : virtual ~testServiceHandler() {
34 8 : }
35 :
36 8 : virtual void SetUp() override {
37 :
38 8 : }
39 :
40 8 : virtual void SetUp(int argc, char **argv) {
41 :
42 :
43 8 : node = ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testServiceHandler");
44 :
45 8 : ASSERT_NE(node, nullptr);
46 :
47 8 : std::string robot_name = argv[1];
48 :
49 8 : setenv("HAND_NAME",robot_name.c_str(),1);
50 :
51 40 : roseeExecutor.reset(new ROSEE::TestUtils::Process({"ros2", "launch", "end_effector", "test_rosee_startup_launch.py"}));
52 :
53 : }
54 :
55 8 : virtual void TearDown() override {
56 8 : }
57 :
58 : std::string robot_name;
59 : rclcpp::Node::SharedPtr node;
60 : std::unique_ptr<ROSEE::TestUtils::Process> roseeExecutor;
61 :
62 : template <class clientType>
63 12 : bool initClient( std::shared_ptr<rclcpp::Client<clientType>> &rosee_client, std::string serviceName) {
64 :
65 12 : rosee_client = node->create_client<clientType>(serviceName);
66 :
67 12 : RCLCPP_INFO(node->get_logger(), "Waiting for '%s' service for max 5 seconds...", serviceName.c_str());
68 12 : if (!rosee_client->wait_for_service(5s)) {
69 0 : RCLCPP_INFO(node->get_logger(), "Service'%s' not ready after seconds", serviceName.c_str());
70 0 : return false;
71 : }
72 12 : RCLCPP_INFO(node->get_logger(), "Service '%s' ready", serviceName.c_str());
73 :
74 12 : return true;
75 :
76 : };
77 :
78 : };
79 :
80 : /**
81 : * We call the service to include a new generic action multiple time, with both wrong and fallace requests, to see if the errors are
82 : * detected by the server, and correct action are accepted
83 : */
84 8 : TEST_F ( testServiceHandler, callNewAction ) {
85 :
86 4 : SetUp(argc_g, argv_g);
87 :
88 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)
89 :
90 0 : rclcpp::Client<rosee_msg::srv::NewGenericGraspingActionSrv>::SharedPtr rosee_client;
91 4 : ASSERT_TRUE(initClient<rosee_msg::srv::NewGenericGraspingActionSrv>(rosee_client, "/new_generic_grasping_action"));
92 :
93 8 : auto newActionSrv = std::make_shared<rosee_msg::srv::NewGenericGraspingActionSrv::Request>();
94 :
95 : //empty request, error
96 12 : auto result = rosee_client->async_send_request(newActionSrv);
97 4 : std::cout << rclcpp::spin_until_future_complete(node, result) << std::endl;
98 : //EXPECT_EQ( rclcpp::spin_until_future_complete(node, result), rclcpp::FutureReturnCode::SUCCESS);
99 4 : EXPECT_FALSE( result.get()->accepted );
100 4 : EXPECT_FALSE( result.get()->emitted );
101 4 : EXPECT_TRUE( result.get()->error_msg.size() > 0 );
102 :
103 : //empty joint pos, error
104 4 : newActionSrv->new_action.action_name = "newAction";
105 4 : result = rosee_client->async_send_request(newActionSrv);
106 4 : EXPECT_EQ( rclcpp::spin_until_future_complete(node, result), rclcpp::FutureReturnCode::SUCCESS);
107 4 : EXPECT_FALSE( result.get()->accepted );
108 4 : EXPECT_FALSE( result.get()->emitted );
109 4 : EXPECT_TRUE( result.get()->error_msg.size() > 0 );
110 :
111 : //correct request
112 4 : newActionSrv->new_action.action_motor_position.name.push_back("joint_1");
113 4 : newActionSrv->new_action.action_motor_position.position.push_back(1);
114 4 : result = rosee_client->async_send_request(newActionSrv);
115 4 : EXPECT_EQ( rclcpp::spin_until_future_complete(node, result), rclcpp::FutureReturnCode::SUCCESS);
116 4 : EXPECT_TRUE( result.get()->accepted );
117 4 : EXPECT_FALSE( result.get()->emitted );
118 4 : EXPECT_FALSE( result.get()->error_msg.size() > 0 );
119 :
120 : //same request, this time is must fail because a "test1" action is already present
121 4 : result = rosee_client->async_send_request(newActionSrv);
122 4 : EXPECT_EQ( rclcpp::spin_until_future_complete(node, result), rclcpp::FutureReturnCode::SUCCESS);
123 4 : EXPECT_FALSE( result.get()->accepted );
124 4 : EXPECT_FALSE(result.get()->emitted );
125 4 : EXPECT_TRUE( result.get()->error_msg.size() > 0 );
126 :
127 : // error, the joint names in motor pos and count are not the same
128 : //NOTE we use the time so if this test is run multiple time on same machine, the action will have a different name
129 4 : newActionSrv->new_action.action_name = "newAction_" + std::to_string(rclcpp::Clock().now().seconds());
130 4 : newActionSrv->new_action.action_motor_count.name.push_back("error_joint_1");
131 4 : newActionSrv->new_action.action_motor_count.count.push_back(1);
132 4 : result = rosee_client->async_send_request(newActionSrv);
133 4 : EXPECT_EQ( rclcpp::spin_until_future_complete(node, result), rclcpp::FutureReturnCode::SUCCESS);
134 4 : EXPECT_FALSE( result.get()->accepted );
135 4 : EXPECT_FALSE( result.get()->emitted );
136 4 : EXPECT_TRUE( result.get()->error_msg.size() > 0 );
137 :
138 : // correcting previous error, also emitting on yaml
139 4 : newActionSrv->new_action.action_motor_count.name.at(0) = "joint_1";
140 4 : newActionSrv->new_action.action_motor_count.count.at(0) = 1;
141 4 : newActionSrv->emit_yaml = true;
142 4 : result = rosee_client->async_send_request(newActionSrv);
143 4 : EXPECT_EQ( rclcpp::spin_until_future_complete(node, result), rclcpp::FutureReturnCode::SUCCESS);
144 4 : EXPECT_TRUE( result.get()->accepted );
145 4 : EXPECT_TRUE( result.get()->emitted );
146 4 : EXPECT_FALSE( result.get()->error_msg.size() > 0 );
147 :
148 : }
149 :
150 : /***
151 : * Here we send some new action to newGraspingActionServer, and we see if we can retrieve them with another server, GraspingActionAvailable
152 : */
153 8 : TEST_F ( testServiceHandler, callNewActionAndRetrieve ) {
154 :
155 4 : SetUp(argc_g, argv_g);
156 :
157 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)
158 :
159 4 : rclcpp::Client<rosee_msg::srv::NewGenericGraspingActionSrv>::SharedPtr rosee_client_new_action;
160 4 : rclcpp::Client<rosee_msg::srv::GraspingActionsAvailable>::SharedPtr rosee_client_actions_available;
161 :
162 4 : initClient<rosee_msg::srv::NewGenericGraspingActionSrv>(rosee_client_new_action, "/new_generic_grasping_action");
163 4 : initClient<rosee_msg::srv::GraspingActionsAvailable>(rosee_client_actions_available, "/grasping_actions_available");
164 :
165 8 : auto newActionSrv = std::make_shared<rosee_msg::srv::NewGenericGraspingActionSrv::Request>();
166 :
167 8 : std::string requestActionName = "newAction_TEST";
168 4 : newActionSrv->new_action.action_name = requestActionName;
169 4 : newActionSrv->new_action.action_motor_position.name.push_back("joint_1");
170 4 : newActionSrv->new_action.action_motor_position.position.push_back(1);
171 4 : newActionSrv->new_action.elements_involved.push_back("an_element");
172 4 : newActionSrv->emit_yaml = false;
173 8 : auto result = rosee_client_new_action->async_send_request(newActionSrv);
174 4 : EXPECT_EQ( rclcpp::spin_until_future_complete(node, result), rclcpp::FutureReturnCode::SUCCESS);
175 4 : EXPECT_TRUE( result.get()->accepted );
176 4 : EXPECT_FALSE( result.get()->emitted );
177 4 : EXPECT_FALSE( result.get()->error_msg.size() > 0 );
178 :
179 8 : auto graspActionAvailable = std::make_shared<rosee_msg::srv::GraspingActionsAvailable::Request>();
180 :
181 : //we ask for a not existing action, and check for the error
182 4 : graspActionAvailable->action_name = "newAction_TEST_NOT_EXISTENT";
183 8 : auto result2 = rosee_client_actions_available->async_send_request(graspActionAvailable);
184 4 : EXPECT_EQ( rclcpp::spin_until_future_complete(node, result2), rclcpp::FutureReturnCode::SUCCESS);
185 4 : EXPECT_EQ(result2.get()->grasping_actions.size(), 0);
186 :
187 : //we ask for the requestActionName, and check if fields are the same sent by us
188 4 : graspActionAvailable->action_name = requestActionName;
189 : //Compulsory field, otherwise primitive is asked
190 4 : graspActionAvailable->action_type = 1;
191 4 : result2 = rosee_client_actions_available->async_send_request(graspActionAvailable);
192 4 : EXPECT_EQ( rclcpp::spin_until_future_complete(node, result2), rclcpp::FutureReturnCode::SUCCESS);
193 4 : EXPECT_EQ(1, result2.get()->grasping_actions.size());
194 4 : if (result2.get()->grasping_actions.size() > 0) {
195 8 : auto receivedAction = result2.get()->grasping_actions.at(0);
196 4 : EXPECT_EQ(receivedAction.action_name, requestActionName);
197 4 : EXPECT_EQ(receivedAction.action_motor_positions.at(0).name.size(), newActionSrv->new_action.action_motor_position.name.size());
198 4 : EXPECT_EQ(receivedAction.action_motor_positions.at(0).name.at(0), newActionSrv->new_action.action_motor_position.name.at(0));
199 4 : EXPECT_EQ(receivedAction.action_motor_positions.at(0).position.at(0), newActionSrv->new_action.action_motor_position.position.at(0));
200 : //motor count was not filled here but it is filled by default by action costructor
201 4 : EXPECT_EQ(1, receivedAction.action_motor_count.name.size());
202 4 : EXPECT_EQ(receivedAction.action_motor_count.name.at(0), newActionSrv->new_action.action_motor_position.name.at(0));
203 4 : EXPECT_EQ(1, receivedAction.action_motor_count.count.at(0));
204 4 : EXPECT_EQ(1, receivedAction.elements_involved.size());
205 4 : if (receivedAction.elements_involved.size() > 0){
206 4 : EXPECT_EQ(receivedAction.elements_involved.at(0), newActionSrv->new_action.elements_involved.at(0));
207 : }
208 : }
209 4 : }
210 :
211 :
212 :
213 : } //namespace
214 :
215 4 : int main ( int argc, char **argv ) {
216 :
217 4 : if (argc < 2 ){
218 :
219 0 : std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
220 0 : return -1;
221 : }
222 :
223 4 : rclcpp::init ( argc, argv );
224 :
225 4 : ::testing::InitGoogleTest ( &argc, argv );
226 4 : ::testing::AddGlobalTestEnvironment(new MyTestEnvironment(argc, argv));
227 :
228 4 : return RUN_ALL_TESTS();
229 : }
|
