services_clients.cpp
Go to the documentation of this file.
1 #include <actionlib/client/simple_action_client.h>
2 #include <eigen_conversions/eigen_msg.h>
3 #include <ram_msgs/AdditiveManufacturingTrajectory.h>
4 #include <ram_path_planning/contours.hpp>
5 #include <ram_path_planning/donghong_ding.hpp>
6 #include <ram_path_planning/ContoursAction.h>
7 #include <ram_path_planning/DonghongDingAction.h>
8 #include <ros/package.h>
9 #include <ros/ros.h>
10 
11 #include <gtest/gtest.h>
12 
13 std::unique_ptr<ros::NodeHandle> nh;
14 
15 // Action clients
16 typedef actionlib::SimpleActionClient<ram_path_planning::DonghongDingAction> DonghongDingActionClient;
17 typedef actionlib::SimpleActionClient<ram_path_planning::ContoursAction> ContoursActionClient;
18 
19 std::unique_ptr<DonghongDingActionClient> donghong_ding_ac;
20 std::unique_ptr<ContoursActionClient> contours_ac;
21 
22 bool use_gui;
23 
24 TEST(TestSuite, testSrvExistence)
25 {
26  ram_path_planning::DonghongDing<ram_path_planning::DonghongDingAction> donghong_ding;
27  ram_path_planning::Contours<ram_path_planning::ContoursAction> contours;
28 
29  donghong_ding_ac.reset(new DonghongDingActionClient(donghong_ding.service_name_, true));
30  bool donghong_ding_exists(donghong_ding_ac->waitForServer(ros::Duration(1)));
31 
32  contours_ac.reset(new ContoursActionClient(contours.service_name_, true));
33  bool contours_exists(contours_ac->waitForServer(ros::Duration(1)));
34 
35  EXPECT_TRUE(donghong_ding_exists && contours_exists);
36 
37 }
40 TEST(TestSuite, testPolygonWithinternalContoursDongHongDing)
41 {
42  // Donghong Ding Algorithm
43  ram_path_planning::DonghongDingGoal goal;
44 
45  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/2_polygons_4_contours.yaml";
46  goal.deposited_material_width = 1e-3;
47  goal.contours_filtering_tolerance = 0.25e-3;
48  goal.height_between_layers = 2e-3;
49  goal.number_of_layers = 2;
50 
51  donghong_ding_ac->sendGoal(goal);
52  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
53 
54  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
55  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
56 
57  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
58 }
59 
60 TEST(TestSuite, testIntersectedPolygonsDongHongDing)
61 {
62  ram_path_planning::DonghongDingGoal goal;
63  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/polygon_intersection.yaml";
64  goal.deposited_material_width = 1e-3;
65  goal.contours_filtering_tolerance = 0.25e-3;
66  goal.height_between_layers = 0.001;
67  goal.number_of_layers = 2;
68 
69  donghong_ding_ac->sendGoal(goal);
70  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
71 
72  bool state_aborted(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::ABORTED);
73  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
74 
75  EXPECT_TRUE(success && state_aborted && !result->error_msg.empty()); // Should fail because polygons intersect
76 }
77 
78 TEST(TestSuite, testConvexPolygonDongHongDing)
79 {
80  ram_path_planning::DonghongDingGoal goal;
81  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/round_rectangle.yaml";
82  goal.deposited_material_width = 1e-3;
83  goal.contours_filtering_tolerance = 0.25e-3;
84  goal.height_between_layers = 2e-3;
85  goal.number_of_layers = 2;
86 
87  donghong_ding_ac->sendGoal(goal);
88  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
89 
90  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
91  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
92 
93  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
94 }
95 
96 TEST(TestSuite, testConcavePolygonBigDongHongDing)
97 {
98  ram_path_planning::DonghongDingGoal goal;
99  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/concave_4_contours_big.yaml";
100  goal.deposited_material_width = 0.1;
101  goal.contours_filtering_tolerance = 0.5e-3;
102  goal.height_between_layers = 0.1;
103  goal.number_of_layers = 2;
104 
105  donghong_ding_ac->sendGoal(goal);
106  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
107 
108  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
109  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
110 
111  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
112 }
113 
114 TEST(TestSuite, testConcavePolygonSmallDongHongDing)
115 {
116  ram_path_planning::DonghongDingGoal goal;
117  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/concave_4_contours_small.yaml";
118  goal.deposited_material_width = 1e-3;
119  goal.contours_filtering_tolerance = 0.25e-5;
120  goal.height_between_layers = 0.1;
121  goal.number_of_layers = 2;
122 
123  donghong_ding_ac->sendGoal(goal);
124  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
125 
126  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
127  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
128 
129  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
130 }
131 
132 TEST(TestSuite, testStarDongHongDing)
133 {
134  ram_path_planning::DonghongDingGoal goal;
135  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/star.yaml";
136  goal.deposited_material_width = 1e-3;
137  goal.contours_filtering_tolerance = 0.25e-3;
138  goal.height_between_layers = 0.1;
139  goal.number_of_layers = 2;
140 
141  donghong_ding_ac->sendGoal(goal);
142  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
143 
144  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
145  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
146 
147  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
148 }
149 
151 
152 TEST(TestSuite, testSTLFileTwistedPyramidDongHongDing)
153 {
154  ram_path_planning::DonghongDingGoal goal;
155  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/twisted_pyramid.stl";
156  goal.deposited_material_width = 1e-3;
157  goal.contours_filtering_tolerance = 1e-3;
158  goal.height_between_layers = 5e-3;
159 
160  donghong_ding_ac->sendGoal(goal);
161  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
162 
163  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
164  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
165 
166  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
167 }
168 
169 TEST(TestSuite, testPLYFileTwistedPyramidDongHongDing)
170 {
171  ram_path_planning::DonghongDingGoal goal;
172  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/twisted_pyramid.ply";
173  goal.deposited_material_width = 0.001;
174  goal.contours_filtering_tolerance = 0.5 * 1e-3;
175  goal.height_between_layers = 0.005;
176 
177  donghong_ding_ac->sendGoal(goal);
178  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
179 
180  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
181  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
182 
183  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
184 }
185 
186 TEST(TestSuite, testOBJFileTwistedPyramidDongHongDing)
187 {
188  ram_path_planning::DonghongDingGoal goal;
189  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/twisted_pyramid.obj";
190  goal.deposited_material_width = 0.001;
191  goal.contours_filtering_tolerance = 0.005 * 1e-3;
192  goal.height_between_layers = 0.005;
193 
194  donghong_ding_ac->sendGoal(goal);
195  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
196 
197  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
198  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
199 
200  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
201 }
202 
203 TEST(TestSuite, testPLYFileTwoTwistedPyramidsDongHongDing)
204 {
205  ram_path_planning::DonghongDingGoal goal;
206  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/two_twisted_pyramids.ply";
207  goal.deposited_material_width = 1e-3;
208  goal.contours_filtering_tolerance = 0.25e-3;
209  goal.height_between_layers = 5e-3;
210 
211  donghong_ding_ac->sendGoal(goal);
212  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
213 
214  bool state_aborted(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::ABORTED);
215  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
216 
217  EXPECT_TRUE(success && state_aborted && !result->error_msg.empty()); // Not implemented yet!
218 }
219 
220 TEST(TestSuite, testConeTruncatedDongHongDing)
221 {
222  ram_path_planning::DonghongDingGoal goal;
223  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/cone_truncated.ply";
224  goal.deposited_material_width = 0.001;
225  goal.contours_filtering_tolerance = 0.6 * 1e-3;
226  goal.height_between_layers = 0.005;
227 
228  donghong_ding_ac->sendGoal(goal);
229  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
230 
231  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
232  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
233 
234  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
235 }
236 
237 TEST(TestSuite, testDomeDongHongDing)
238 {
239  ram_path_planning::DonghongDingGoal goal;
240  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/dome.ply";
241  goal.deposited_material_width = 0.001;
242  goal.contours_filtering_tolerance = 0.5 * 1e-3;
243  goal.height_between_layers = 0.002;
244 
245  donghong_ding_ac->sendGoal(goal);
246  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
247 
248  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
249  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
250 
251  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
252 }
253 
254 TEST(TestSuite, testInversedPyramidDongHongDing)
255 {
256  ram_path_planning::DonghongDingGoal goal;
257  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/inversed_pyramid.ply";
258  goal.deposited_material_width = 0.001;
259  goal.contours_filtering_tolerance = 0;
260  goal.height_between_layers = 0.005;
261 
262  donghong_ding_ac->sendGoal(goal);
263  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
264 
265  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
266  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
267 
268  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
269 }
270 
271 TEST(TestSuite, testSTLCubeNonZSlicingDongHongDing)
272 {
273  ram_path_planning::DonghongDingGoal goal;
274  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/cube.stl";
275  goal.deposited_material_width = 1e-3;
276  goal.contours_filtering_tolerance = 1e-3;
277  goal.height_between_layers = 5e-3;
278  // Slicing vector is not Z:
279  goal.slicing_direction.x = -0.15;
280  goal.slicing_direction.y = -0.2;
281  goal.slicing_direction.z = 1;
282 
283  donghong_ding_ac->sendGoal(goal);
284  bool success(donghong_ding_ac->waitForResult(ros::Duration(1)));
285 
286  bool state_succeeded(donghong_ding_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
287  ram_path_planning::DonghongDingResultConstPtr result(donghong_ding_ac->getResult());
288 
289  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
290 }
291 
294 TEST(TestSuite, testPolygonWithinternalContours)
295 {
296  // Contours Algorithm
297  ram_path_planning::ContoursGoal goal;
298  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/2_polygons_4_contours.yaml";
299  goal.deposited_material_width = 1e-3;
300  goal.height_between_layers = 2e-3;
301  goal.number_of_layers = 2;
302 
303  contours_ac->sendGoal(goal);
304  bool success(contours_ac->waitForResult(ros::Duration(1)));
305 
306  bool state_aborted(contours_ac->getState().state_ == actionlib::SimpleClientGoalState::ABORTED);
307  ram_path_planning::ContoursResultConstPtr result(contours_ac->getResult());
308 
309  EXPECT_TRUE(success && state_aborted && !result->error_msg.empty()); // Should fail because polygons contain internal contours
310 }
311 
312 TEST(TestSuite, testIntersectedPolygonsContours)
313 {
314  ram_path_planning::ContoursGoal goal;
315  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/polygon_intersection.yaml";
316  goal.deposited_material_width = 1e-3;
317  goal.height_between_layers = 0.001;
318  goal.number_of_layers = 2;
319 
320  contours_ac->sendGoal(goal);
321  bool success(contours_ac->waitForResult(ros::Duration(1)));
322 
323  bool state_aborted(contours_ac->getState().state_ == actionlib::SimpleClientGoalState::ABORTED);
324  ram_path_planning::ContoursResultConstPtr result(contours_ac->getResult());
325 
326  EXPECT_TRUE(success && state_aborted && !result->error_msg.empty()); // Should fail because polygons intersect
327 }
328 
329 TEST(TestSuite, testConvexPolygonContours)
330 {
331  ram_path_planning::ContoursGoal goal;
332  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/round_rectangle.yaml";
333  goal.deposited_material_width = 1e-3;
334  goal.height_between_layers = 2e-3;
335  goal.number_of_layers = 2;
336 
337  contours_ac->sendGoal(goal);
338  bool success(contours_ac->waitForResult(ros::Duration(1)));
339 
340  bool state_succeeded(contours_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
341  ram_path_planning::ContoursResultConstPtr result(contours_ac->getResult());
342 
343  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
344 }
345 
346 TEST(TestSuite, testStarContours)
347 {
348  ram_path_planning::ContoursGoal goal;
349  goal.file = ros::package::getPath("ram_path_planning") + "/yaml/star.yaml";
350  goal.deposited_material_width = 1e-3;
351  goal.height_between_layers = 0.1;
352  goal.number_of_layers = 2;
353 
354  contours_ac->sendGoal(goal);
355  bool success(contours_ac->waitForResult(ros::Duration(1)));
356 
357  bool state_succeeded(contours_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
358  ram_path_planning::ContoursResultConstPtr result(contours_ac->getResult());
359 
360  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
361 }
362 
364 
365 TEST(TestSuite, testSTLFileTwistedPyramidContours)
366 {
367  ram_path_planning::ContoursGoal goal;
368  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/twisted_pyramid.stl";
369  goal.deposited_material_width = 1e-3;
370  goal.height_between_layers = 5e-3;
371 
372  contours_ac->sendGoal(goal);
373  bool success(contours_ac->waitForResult(ros::Duration(1)));
374 
375  bool state_succeeded(contours_ac->getState().state_ == actionlib::SimpleClientGoalState::SUCCEEDED);
376  ram_path_planning::ContoursResultConstPtr result(contours_ac->getResult());
377 
378  EXPECT_TRUE(success && state_succeeded && result->error_msg.empty());
379 }
380 
381 TEST(TestSuite, testPLYFileTwoTwistedPyramidsContours)
382 {
383  ram_path_planning::ContoursGoal goal;
384  goal.file = ros::package::getPath("ram_path_planning") + "/meshes/two_twisted_pyramids.ply";
385  goal.deposited_material_width = 1e-3;
386  goal.height_between_layers = 5e-3;
387 
388  contours_ac->sendGoal(goal);
389  bool success(contours_ac->waitForResult(ros::Duration(1)));
390 
391  bool state_aborted(contours_ac->getState().state_ == actionlib::SimpleClientGoalState::ABORTED);
392  ram_path_planning::ContoursResultConstPtr result(contours_ac->getResult());
393 
394  EXPECT_TRUE(success && state_aborted && !result->error_msg.empty()); // Not implemented yet!
395 
396  if (use_gui)
397  ROS_ERROR_STREAM("PLEASE CLOSE THE VTK WINDOW TO TERMINATE");
398 }
399 
400 int main(int argc,
401  char **argv)
402 {
403  ros::init(argc, argv, "path_planning_test_client");
404  nh.reset(new ros::NodeHandle);
405 
406  nh->param<bool>("use_gui", use_gui, false);
407  if (use_gui)
408  ROS_ERROR_STREAM("Press 'enter' in the terminal to go to the next step");
409 
410  testing::InitGoogleTest(&argc, argv);
411  return RUN_ALL_TESTS();
412 }
ros::NodeHandle
donghong_ding.hpp
nh
std::unique_ptr< ros::NodeHandle > nh
Definition: services_clients.cpp:13
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: services_clients.cpp:400
ram_path_planning::PathPlanningAlgorithm::service_name_
const std::string service_name_
Definition: path_planning_algorithm.hpp:31
ram_path_planning::Contours
Definition: contours.hpp:9
donghong_ding_ac
std::unique_ptr< DonghongDingActionClient > donghong_ding_ac
Definition: services_clients.cpp:19
DonghongDingActionClient
actionlib::SimpleActionClient< ram_path_planning::DonghongDingAction > DonghongDingActionClient
Definition: services_clients.cpp:16
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
TEST
TEST(TestSuite, testSrvExistence)
Definition: services_clients.cpp:24
ContoursActionClient
actionlib::SimpleActionClient< ram_path_planning::ContoursAction > ContoursActionClient
Definition: services_clients.cpp:17
use_gui
bool use_gui
Definition: services_clients.cpp:22
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
ram_path_planning::DonghongDing
Definition: donghong_ding.hpp:10
simple_action_client.h
eigen_msg.h
contours_ac
std::unique_ptr< ContoursActionClient > contours_ac
Definition: services_clients.cpp:20

ram_path_planning
Author(s): Andres Campos - Institut Maupertuis
autogenerated on Mon Jun 10 2019 14:50:03