init_locations.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include "gazebo_msgs/SetModelState.h"
3 #include "gazebo_msgs/GetModelState.h"
4 #include <cstdlib>
5 #include <math.h>
6 
7 #include <fstream>
8 #include <sstream>
9 
10 using namespace std;
11 
12 #define PI 3.1415926
13 
14 const float camera_height = 1.1;
15 
16 bool setStartPose(ros::ServiceClient client, int rbtID, float x, float y, float z = camera_height)
17 {
18  float x0 = x, y0 = y, z0 = z;
19  float theta0 = 0.0;
20  float ox0 = 0.0, oy0 = 0.0, oz0 = 1.0;
21  geometry_msgs::Pose start_pose0;
22  start_pose0.position.x = x0;
23  start_pose0.position.y = y0;
24  start_pose0.position.z = z0;
25  start_pose0.orientation.x = ox0*sin(theta0/2);
26  start_pose0.orientation.y = oy0*sin(theta0/2);
27  start_pose0.orientation.z = oz0*sin(theta0/2);
28  start_pose0.orientation.w = cos(theta0/2);
29  geometry_msgs::Twist start_twist0;
30  start_twist0.linear.x = 0.0;
31  start_twist0.linear.y = 0.0;
32  start_twist0.linear.z = 0.0;
33  start_twist0.angular.x = 0.0;
34  start_twist0.angular.y = 0.0;
35  start_twist0.angular.z = 0.0;
36  gazebo_msgs::ModelState modelstate0;
37  char name[100];
38  sprintf(name, "robot_%d", rbtID);
39  modelstate0.model_name = (std::string)name;
40  modelstate0.reference_frame = (std::string) "world";
41  modelstate0.pose = start_pose0;
42  modelstate0.twist = start_twist0;
43  gazebo_msgs::SetModelState setmodelstate0;
44  setmodelstate0.request.model_state = modelstate0;
45  //ROS_INFO("call server to move a model");
46  if (client.call(setmodelstate0))
47  {
48  //ROS_INFO("success");
49  }
50  else
51  {
52  ROS_ERROR("Failed to call service");
53  return false;
54  }
55  return true;
56 }
57 
58 int main(int argc, char **argv)
59 {
60  ros::init(argc, argv, "ModelState_client");
61 
62  ros::NodeHandle n;
63 
64  // client
65  ros::ServiceClient sclient0 = n.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
66  ros::ServiceClient gclient0 = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
67 
68  bool poseSetSuccess;
69 
70 /*
71  // suncad2_1f 20rbt
72  // 0
73  poseSetSuccess = setStartPose(sclient0, 0, 0.0, -5.0);
74  if(!poseSetSuccess) printf("set pose failed\n");
75  // 1
76  poseSetSuccess = setStartPose(sclient0, 1, 0.5, -5.0);
77  if(!poseSetSuccess) printf("set pose failed\n");
78  // 2
79  poseSetSuccess = setStartPose(sclient0, 2, 1.0, -5.0);
80  if(!poseSetSuccess) printf("set pose failed\n");
81  // 3
82  poseSetSuccess = setStartPose(sclient0, 3, 1.5, -5.0);
83  if(!poseSetSuccess) printf("set pose failed\n");
84  // 4
85  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -5.0);
86  if(!poseSetSuccess) printf("set pose failed\n");
87  // 5
88  poseSetSuccess = setStartPose(sclient0, 5, 2.5, -5.0);
89  if(!poseSetSuccess) printf("set pose failed\n");
90  // 6
91  poseSetSuccess = setStartPose(sclient0, 6, 3.0, -5.0);
92  if(!poseSetSuccess) printf("set pose failed\n");
93  // 7
94  poseSetSuccess = setStartPose(sclient0, 7, 3.5, -5.0);
95  if(!poseSetSuccess) printf("set pose failed\n");
96  // 8
97  poseSetSuccess = setStartPose(sclient0, 8, 4.0, -5.0);
98  if(!poseSetSuccess) printf("set pose failed\n");
99  // 9
100  poseSetSuccess = setStartPose(sclient0, 9, 4.5, -5.0);
101  if(!poseSetSuccess) printf("set pose failed\n");
102  // 10
103  poseSetSuccess = setStartPose(sclient0, 10, 0.0, -4.5);
104  if(!poseSetSuccess) printf("set pose failed\n");
105  // 11
106  poseSetSuccess = setStartPose(sclient0, 11, 0.5, -4.5);
107  if(!poseSetSuccess) printf("set pose failed\n");
108  // 12
109  poseSetSuccess = setStartPose(sclient0, 12, 1.0, -4.5);
110  if(!poseSetSuccess) printf("set pose failed\n");
111  // 13
112  poseSetSuccess = setStartPose(sclient0, 13, 1.5, -4.5);
113  if(!poseSetSuccess) printf("set pose failed\n");
114  // 14
115  poseSetSuccess = setStartPose(sclient0, 14, 2.0, -4.5);
116  if(!poseSetSuccess) printf("set pose failed\n");
117  // 15
118  poseSetSuccess = setStartPose(sclient0, 15, 2.5, -4.5);
119  if(!poseSetSuccess) printf("set pose failed\n");
120  // 16
121  poseSetSuccess = setStartPose(sclient0, 16, 3.0, -4.5);
122  if(!poseSetSuccess) printf("set pose failed\n");
123  // 17
124  poseSetSuccess = setStartPose(sclient0, 17, 3.5, -4.5);
125  if(!poseSetSuccess) printf("set pose failed\n");
126  // 18
127  poseSetSuccess = setStartPose(sclient0, 18, 4.0, -4.5);
128  if(!poseSetSuccess) printf("set pose failed\n");
129  // 19
130  poseSetSuccess = setStartPose(sclient0, 19, 4.5, -4.5);
131  if(!poseSetSuccess) printf("set pose failed\n");
132 //*/
133 
134 // // sdu
135 // // 0
136 // poseSetSuccess = setStartPose(sclient0, 0, -3.0, 0.0);
137 // if(!poseSetSuccess) printf("set pose failed\n");
138 // // 1
139 // poseSetSuccess = setStartPose(sclient0, 1, -3.0, -1.0);
140 // if(!poseSetSuccess) printf("set pose failed\n");
141 // // 2
142 // poseSetSuccess = setStartPose(sclient0, 2, -3.0, -2.0);
143 // if(!poseSetSuccess) printf("set pose failed\n");
144 // // 3
145 // poseSetSuccess = setStartPose(sclient0, 3, -4.0, -2.0);
146 // if(!poseSetSuccess) printf("set pose failed\n");
147 // //*/
148 
149 
150 /*
151  // pku3
152  // 0
153  poseSetSuccess = setStartPose(sclient0, 0, 1.0, 1.0);
154  if(!poseSetSuccess) printf("set pose failed\n");
155  // 1
156  poseSetSuccess = setStartPose(sclient0, 1, 1.0, -1.0);
157  if(!poseSetSuccess) printf("set pose failed\n");
158  // 2
159  poseSetSuccess = setStartPose(sclient0, 2, 1.0, 0.0);
160  if(!poseSetSuccess) printf("set pose failed\n");
161  // 3
162  poseSetSuccess = setStartPose(sclient0, 3, -4.0, -2.0);
163  if(!poseSetSuccess) printf("set pose failed\n");
164  // 4
165  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -3.0);
166  if(!poseSetSuccess) printf("set pose failed\n");
167  // 5
168  poseSetSuccess = setStartPose(sclient0, 5, 0.0, -2.5);
169  if(!poseSetSuccess) printf("set pose failed\n");
170  // 6
171  poseSetSuccess = setStartPose(sclient0, 6, 0.5, -2.5);
172  if(!poseSetSuccess) printf("set pose failed\n");
173  // 7
174  poseSetSuccess = setStartPose(sclient0, 7, 1.0, -2.5);
175  if(!poseSetSuccess) printf("set pose failed\n");
176  // 8
177  poseSetSuccess = setStartPose(sclient0, 8, 1.5, -2.5);
178  if(!poseSetSuccess) printf("set pose failed\n");
179  // 9
180  poseSetSuccess = setStartPose(sclient0, 9, 2.0, -2.5);
181  if(!poseSetSuccess) printf("set pose failed\n");
182 //*/
183 
184 /*
185  // bfa_home
186  // 0
187  poseSetSuccess = setStartPose(sclient0, 0, 2.0, 3.0);
188  if(!poseSetSuccess) printf("set pose failed\n");
189  // 1
190  poseSetSuccess = setStartPose(sclient0, 1, 2.0, 2.5);
191  if(!poseSetSuccess) printf("set pose failed\n");
192  // 2
193  poseSetSuccess = setStartPose(sclient0, 2, -1.0, -2.0);
194  if(!poseSetSuccess) printf("set pose failed\n");
195  // 3
196  poseSetSuccess = setStartPose(sclient0, 3, -4.0, -2.0);
197  if(!poseSetSuccess) printf("set pose failed\n");
198  // 4
199  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -3.0);
200  if(!poseSetSuccess) printf("set pose failed\n");
201  // 5
202  poseSetSuccess = setStartPose(sclient0, 5, 0.0, -2.5);
203  if(!poseSetSuccess) printf("set pose failed\n");
204  // 6
205  poseSetSuccess = setStartPose(sclient0, 6, 0.5, -2.5);
206  if(!poseSetSuccess) printf("set pose failed\n");
207  // 7
208  poseSetSuccess = setStartPose(sclient0, 7, 1.0, -2.5);
209  if(!poseSetSuccess) printf("set pose failed\n");
210  // 8
211  poseSetSuccess = setStartPose(sclient0, 8, 1.5, -2.5);
212  if(!poseSetSuccess) printf("set pose failed\n");
213  // 9
214  poseSetSuccess = setStartPose(sclient0, 9, 2.0, -2.5);
215  if(!poseSetSuccess) printf("set pose failed\n");
216 //*/
217 
218 /*
219  // pku1
220  // 0
221  poseSetSuccess = setStartPose(sclient0, 0, 5.0, -2.0);
222  if(!poseSetSuccess) printf("set pose failed\n");
223  // 1
224  poseSetSuccess = setStartPose(sclient0, 1, 2.0, -2.0);
225  if(!poseSetSuccess) printf("set pose failed\n");
226  // 2
227  poseSetSuccess = setStartPose(sclient0, 2, -1.0, -2.0);
228  if(!poseSetSuccess) printf("set pose failed\n");
229  // 3
230  poseSetSuccess = setStartPose(sclient0, 3, -4.0, -2.0);
231  if(!poseSetSuccess) printf("set pose failed\n");
232  // 4
233  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -3.0);
234  if(!poseSetSuccess) printf("set pose failed\n");
235  // 5
236  poseSetSuccess = setStartPose(sclient0, 5, 0.0, -2.5);
237  if(!poseSetSuccess) printf("set pose failed\n");
238  // 6
239  poseSetSuccess = setStartPose(sclient0, 6, 0.5, -2.5);
240  if(!poseSetSuccess) printf("set pose failed\n");
241  // 7
242  poseSetSuccess = setStartPose(sclient0, 7, 1.0, -2.5);
243  if(!poseSetSuccess) printf("set pose failed\n");
244  // 8
245  poseSetSuccess = setStartPose(sclient0, 8, 1.5, -2.5);
246  if(!poseSetSuccess) printf("set pose failed\n");
247  // 9
248  poseSetSuccess = setStartPose(sclient0, 9, 2.0, -2.5);
249  if(!poseSetSuccess) printf("set pose failed\n");
250 //*/
251 
252 
253  // mp01
254  // 0
255  poseSetSuccess = setStartPose(sclient0, 0, 2.0, -3.0);
256  if(!poseSetSuccess) printf("set pose failed\n");
257  // 1
258  poseSetSuccess = setStartPose(sclient0, 1, 1.0, -3.0);
259  if(!poseSetSuccess) printf("set pose failed\n");
260  // 2
261  poseSetSuccess = setStartPose(sclient0, 2, 0.0, -3.0);
262  if(!poseSetSuccess) printf("set pose failed\n");
263  // 3
264  poseSetSuccess = setStartPose(sclient0, 3, -1.0, -3.0);
265  if(!poseSetSuccess) printf("set pose failed\n");
266  // 4
267  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -3.0);
268  if(!poseSetSuccess) printf("set pose failed\n");
269  // 5
270  poseSetSuccess = setStartPose(sclient0, 5, 0.0, -2.5);
271  if(!poseSetSuccess) printf("set pose failed\n");
272  // 6
273  poseSetSuccess = setStartPose(sclient0, 6, 0.5, -2.5);
274  if(!poseSetSuccess) printf("set pose failed\n");
275  // 7
276  poseSetSuccess = setStartPose(sclient0, 7, 1.0, -2.5);
277  if(!poseSetSuccess) printf("set pose failed\n");
278  // 8
279  poseSetSuccess = setStartPose(sclient0, 8, 1.5, -2.5);
280  if(!poseSetSuccess) printf("set pose failed\n");
281  // 9
282  poseSetSuccess = setStartPose(sclient0, 9, 2.0, -2.5);
283  if(!poseSetSuccess) printf("set pose failed\n");
284 //*/
285 
286 /*
287  // sun4e5b_1f
288  // 0
289  poseSetSuccess = setStartPose(sclient0, 0, 1.0, -3.0);
290  if(!poseSetSuccess) printf("set pose failed\n");
291  // 1
292  poseSetSuccess = setStartPose(sclient0, 1, 1.5, -3.0);
293  if(!poseSetSuccess) printf("set pose failed\n");
294  // 2
295  poseSetSuccess = setStartPose(sclient0, 2, 2.0, -3.0);
296  if(!poseSetSuccess) printf("set pose failed\n");
297  // 3
298  poseSetSuccess = setStartPose(sclient0, 3, 2.5, -3.0);
299  if(!poseSetSuccess) printf("set pose failed\n");
300  // 4
301  poseSetSuccess = setStartPose(sclient0, 4, 3.0, -3.0);
302  if(!poseSetSuccess) printf("set pose failed\n");
303  // 5
304  poseSetSuccess = setStartPose(sclient0, 5, 1.0, -2.5);
305  if(!poseSetSuccess) printf("set pose failed\n");
306  // 6
307  poseSetSuccess = setStartPose(sclient0, 6, 1.5, -2.5);
308  if(!poseSetSuccess) printf("set pose failed\n");
309  // 7
310  poseSetSuccess = setStartPose(sclient0, 7, 2.0, -2.5);
311  if(!poseSetSuccess) printf("set pose failed\n");
312  // 8
313  poseSetSuccess = setStartPose(sclient0, 8, 2.5, -2.5);
314  if(!poseSetSuccess) printf("set pose failed\n");
315  // 9
316  poseSetSuccess = setStartPose(sclient0, 9, 3.0, -2.5);
317  if(!poseSetSuccess) printf("set pose failed\n");
318 //*/
319 
320 /*
321  // sun65f8_2f
322  // 0
323  poseSetSuccess = setStartPose(sclient0, 0, 0.0, -3.0);
324  if(!poseSetSuccess) printf("set pose failed\n");
325  // 1
326  poseSetSuccess = setStartPose(sclient0, 1, 0.5, -3.0);
327  if(!poseSetSuccess) printf("set pose failed\n");
328  // 2
329  poseSetSuccess = setStartPose(sclient0, 2, 1.0, -3.0);
330  if(!poseSetSuccess) printf("set pose failed\n");
331  // 3
332  poseSetSuccess = setStartPose(sclient0, 3, 1.5, -3.0);
333  if(!poseSetSuccess) printf("set pose failed\n");
334  // 4
335  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -3.0);
336  if(!poseSetSuccess) printf("set pose failed\n");
337  // 5
338  poseSetSuccess = setStartPose(sclient0, 5, 0.0, -2.5);
339  if(!poseSetSuccess) printf("set pose failed\n");
340  // 6
341  poseSetSuccess = setStartPose(sclient0, 6, 0.5, -2.5);
342  if(!poseSetSuccess) printf("set pose failed\n");
343  // 7
344  poseSetSuccess = setStartPose(sclient0, 7, 1.0, -2.5);
345  if(!poseSetSuccess) printf("set pose failed\n");
346  // 8
347  poseSetSuccess = setStartPose(sclient0, 8, 1.5, -2.5);
348  if(!poseSetSuccess) printf("set pose failed\n");
349  // 9
350  poseSetSuccess = setStartPose(sclient0, 9, 2.0, -2.5);
351  if(!poseSetSuccess) printf("set pose failed\n");
352 //*/
353 
354 /*
355 // sun0d1d_1f
356  // 0
357  poseSetSuccess = setStartPose(sclient0, 0, -6.0, 0.0);
358  if(!poseSetSuccess) printf("set pose failed\n");
359  // 1
360  poseSetSuccess = setStartPose(sclient0, 1, -6.0, -0.5);
361  if(!poseSetSuccess) printf("set pose failed\n");
362  // 2
363  poseSetSuccess = setStartPose(sclient0, 2, -6.0, -1.0);
364  if(!poseSetSuccess) printf("set pose failed\n");
365  // 3
366  poseSetSuccess = setStartPose(sclient0, 3, -6.0, -1.5);
367  if(!poseSetSuccess) printf("set pose failed\n");
368  // 4
369  poseSetSuccess = setStartPose(sclient0, 4, -6.0, -2.0);
370  if(!poseSetSuccess) printf("set pose failed\n");
371  // 5
372  poseSetSuccess = setStartPose(sclient0, 5, -7.0, -0.0);
373  if(!poseSetSuccess) printf("set pose failed\n");
374  // 6
375  poseSetSuccess = setStartPose(sclient0, 6, -7.0, -0.5);
376  if(!poseSetSuccess) printf("set pose failed\n");
377  // 7
378  poseSetSuccess = setStartPose(sclient0, 7, -7.0, -1.0);
379  if(!poseSetSuccess) printf("set pose failed\n");
380  // 8
381  poseSetSuccess = setStartPose(sclient0, 8, -7.0, -1.5);
382  if(!poseSetSuccess) printf("set pose failed\n");
383  // 9
384  poseSetSuccess = setStartPose(sclient0, 9, -7.0, -2.0);
385  if(!poseSetSuccess) printf("set pose failed\n");
386 //*/
387 
388 /*
389 // mp01_2
390  // 0
391  poseSetSuccess = setStartPose(sclient0, 0, 2.0, -3.0);
392  if(!poseSetSuccess) printf("set pose failed\n");
393  // 1
394  poseSetSuccess = setStartPose(sclient0, 1, 1.0, -3.0);
395  if(!poseSetSuccess) printf("set pose failed\n");
396  // 2
397  poseSetSuccess = setStartPose(sclient0, 2, 0.0, -3.0);
398  if(!poseSetSuccess) printf("set pose failed\n");
399  // 3
400  poseSetSuccess = setStartPose(sclient0, 3, -1.0, -3.0);
401  if(!poseSetSuccess) printf("set pose failed\n");
402  // 4
403  poseSetSuccess = setStartPose(sclient0, 4, -4.0, 2.0);
404  if(!poseSetSuccess) printf("set pose failed\n");
405  // 5
406  poseSetSuccess = setStartPose(sclient0, 5, 0.0, 1.5);
407  if(!poseSetSuccess) printf("set pose failed\n");
408  // 6
409  poseSetSuccess = setStartPose(sclient0, 6, -1.0, 1.5);
410  if(!poseSetSuccess) printf("set pose failed\n");
411  // 7
412  poseSetSuccess = setStartPose(sclient0, 7, -2.0, 1.5);
413  if(!poseSetSuccess) printf("set pose failed\n");
414  // 8
415  poseSetSuccess = setStartPose(sclient0, 8, -3.0, 1.5);
416  if(!poseSetSuccess) printf("set pose failed\n");
417  // 9
418  poseSetSuccess = setStartPose(sclient0, 9, -4.0, 1.5);
419  if(!poseSetSuccess) printf("set pose failed\n");
420 //*/
421 
422 /*
423 // mp02_timefirst not finished
424  // 0
425  poseSetSuccess = setStartPose(sclient0, 0, -1.5, 0.5);
426  if(!poseSetSuccess) printf("set pose failed\n");
427  // 1
428  poseSetSuccess = setStartPose(sclient0, 1, -1.0, 0.5);
429  if(!poseSetSuccess) printf("set pose failed\n");
430  // 2
431  poseSetSuccess = setStartPose(sclient0, 2, -0.5, 0.5);
432  if(!poseSetSuccess) printf("set pose failed\n");
433  // 3
434  poseSetSuccess = setStartPose(sclient0, 3, 0.0, 0.5);
435  if(!poseSetSuccess) printf("set pose failed\n");
436  // 4
437  poseSetSuccess = setStartPose(sclient0, 4, 0.5, 0.5);
438  if(!poseSetSuccess) printf("set pose failed\n");
439  // 5
440  poseSetSuccess = setStartPose(sclient0, 5, -1.5, 1.0);
441  if(!poseSetSuccess) printf("set pose failed\n");
442  // 6
443  poseSetSuccess = setStartPose(sclient0, 6, -1.0, 1.0);
444  if(!poseSetSuccess) printf("set pose failed\n");
445  // 7
446  poseSetSuccess = setStartPose(sclient0, 7, -0.5, 1.0);
447  if(!poseSetSuccess) printf("set pose failed\n");
448  // 8
449  poseSetSuccess = setStartPose(sclient0, 8, 0.0, 1.0);
450  if(!poseSetSuccess) printf("set pose failed\n");
451  // 9
452  poseSetSuccess = setStartPose(sclient0, 9, 0.5, 1.0);
453  if(!poseSetSuccess) printf("set pose failed\n");
454 //*/
455 
456 /*
457 // mp02
458  // 0
459  poseSetSuccess = setStartPose(sclient0, 0, -1.5, 0.5);
460  if(!poseSetSuccess) printf("set pose failed\n");
461  // 1
462  poseSetSuccess = setStartPose(sclient0, 1, -1.0, 0.5);
463  if(!poseSetSuccess) printf("set pose failed\n");
464  // 2
465  poseSetSuccess = setStartPose(sclient0, 2, -0.5, 0.5);
466  if(!poseSetSuccess) printf("set pose failed\n");
467  // 3
468  poseSetSuccess = setStartPose(sclient0, 3, 0.0, 0.5);
469  if(!poseSetSuccess) printf("set pose failed\n");
470  // 4
471  poseSetSuccess = setStartPose(sclient0, 4, 0.5, 0.5);
472  if(!poseSetSuccess) printf("set pose failed\n");
473  // 5
474  poseSetSuccess = setStartPose(sclient0, 5, -1.5, 1.0);
475  if(!poseSetSuccess) printf("set pose failed\n");
476  // 6
477  poseSetSuccess = setStartPose(sclient0, 6, -1.0, 1.0);
478  if(!poseSetSuccess) printf("set pose failed\n");
479  // 7
480  poseSetSuccess = setStartPose(sclient0, 7, -0.5, 1.0);
481  if(!poseSetSuccess) printf("set pose failed\n");
482  // 8
483  poseSetSuccess = setStartPose(sclient0, 8, 0.0, 1.0);
484  if(!poseSetSuccess) printf("set pose failed\n");
485  // 9
486  poseSetSuccess = setStartPose(sclient0, 9, 0.5, 1.0);
487  if(!poseSetSuccess) printf("set pose failed\n");
488 //*/
489 
490 /*
491  // mp03
492  poseSetSuccess = setStartPose(sclient0, 0, 0.0, 0.0);
493  if(!poseSetSuccess) printf("set pose failed\n");
494  // 1
495  poseSetSuccess = setStartPose(sclient0, 1, 1.0, 0.0);
496  if(!poseSetSuccess) printf("set pose failed\n");
497  // 2
498  poseSetSuccess = setStartPose(sclient0, 2, 2.0, 0.0);
499  if(!poseSetSuccess) printf("set pose failed\n");
500  // 3
501  poseSetSuccess = setStartPose(sclient0, 3, 3.0, 0.0);
502  if(!poseSetSuccess) printf("set pose failed\n");
503  // 4
504  poseSetSuccess = setStartPose(sclient0, 4, 3.0, -3.0);
505  if(!poseSetSuccess) printf("set pose failed\n");
506  // 5
507  poseSetSuccess = setStartPose(sclient0, 5, 1.0, -2.5);
508  if(!poseSetSuccess) printf("set pose failed\n");
509  // 6
510  poseSetSuccess = setStartPose(sclient0, 6, 1.5, -2.5);
511  if(!poseSetSuccess) printf("set pose failed\n");
512  // 7
513  poseSetSuccess = setStartPose(sclient0, 7, 2.0, -2.5);
514  if(!poseSetSuccess) printf("set pose failed\n");
515  // 8
516  poseSetSuccess = setStartPose(sclient0, 8, 2.5, -2.5);
517  if(!poseSetSuccess) printf("set pose failed\n");
518  // 9
519  poseSetSuccess = setStartPose(sclient0, 9, 3.0, -2.5);
520  if(!poseSetSuccess) printf("set pose failed\n");
521 //*/
522 
523 /*
524 // sun8cd9 ke ting
525  // 0
526  poseSetSuccess = setStartPose(sclient0, 0, 0.0, 2.0);
527  if(!poseSetSuccess) printf("set pose failed\n");
528  // 1
529  poseSetSuccess = setStartPose(sclient0, 1, -1.0, 2.0);
530  if(!poseSetSuccess) printf("set pose failed\n");
531  // 2
532  poseSetSuccess = setStartPose(sclient0, 2, -2.0, 2.0);
533  if(!poseSetSuccess) printf("set pose failed\n");
534  // 3
535  poseSetSuccess = setStartPose(sclient0, 3, -3.0, 2.0);
536  if(!poseSetSuccess) printf("set pose failed\n");
537  // 4
538  poseSetSuccess = setStartPose(sclient0, 4, -4.0, 2.0);
539  if(!poseSetSuccess) printf("set pose failed\n");
540  // 5
541  poseSetSuccess = setStartPose(sclient0, 5, 0.0, 1.5);
542  if(!poseSetSuccess) printf("set pose failed\n");
543  // 6
544  poseSetSuccess = setStartPose(sclient0, 6, -1.0, 1.5);
545  if(!poseSetSuccess) printf("set pose failed\n");
546  // 7
547  poseSetSuccess = setStartPose(sclient0, 7, -2.0, 1.5);
548  if(!poseSetSuccess) printf("set pose failed\n");
549  // 8
550  poseSetSuccess = setStartPose(sclient0, 8, -3.0, 1.5);
551  if(!poseSetSuccess) printf("set pose failed\n");
552  // 9
553  poseSetSuccess = setStartPose(sclient0, 9, -4.0, 1.5);
554  if(!poseSetSuccess) printf("set pose failed\n");
555 //*/
556 
557 /*
558 // sun8cd9
559  // 0
560  poseSetSuccess = setStartPose(sclient0, 0, -6.0, -1.0);
561  if(!poseSetSuccess) printf("set pose failed\n");
562  // 1
563  poseSetSuccess = setStartPose(sclient0, 1, -6.0, 0.0);
564  if(!poseSetSuccess) printf("set pose failed\n");
565  // 2
566  poseSetSuccess = setStartPose(sclient0, 2, -6.0, 1.0);
567  if(!poseSetSuccess) printf("set pose failed\n");
568  // 3
569  poseSetSuccess = setStartPose(sclient0, 3, -6.0, 2.0);
570  if(!poseSetSuccess) printf("set pose failed\n");
571  // 4
572  poseSetSuccess = setStartPose(sclient0, 4, -6.0, 3.0);
573  if(!poseSetSuccess) printf("set pose failed\n");
574  // 5
575  poseSetSuccess = setStartPose(sclient0, 5, -5.0, -1.0);
576  if(!poseSetSuccess) printf("set pose failed\n");
577  // 6
578  poseSetSuccess = setStartPose(sclient0, 6, -5.0, 0.0);
579  if(!poseSetSuccess) printf("set pose failed\n");
580  // 7
581  poseSetSuccess = setStartPose(sclient0, 7, -5.0, 1.0);
582  if(!poseSetSuccess) printf("set pose failed\n");
583  // 8
584  poseSetSuccess = setStartPose(sclient0, 8, -5.0, 2.0);
585  if(!poseSetSuccess) printf("set pose failed\n");
586  // 9
587  poseSetSuccess = setStartPose(sclient0, 9, -5.0, 3.0);
588  if(!poseSetSuccess) printf("set pose failed\n");
589 //*/
590 
591 /*
592 // sun5eb2
593  // 0
594  poseSetSuccess = setStartPose(sclient0, 0, -0.5, -1.0);
595  if(!poseSetSuccess) printf("set pose failed\n");
596  // 1
597  poseSetSuccess = setStartPose(sclient0, 1, -0.5, -1.5);
598  if(!poseSetSuccess) printf("set pose failed\n");
599  // 2
600  poseSetSuccess = setStartPose(sclient0, 2, -0.5, -2.0);
601  if(!poseSetSuccess) printf("set pose failed\n");
602  // 3
603  poseSetSuccess = setStartPose(sclient0, 3, -0.5, -2.5);
604  if(!poseSetSuccess) printf("set pose failed\n");
605  // 4
606  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -9.5);
607  if(!poseSetSuccess) printf("set pose failed\n");
608  // 5
609  poseSetSuccess = setStartPose(sclient0, 5, 2.5, -9.5);
610  if(!poseSetSuccess) printf("set pose failed\n");
611  // 6
612  poseSetSuccess = setStartPose(sclient0, 6, 3.0, -9.5);
613  if(!poseSetSuccess) printf("set pose failed\n");
614  // 7
615  poseSetSuccess = setStartPose(sclient0, 7, 3.5, -9.5);
616  if(!poseSetSuccess) printf("set pose failed\n");
617  // 8
618  poseSetSuccess = setStartPose(sclient0, 8, 4.0, -9.5);
619  if(!poseSetSuccess) printf("set pose failed\n");
620  // 9
621  poseSetSuccess = setStartPose(sclient0, 9, 4.5, -9.5);
622  if(!poseSetSuccess) printf("set pose failed\n");
623 //*/
624 
625 /*
626  // big mp04
627  // 0
628  poseSetSuccess = setStartPose(sclient0, 0, 0.0, 7.0);
629  if(!poseSetSuccess) printf("set pose failed\n");
630  // 1
631  poseSetSuccess = setStartPose(sclient0, 1, 0.0, 8.0);
632  if(!poseSetSuccess) printf("set pose failed\n");
633  // 2
634  poseSetSuccess = setStartPose(sclient0, 2, 0.0, 9.0);
635  if(!poseSetSuccess) printf("set pose failed\n");
636  // 3
637  poseSetSuccess = setStartPose(sclient0, 3, 1.0, 7.0);
638  if(!poseSetSuccess) printf("set pose failed\n");
639  // 4
640  poseSetSuccess = setStartPose(sclient0, 4, 1.0, 8.0);
641  if(!poseSetSuccess) printf("set pose failed\n");
642  // 5
643  poseSetSuccess = setStartPose(sclient0, 5, 1.0, 9.0);
644  if(!poseSetSuccess) printf("set pose failed\n");
645  // 6
646  poseSetSuccess = setStartPose(sclient0, 6, 1.5, -2.5);
647  if(!poseSetSuccess) printf("set pose failed\n");
648  // 7
649  poseSetSuccess = setStartPose(sclient0, 7, 2.0, -2.5);
650  if(!poseSetSuccess) printf("set pose failed\n");
651  // 8
652  poseSetSuccess = setStartPose(sclient0, 8, 2.5, -2.5);
653  if(!poseSetSuccess) printf("set pose failed\n");
654  // 9
655  poseSetSuccess = setStartPose(sclient0, 9, 3.0, -2.5);
656  if(!poseSetSuccess) printf("set pose failed\n");
657 //*/
658 
659 /*
660 // bfa
661  // 0
662  poseSetSuccess = setStartPose(sclient0, 0, 4.0, 0.0);
663  if(!poseSetSuccess) printf("set pose failed\n");
664  // 1
665  poseSetSuccess = setStartPose(sclient0, 1, 4.0, 0.5);
666  if(!poseSetSuccess) printf("set pose failed\n");
667  // 2
668  poseSetSuccess = setStartPose(sclient0, 2, 4.0, 1.5);
669  if(!poseSetSuccess) printf("set pose failed\n");
670  // 3
671  poseSetSuccess = setStartPose(sclient0, 3, 4.0, 2.0);
672  if(!poseSetSuccess) printf("set pose failed\n");
673  // 4
674  poseSetSuccess = setStartPose(sclient0, 4, 4.0, 2.5);
675  if(!poseSetSuccess) printf("set pose failed\n");
676  // 5
677  poseSetSuccess = setStartPose(sclient0, 5, 4.0, 3.0);
678  if(!poseSetSuccess) printf("set pose failed\n");
679  // 6
680  poseSetSuccess = setStartPose(sclient0, 6, 3.0, 3.5);
681  if(!poseSetSuccess) printf("set pose failed\n");
682  // 7
683  poseSetSuccess = setStartPose(sclient0, 7, 3.5, -9.5);
684  if(!poseSetSuccess) printf("set pose failed\n");
685  // 8
686  poseSetSuccess = setStartPose(sclient0, 8, 4.0, -9.5);
687  if(!poseSetSuccess) printf("set pose failed\n");
688  // 9
689  poseSetSuccess = setStartPose(sclient0, 9, 4.5, -9.5);
690  if(!poseSetSuccess) printf("set pose failed\n");
691  //*/
692 /*
693 // bfagjj
694  // 0
695  poseSetSuccess = setStartPose(sclient0, 0, -3.0, -7.0);
696  if(!poseSetSuccess) printf("set pose failed\n");
697  // 1
698  poseSetSuccess = setStartPose(sclient0, 1, -3.0, -6.0);
699  if(!poseSetSuccess) printf("set pose failed\n");
700  // 2
701  poseSetSuccess = setStartPose(sclient0, 2, -3.0, -5.0);
702  if(!poseSetSuccess) printf("set pose failed\n");
703  // 3
704  poseSetSuccess = setStartPose(sclient0, 3, -3.0, -4.0);
705  if(!poseSetSuccess) printf("set pose failed\n");
706  // 4
707  poseSetSuccess = setStartPose(sclient0, 4, -3.0, -3.0);
708  if(!poseSetSuccess) printf("set pose failed\n");
709  // 5
710  poseSetSuccess = setStartPose(sclient0, 5, -3.0, -2.0);
711  if(!poseSetSuccess) printf("set pose failed\n");
712  // 6
713  poseSetSuccess = setStartPose(sclient0, 6, 3.0, -9.5);
714  if(!poseSetSuccess) printf("set pose failed\n");
715  // 7
716  poseSetSuccess = setStartPose(sclient0, 7, 3.5, -9.5);
717  if(!poseSetSuccess) printf("set pose failed\n");
718  // 8
719  poseSetSuccess = setStartPose(sclient0, 8, 4.0, -9.5);
720  if(!poseSetSuccess) printf("set pose failed\n");
721  // 9
722  poseSetSuccess = setStartPose(sclient0, 9, 4.5, -9.5);
723  if(!poseSetSuccess) printf("set pose failed\n");
724 //*/
725 
726 /*
727 // jy0
728  // 0
729  poseSetSuccess = setStartPose(sclient0, 0, 5.5, 2.0);
730  if(!poseSetSuccess) printf("set pose failed\n");
731  // 1
732  poseSetSuccess = setStartPose(sclient0, 1, 5.0, 2.0);
733  if(!poseSetSuccess) printf("set pose failed\n");
734  // 2
735  poseSetSuccess = setStartPose(sclient0, 2, 4.5, 2.0);
736  if(!poseSetSuccess) printf("set pose failed\n");
737  // 3
738  poseSetSuccess = setStartPose(sclient0, 3, 5.0, 1.5);
739  if(!poseSetSuccess) printf("set pose failed\n");
740  // 4
741  poseSetSuccess = setStartPose(sclient0, 4, -1.0, 0.0);
742  if(!poseSetSuccess) printf("set pose failed\n");
743  // 5
744  poseSetSuccess = setStartPose(sclient0, 5, 2.5, -9.5);
745  if(!poseSetSuccess) printf("set pose failed\n");
746  // 6
747  poseSetSuccess = setStartPose(sclient0, 6, 3.0, -9.5);
748  if(!poseSetSuccess) printf("set pose failed\n");
749  // 7
750  poseSetSuccess = setStartPose(sclient0, 7, 3.5, -9.5);
751  if(!poseSetSuccess) printf("set pose failed\n");
752  // 8
753  poseSetSuccess = setStartPose(sclient0, 8, 4.0, -9.5);
754  if(!poseSetSuccess) printf("set pose failed\n");
755  // 9
756  poseSetSuccess = setStartPose(sclient0, 9, 4.5, -9.5);
757  if(!poseSetSuccess) printf("set pose failed\n");
758 //*/
759 
760 /*
761 // jy2
762  // 0
763  poseSetSuccess = setStartPose(sclient0, 0, 2.5, 0.0);
764  if(!poseSetSuccess) printf("set pose failed\n");
765  // 1
766  poseSetSuccess = setStartPose(sclient0, 1, 2.0, 0.0);
767  if(!poseSetSuccess) printf("set pose failed\n");
768  // 2
769  poseSetSuccess = setStartPose(sclient0, 2, 1.5, 0.0);
770  if(!poseSetSuccess) printf("set pose failed\n");
771  // 3
772  poseSetSuccess = setStartPose(sclient0, 3, 0.5, 0.0);
773  if(!poseSetSuccess) printf("set pose failed\n");
774  // 4
775  poseSetSuccess = setStartPose(sclient0, 4, -1.0, 0.0);
776  if(!poseSetSuccess) printf("set pose failed\n");
777  // 5
778  poseSetSuccess = setStartPose(sclient0, 5, 2.5, -9.5);
779  if(!poseSetSuccess) printf("set pose failed\n");
780  // 6
781  poseSetSuccess = setStartPose(sclient0, 6, 3.0, -9.5);
782  if(!poseSetSuccess) printf("set pose failed\n");
783  // 7
784  poseSetSuccess = setStartPose(sclient0, 7, 3.5, -9.5);
785  if(!poseSetSuccess) printf("set pose failed\n");
786  // 8
787  poseSetSuccess = setStartPose(sclient0, 8, 4.0, -9.5);
788  if(!poseSetSuccess) printf("set pose failed\n");
789  // 9
790  poseSetSuccess = setStartPose(sclient0, 9, 4.5, -9.5);
791  if(!poseSetSuccess) printf("set pose failed\n");
792 
793  //*/
794 
795 /*
796  // wallob
797  // 0
798  poseSetSuccess = setStartPose(sclient0, 0, 0.0, -9.5);
799  if(!poseSetSuccess) printf("set pose failed\n");
800  // 1
801  poseSetSuccess = setStartPose(sclient0, 1, 0.5, -9.5);
802  if(!poseSetSuccess) printf("set pose failed\n");
803  // 2
804  poseSetSuccess = setStartPose(sclient0, 2, 1.0, -9.5);
805  if(!poseSetSuccess) printf("set pose failed\n");
806  // 3
807  poseSetSuccess = setStartPose(sclient0, 3, 1.5, -9.5);
808  if(!poseSetSuccess) printf("set pose failed\n");
809  // 4
810  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -9.5);
811  if(!poseSetSuccess) printf("set pose failed\n");
812  // 5
813  poseSetSuccess = setStartPose(sclient0, 5, 2.5, -9.5);
814  if(!poseSetSuccess) printf("set pose failed\n");
815  // 6
816  poseSetSuccess = setStartPose(sclient0, 6, 3.0, -9.5);
817  if(!poseSetSuccess) printf("set pose failed\n");
818  // 7
819  poseSetSuccess = setStartPose(sclient0, 7, 3.5, -9.5);
820  if(!poseSetSuccess) printf("set pose failed\n");
821  // 8
822  poseSetSuccess = setStartPose(sclient0, 8, 4.0, -9.5);
823  if(!poseSetSuccess) printf("set pose failed\n");
824  // 9
825  poseSetSuccess = setStartPose(sclient0, 9, 4.5, -9.5);
826  if(!poseSetSuccess) printf("set pose failed\n");
827 */
828 /*
829 // rect center
830  // 0
831  poseSetSuccess = setStartPose(sclient0, 0, 1.0, 1.0);
832  if(!poseSetSuccess) printf("set pose failed\n");
833  // 1
834  poseSetSuccess = setStartPose(sclient0, 1, 1.0, -1.0);
835  if(!poseSetSuccess) printf("set pose failed\n");
836  // 2
837  poseSetSuccess = setStartPose(sclient0, 2, -1.0, -1.0);
838  if(!poseSetSuccess) printf("set pose failed\n");
839  // 3
840  poseSetSuccess = setStartPose(sclient0, 3, -1.0, 1.0);
841  if(!poseSetSuccess) printf("set pose failed\n");
842  // 4
843  poseSetSuccess = setStartPose(sclient0, 4, 2.0, 1.0);
844  if(!poseSetSuccess) printf("set pose failed\n");
845  // 5
846  poseSetSuccess = setStartPose(sclient0, 5, 1.0, -2.0);
847  if(!poseSetSuccess) printf("set pose failed\n");
848  // 6
849  poseSetSuccess = setStartPose(sclient0, 6, -2.0, -1.0);
850  if(!poseSetSuccess) printf("set pose failed\n");
851  // 7
852  poseSetSuccess = setStartPose(sclient0, 7, -1.0, 2.0);
853  if(!poseSetSuccess) printf("set pose failed\n");
854  // 8
855  poseSetSuccess = setStartPose(sclient0, 8, 1.0, 2.0);
856  if(!poseSetSuccess) printf("set pose failed\n");
857  // 9
858  poseSetSuccess = setStartPose(sclient0, 9, -1.0, -2.0);
859  if(!poseSetSuccess) printf("set pose failed\n");
860 //*/
861 
862  /*
863 // rect
864  // 0
865  poseSetSuccess = setStartPose(sclient0, 0, 0.0, -4.5);
866  if(!poseSetSuccess) printf("set pose failed\n");
867  // 1
868  poseSetSuccess = setStartPose(sclient0, 1, 0.5, -4.5);
869  if(!poseSetSuccess) printf("set pose failed\n");
870  // 2
871  poseSetSuccess = setStartPose(sclient0, 2, 1.0, -4.5);
872  if(!poseSetSuccess) printf("set pose failed\n");
873  // 3
874  poseSetSuccess = setStartPose(sclient0, 3, 1.5, -4.5);
875  if(!poseSetSuccess) printf("set pose failed\n");
876  // 4
877  poseSetSuccess = setStartPose(sclient0, 4, 2.0, -4.5);
878  if(!poseSetSuccess) printf("set pose failed\n");
879  // 5
880  poseSetSuccess = setStartPose(sclient0, 5, 0.0, -5.0);
881  if(!poseSetSuccess) printf("set pose failed\n");
882  // 6
883  poseSetSuccess = setStartPose(sclient0, 6, 0.5, -5.0);
884  if(!poseSetSuccess) printf("set pose failed\n");
885  // 7
886  poseSetSuccess = setStartPose(sclient0, 7, 1.0, -5.0);
887  if(!poseSetSuccess) printf("set pose failed\n");
888  // 8
889  poseSetSuccess = setStartPose(sclient0, 8, 1.5, -5.0);
890  if(!poseSetSuccess) printf("set pose failed\n");
891  // 9
892  poseSetSuccess = setStartPose(sclient0, 9, 2.0, -5.0);
893  if(!poseSetSuccess) printf("set pose failed\n");
894  */
895 
896  /*
897  // 0
898  poseSetSuccess = setStartPose(sclient0, 0, 2.0, 0);
899  if(!poseSetSuccess) printf("set pose failed\n");
900  // 1
901  poseSetSuccess = setStartPose(sclient0, 1, 1.5, 0);
902  if(!poseSetSuccess) printf("set pose failed\n");
903  // 2
904  poseSetSuccess = setStartPose(sclient0, 2, 1.0, 0);
905  if(!poseSetSuccess) printf("set pose failed\n");
906  // 3
907  poseSetSuccess = setStartPose(sclient0, 3, 0.5, 0);
908  if(!poseSetSuccess) printf("set pose failed\n");
909  // 4
910  poseSetSuccess = setStartPose(sclient0, 4, 0.0, 0);
911  if(!poseSetSuccess) printf("set pose failed\n");
912  // 5
913  poseSetSuccess = setStartPose(sclient0, 5, 2.0, 0.5);
914  if(!poseSetSuccess) printf("set pose failed\n");
915  // 6
916  poseSetSuccess = setStartPose(sclient0, 6, 1.5, 0.5);
917  if(!poseSetSuccess) printf("set pose failed\n");
918  // 7
919  poseSetSuccess = setStartPose(sclient0, 7, 1.0, 0.5);
920  if(!poseSetSuccess) printf("set pose failed\n");
921  // 8
922  poseSetSuccess = setStartPose(sclient0, 8, 0.5, 0.5);
923  if(!poseSetSuccess) printf("set pose failed\n");
924  // 9
925  poseSetSuccess = setStartPose(sclient0, 9, 0.0, 0.5);
926  if(!poseSetSuccess) printf("set pose failed\n");
927 */
928 
929  return 0;
930 }
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
main
int main(int argc, char **argv)
Definition: init_locations.cpp:58
ros::NodeHandle
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::ServiceClient::call
bool call(MReq &req, MRes &res)
camera_height
const float camera_height
Definition: init_locations.cpp:14
setStartPose
bool setStartPose(ros::ServiceClient client, int rbtID, float x, float y, float z=camera_height)
Definition: init_locations.cpp:16
ROS_ERROR
#define ROS_ERROR(...)

virtual_scan
Author(s):
autogenerated on Mon Feb 28 2022 23:02:06