marker_test.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 
3 #include "visualization_msgs/Marker.h"
4 #include "visualization_msgs/MarkerArray.h"
5 
6 #include <tf/transform_broadcaster.h>
7 #include <tf/tf.h>
8 
9 ros::Publisher g_marker_pub;
10 
11 void emitRow(const std::string type_name, uint32_t type, int32_t x_pos, float r, float g, float b,
12  ros::Duration lifetime, ros::Publisher& pub, bool frame_locked = true, std::string frame_id = std::string("/base_link"),
13  float sx = 1.0, float sy = 1.0, float sz = 1.0)
14 {
15  static uint32_t count = 0;
16  for (int i = -5; i < 5; ++i)
17  {
18  visualization_msgs::Marker marker;
19  marker.header.frame_id = frame_id;
20  ros::Time ros_time = ros::Time::now();
21 // ros_time.sec -=1;
22  marker.header.stamp = ros_time;
23  marker.ns = "marker_test_" + type_name;
24  marker.id = i;
25  marker.type = type;
26  marker.action = visualization_msgs::Marker::ADD;
27  marker.pose.position.x = x_pos;
28  marker.pose.position.y = (i * 2);
29  marker.pose.position.z = 0;
30  marker.pose.orientation.x = 0.0;
31  marker.pose.orientation.y = 0.0;
32  marker.pose.orientation.z = 0.0;
33  marker.pose.orientation.w = 1.0;
34  marker.scale.x = sx;
35  marker.scale.y = sy;
36  marker.scale.z = sz;
37  marker.color.r = r;
38  marker.color.g = g;
39  marker.color.b = b;
40  marker.color.a = float(i+5) / 10.0;
41 
42  marker.lifetime = lifetime;
43  marker.frame_locked = frame_locked;
44  marker.text = "This is some text\nthis is a new line\nthis is another line\nand another adfoije owijeoiwej\na really really really really really really really really really really long one";
45  marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
46  marker.mesh_use_embedded_materials = (i > int((count / 12) % 5));
47  pub.publish(marker);
48  }
49 
50  ++count;
51 }
52 
53 void publishCallback(const ros::TimerEvent&)
54 {
55  static uint32_t counter = 0;
56 
57  ROS_INFO("Publishing");
58  int32_t x_pos = -15;
59  emitRow("arrows", visualization_msgs::Marker::ARROW, x_pos, 1.0, 0.0, 0.0, ros::Duration(), g_marker_pub);
60  x_pos += 3;
61  emitRow("cubes", visualization_msgs::Marker::CUBE, x_pos, 0.0, 1.0, 0.0, ros::Duration(), g_marker_pub);
62  x_pos += 3;
63  emitRow("cubes_frame_locked", visualization_msgs::Marker::CUBE, x_pos, 1.0, 1.0, 0.0, ros::Duration(), g_marker_pub, true, "/my_link");
64  x_pos += 3;
65  emitRow("spheres", visualization_msgs::Marker::SPHERE, x_pos, 0.0, 0.0, 1.0, ros::Duration(), g_marker_pub);
66  x_pos += 3;
67  emitRow("cylinder", visualization_msgs::Marker::CYLINDER, x_pos, 1.0, 0.0, 0.0, ros::Duration(), g_marker_pub);
68  x_pos += 3;
69  emitRow("arrows_with_lifetime", visualization_msgs::Marker::ARROW, x_pos, 0.0, 1.0, 0.0, ros::Duration(0.6),
70  g_marker_pub);
71  x_pos += 3;
72  emitRow("cubes_with_lifetime", visualization_msgs::Marker::CUBE, x_pos, 0.0, 0.0, 1.0, ros::Duration(0.7),
73  g_marker_pub);
74  x_pos += 3;
75  emitRow("spheres_with_lifetime", visualization_msgs::Marker::SPHERE, x_pos, 1.0, 0.0, 0.0, ros::Duration(0.8),
76  g_marker_pub);
77  x_pos += 3;
78  emitRow("cylinder_with_lifetime", visualization_msgs::Marker::CYLINDER, x_pos, 0.0, 1.0, 0.0, ros::Duration(0.9),
79  g_marker_pub);
80  x_pos += 3;
81  emitRow("text_view_facing", visualization_msgs::Marker::TEXT_VIEW_FACING, x_pos, 1.0, 1.0, 1.0, ros::Duration(),
82  g_marker_pub, false, "/base_link", 1.0, 1.0, 0.2);
83  x_pos += 3;
84  emitRow("mesh_resource", visualization_msgs::Marker::MESH_RESOURCE, x_pos, 0.0, 1.0, 1.0, ros::Duration(),
85  g_marker_pub);
86  x_pos += 3;
87 
88  emitRow("invalid_scales", visualization_msgs::Marker::CUBE, x_pos, 0.0, 1.0, 1.0, ros::Duration(), g_marker_pub, false, "/base_link", 0.0, 1.0, 1.0);
89  x_pos += 3;
90 
91  {
92  for (int i = -5; i < 5; ++i)
93  {
94  visualization_msgs::Marker marker;
95  marker.header.frame_id = "/base_link";
96  marker.header.stamp = ros::Time::now();
97  marker.ns = "marker_test_arrow_by_points";
98  marker.id = i;
99  marker.type = visualization_msgs::Marker::ARROW;
100  marker.action = visualization_msgs::Marker::ADD;
101  marker.pose.orientation.x = 0.0;
102  marker.pose.orientation.y = 0.0;
103  marker.pose.orientation.z = 0.0;
104  marker.pose.orientation.w = 1.0;
105  marker.pose.position.x = x_pos;
106  marker.pose.position.y = i * 2;
107  marker.scale.x = 0.25;
108  marker.scale.y = 0.5;
109  marker.color.r = 0.0;
110  marker.color.g = 1.0;
111  marker.color.b = 1.0;
112  marker.color.a = 1.0;
113  marker.frame_locked = true;
114 
115  if (counter % 2 == 0)
116  {
117  marker.points.resize(1);
118  marker.points[0].x = 0.0f;
119  marker.points[0].y = 0.0f;
120  marker.points[0].z = 0.0f;
121  }
122  else
123  {
124  marker.points.resize(2);
125  marker.points[0].x = 0.0f;
126  marker.points[0].y = 0.0f;
127  marker.points[0].z = 0.0f;
128  marker.points[1].x = 1.0f;
129  marker.points[1].y = 0.0f;
130  marker.points[1].z = 0.0f;
131  }
132  g_marker_pub.publish(marker);
133  }
134  }
135 
136  x_pos += 3;
137 
138  {
139  visualization_msgs::Marker marker;
140  marker.header.frame_id = "/base_link";
141  marker.header.stamp = ros::Time::now();
142  marker.ns = "marker_test_cube_list";
143  marker.id = 0;
144  marker.type = visualization_msgs::Marker::CUBE_LIST;
145  marker.action = visualization_msgs::Marker::ADD;
146  marker.pose.orientation.x = 0.0;
147  marker.pose.orientation.y = 0.0;
148  marker.pose.orientation.z = 0.0;
149  marker.pose.orientation.w = 1.0;
150  marker.pose.position.x = x_pos;
151  marker.scale.x = 0.05;
152  marker.scale.y = 0.05;
153  marker.scale.z = 0.05;
154  marker.color.r = 1.0;
155  marker.color.g = 1.0;
156  marker.color.b = 0.0;
157  marker.color.a = 1.0;
158  marker.frame_locked = true;
159 
160  for (int x = 0; x < 10; ++x)
161  {
162  for (int y = 0; y < 10; ++y)
163  {
164  for (int z = 0; z < 10; ++z)
165  {
166  geometry_msgs::Point p;
167  p.x = x * 0.1f;
168  p.y = y * 0.1f;
169  p.z = z * 0.1f;
170 
171  marker.points.push_back(p);
172  }
173  }
174  }
175  g_marker_pub.publish(marker);
176  }
177 
178  x_pos += 3;
179 
180  {
181  visualization_msgs::Marker marker;
182  marker.header.frame_id = "/base_link";
183  marker.header.stamp = ros::Time::now();
184  marker.ns = "marker_test_cube_list_color_per";
185  marker.id = 0;
186  marker.type = visualization_msgs::Marker::CUBE_LIST;
187  marker.action = visualization_msgs::Marker::ADD;
188  marker.pose.orientation.x = 0.0;
189  marker.pose.orientation.y = 0.0;
190  marker.pose.orientation.z = 0.0;
191  marker.pose.orientation.w = 1.0;
192  marker.pose.position.x = x_pos;
193  marker.scale.x = 0.05;
194  marker.scale.y = 0.05;
195  marker.scale.z = 0.05;
196  marker.color.r = 1.0;
197  marker.color.g = 1.0;
198  marker.color.b = 0.0;
199  marker.color.a = 1.0;
200  marker.frame_locked = true;
201 
202  for (int x = 0; x < 10; ++x)
203  {
204  for (int y = 0; y < 10; ++y)
205  {
206  for (int z = 0; z < 10; ++z)
207  {
208  geometry_msgs::Point p;
209  p.x = x * 0.1f;
210  p.y = y * 0.1f;
211  p.z = z * 0.1f;
212 
213  marker.points.push_back(p);
214 
215  std_msgs::ColorRGBA c;
216  c.r = x * 0.1;
217  c.g = y * 0.1;
218  c.b = z * 0.1;
219  c.a = 1.0;
220  marker.colors.push_back(c);
221  }
222  }
223  }
224  g_marker_pub.publish(marker);
225  }
226 
227  x_pos += 3;
228 
229  {
230  visualization_msgs::Marker marker;
231  marker.header.frame_id = "/base_link";
232  marker.header.stamp = ros::Time::now();
233  marker.ns = "marker_test_point_list_alpha_per";
234  marker.id = 0;
235  marker.action = visualization_msgs::Marker::ADD;
236  marker.pose.orientation.x = 0.0;
237  marker.pose.orientation.y = 0.0;
238  marker.pose.orientation.z = 0.0;
239  marker.pose.orientation.w = 1.0;
240  marker.pose.position.x = x_pos;
241  marker.scale.x = 0.05;
242  marker.scale.y = 0.05;
243  marker.scale.z = 0.05;
244  marker.color.r = 1.0;
245  marker.color.g = 1.0;
246  marker.color.b = 0.0;
247  marker.color.a = 1.0;
248  marker.frame_locked = true;
249 
250  for( int type = visualization_msgs::Marker::CUBE_LIST; type <= visualization_msgs::Marker::POINTS; type++ )
251  {
252  marker.id = type;
253  marker.pose.position.x += 0.5;
254  marker.type = type;
255 
256  for (int y = 0; y < 10; ++y)
257  {
258  geometry_msgs::Point p;
259  p.x = 0;
260  p.y = y * 0.1f;
261  p.z = 0;
262 
263  marker.points.push_back(p);
264 
265  std_msgs::ColorRGBA c;
266  c.r = 1;
267  c.g = 1;
268  c.b = 1;
269  c.a = (float)y * 0.1 + 0.1;
270  marker.colors.push_back(c);
271  }
272  g_marker_pub.publish(marker);
273  }
274  }
275 
276  x_pos += 3;
277 
278  {
279  visualization_msgs::Marker marker;
280  marker.header.frame_id = "/base_link";
281  marker.header.stamp = ros::Time::now();
282  marker.ns = "marker_test_sphere_list";
283  marker.id = 0;
284  marker.type = visualization_msgs::Marker::SPHERE_LIST;
285  marker.action = visualization_msgs::Marker::ADD;
286  marker.pose.orientation.x = 0.0;
287  marker.pose.orientation.y = 0.0;
288  marker.pose.orientation.z = 0.0;
289  marker.pose.orientation.w = 1.0;
290  marker.pose.position.x = x_pos;
291  marker.scale.x = 0.05;
292  marker.scale.y = 0.05;
293  marker.scale.z = 0.05;
294  marker.color.r = 1.0;
295  marker.color.g = 1.0;
296  marker.color.b = 1.0;
297  marker.color.a = 1.0;
298  marker.frame_locked = true;
299 
300  for (int x = 0; x < 10; ++x)
301  {
302  for (int y = 0; y < 10; ++y)
303  {
304  for (int z = 0; z < 1; ++z)
305  {
306  geometry_msgs::Point p;
307  p.x = x * 0.1f;
308  p.y = y * 0.1f;
309  p.z = z * 0.1f;
310 
311  marker.points.push_back(p);
312 
313  std_msgs::ColorRGBA c;
314  c.r = x * 0.1;
315  c.g = y * 0.1;
316  c.b = 0.5;
317  c.a = 1.0;
318  marker.colors.push_back(c);
319  }
320  }
321  }
322  g_marker_pub.publish(marker);
323  }
324 
325  x_pos += 3;
326 
327  {
328  visualization_msgs::Marker marker;
329  marker.header.frame_id = "/base_link";
330  marker.header.stamp = ros::Time::now();
331  marker.ns = "marker_test_points";
332  marker.id = 0;
333  marker.type = visualization_msgs::Marker::POINTS;
334  marker.action = visualization_msgs::Marker::ADD;
335  marker.pose.orientation.x = 0.0;
336  marker.pose.orientation.y = 0.0;
337  marker.pose.orientation.z = 0.0;
338  marker.pose.orientation.w = 1.0;
339  marker.pose.position.x = x_pos;
340  marker.scale.x = 0.02;
341  marker.scale.y = 0.02;
342  marker.scale.z = 0.02;
343  marker.color.r = 1.0;
344  marker.color.g = 0.0;
345  marker.color.b = 1.0;
346  marker.color.a = 1.0;
347  marker.frame_locked = true;
348 
349  for (int x = 0; x < 10; ++x)
350  {
351  for (int y = 0; y < 10; ++y)
352  {
353  for (int z = 0; z < 10; ++z)
354  {
355  geometry_msgs::Point p;
356  p.x = x * 0.1f;
357  p.y = y * 0.1f;
358  p.z = z * 0.1f;
359 
360  marker.points.push_back(p);
361  }
362  }
363  }
364  g_marker_pub.publish(marker);
365  }
366 
367  x_pos += 3;
368 
369  {
370  visualization_msgs::Marker marker;
371  marker.header.frame_id = "/base_link";
372  marker.header.stamp = ros::Time::now();
373  marker.ns = "marker_test_points_color_per";
374  marker.id = 0;
375  marker.type = visualization_msgs::Marker::POINTS;
376  marker.action = visualization_msgs::Marker::ADD;
377  marker.pose.orientation.x = 0.0;
378  marker.pose.orientation.y = 0.0;
379  marker.pose.orientation.z = 0.0;
380  marker.pose.orientation.w = 1.0;
381  marker.pose.position.x = x_pos;
382  marker.scale.x = 0.02;
383  marker.scale.y = 0.02;
384  marker.scale.z = 0.02;
385  marker.color.r = 1.0;
386  marker.color.g = 0.0;
387  marker.color.b = 1.0;
388  marker.color.a = 1.0;
389  marker.frame_locked = true;
390 
391  for (int x = 0; x < 10; ++x)
392  {
393  for (int y = 0; y < 10; ++y)
394  {
395  for (int z = 0; z < 10; ++z)
396  {
397  geometry_msgs::Point p;
398  p.x = x * 0.1f;
399  p.y = y * 0.1f;
400  p.z = z * 0.1f;
401 
402  marker.points.push_back(p);
403 
404  std_msgs::ColorRGBA c;
405  c.r = x * 0.1;
406  c.g = y * 0.1;
407  c.b = z * 0.1;
408  c.a = 1.0;
409  marker.colors.push_back(c);
410  }
411  }
412  }
413  g_marker_pub.publish(marker);
414  }
415 
416  x_pos += 3;
417 
418  {
419  int count = 10;
420  visualization_msgs::Marker marker;
421  marker.header.frame_id = "/base_link";
422  marker.header.stamp = ros::Time::now();
423  marker.ns = "marker_test_line_list";
424  marker.id = 0;
425  marker.type = visualization_msgs::Marker::LINE_LIST;
426  marker.action = visualization_msgs::Marker::ADD;
427  marker.pose.position.x = 0.0;
428  marker.pose.position.y = 0.0;
429  marker.pose.position.z = 0.0;
430  marker.pose.orientation.x = 0.0;
431  marker.pose.orientation.y = 0.0;
432  marker.pose.orientation.z = 0.0;
433  marker.pose.orientation.w = 1.0;
434  marker.pose.position.x = x_pos;
435  marker.scale.x = 0.1;
436  marker.color.r = 1.0;
437  marker.color.a = 1.0;
438  marker.frame_locked = true;
439 
440  for (int i = 0; i < count; ++i)
441  {
442  geometry_msgs::Point p1, p2;
443  p1.x = 0;
444  p1.y = (i - count / 2) * 2;
445  p1.z = 0;
446  p2.x = 0;
447  p2.y = (i - count / 2) * 2;
448  p2.z = 1;
449  marker.points.push_back(p1);
450  marker.points.push_back(p2);
451  }
452  g_marker_pub.publish(marker);
453  }
454 
455  x_pos += 3;
456 
457  {
458  int count = 10;
459  visualization_msgs::Marker marker;
460  marker.header.frame_id = "/base_link";
461  marker.header.stamp = ros::Time::now();
462  marker.ns = "marker_test_line_list_color_per";
463  marker.id = 0;
464  marker.type = visualization_msgs::Marker::LINE_LIST;
465  marker.action = visualization_msgs::Marker::ADD;
466  marker.pose.position.x = 0.0;
467  marker.pose.position.y = 0.0;
468  marker.pose.position.z = 0.0;
469  marker.pose.orientation.x = 0.0;
470  marker.pose.orientation.y = 0.0;
471  marker.pose.orientation.z = 0.0;
472  marker.pose.orientation.w = 1.0;
473  marker.pose.position.x = x_pos;
474  marker.scale.x = 0.1;
475  marker.color.r = 1.0;
476  marker.color.a = 1.0;
477  marker.frame_locked = true;
478 
479  for (int i = 0; i < count; ++i)
480  {
481  geometry_msgs::Point p1, p2;
482  p1.x = 0;
483  p1.y = (i - count / 2) * 2;
484  p1.z = 0;
485  p2.x = 0;
486  p2.y = (i - count / 2) * 2;
487  p2.z = 1;
488  marker.points.push_back(p1);
489  marker.points.push_back(p2);
490 
491  std_msgs::ColorRGBA c;
492  float pct = (float)i / (float)count;
493  c.r = pct * 1.0 + (1 - pct) * 0.0;
494  c.g = pct * 0.0 + (1 - pct) * 0.0;
495  c.b = pct * 0.0 + (1 - pct) * 1.0;
496  c.a = 1.0;
497 
498  marker.colors.push_back(c);
499  marker.colors.push_back(c);
500  }
501  g_marker_pub.publish(marker);
502  }
503 
504  x_pos += 3;
505 
506  {
507  visualization_msgs::Marker marker;
508  marker.header.frame_id = "/base_link";
509  marker.header.stamp = ros::Time::now();
510  marker.ns = "marker_test_line_strip";
511  marker.id = 0;
512  marker.type = visualization_msgs::Marker::LINE_STRIP;
513  marker.action = visualization_msgs::Marker::ADD;
514  marker.pose.position.x = 0.0;
515  marker.pose.position.y = 0.0;
516  marker.pose.position.z = 0.0;
517  marker.pose.orientation.x = 0.0;
518  marker.pose.orientation.y = 0.0;
519  marker.pose.orientation.z = 0.0;
520  marker.pose.orientation.w = 1.0;
521  marker.pose.position.x = x_pos;
522  marker.scale.x = 0.1;
523  marker.color.g = 1.0;
524  marker.color.a = 1.0;
525  marker.frame_locked = true;
526 
527  for (int i = -5; i < 5; ++i)
528  {
529  geometry_msgs::Point p;
530  p.x = 1 + (i % 2);
531  p.y = (i * 2);
532  p.z = 0;
533  marker.points.push_back(p);
534  }
535 
536  g_marker_pub.publish(marker);
537  }
538 
539  x_pos += 3;
540 
541  {
542  visualization_msgs::Marker marker;
543  marker.header.frame_id = "/base_link";
544  marker.header.stamp = ros::Time::now();
545  marker.ns = "marker_test_line_strip_color_per";
546  marker.id = 0;
547  marker.type = visualization_msgs::Marker::LINE_STRIP;
548  marker.action = visualization_msgs::Marker::ADD;
549  marker.pose.position.x = 0.0;
550  marker.pose.position.y = 0.0;
551  marker.pose.position.z = 0.0;
552  marker.pose.orientation.x = 0.0;
553  marker.pose.orientation.y = 0.0;
554  marker.pose.orientation.z = 0.0;
555  marker.pose.orientation.w = 1.0;
556  marker.pose.position.x = x_pos;
557  marker.scale.x = 0.1;
558  marker.color.g = 1.0;
559  marker.color.a = 1.0;
560  marker.frame_locked = true;
561 
562  for (int i = -5; i < 5; ++i)
563  {
564  geometry_msgs::Point p;
565  p.x = 1 + (i % 2);
566  p.y = (i * 2);
567  p.z = 0;
568  marker.points.push_back(p);
569 
570  std_msgs::ColorRGBA c;
571  float pct = (i + 5) / 10.0;
572  c.r = pct * 0.0 + (1 - pct) * 0.0;
573  c.g = pct * 1.0 + (1 - pct) * 0.0;
574  c.b = pct * 0.0 + (1 - pct) * 1.0;
575  c.a = 1.0;
576 
577  marker.colors.push_back(c);
578  }
579 
580  g_marker_pub.publish(marker);
581  }
582 
583  x_pos += 3;
584 
585  {
586  visualization_msgs::Marker marker;
587  marker.header.frame_id = "/base_link";
588  marker.header.stamp = ros::Time::now();
589  marker.ns = "marker_test_triangle_list";
590  marker.id = 0;
591  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
592  marker.action = visualization_msgs::Marker::ADD;
593  marker.pose.position.x = 0.0;
594  marker.pose.position.y = 0.0;
595  marker.pose.position.z = 0.0;
596  marker.pose.orientation.x = 0.0;
597  marker.pose.orientation.y = 0.0;
598  marker.pose.orientation.z = 0.0;
599  marker.pose.orientation.w = 1.0;
600  marker.pose.position.x = x_pos;
601  marker.scale.x = 1.0;
602  marker.scale.y = 1.0;
603  marker.scale.z = 1.0;
604  marker.color.g = 1.0;
605  marker.color.a = 1.0;
606  marker.frame_locked = true;
607 
608  for (int x = 0; x < 10; ++x)
609  {
610  for (int y = 0; y < 10; ++y)
611  {
612  for (int z = 0; z < 10; ++z)
613  {
614  geometry_msgs::Point p;
615  p.x = x * 0.1f;
616  p.y = y * 0.1f;
617  p.z = z * 0.1f;
618 
619  geometry_msgs::Point p2 = p;
620  p2.x = p.x + 0.05;
621 
622  geometry_msgs::Point p3 = p;
623  p3.x = p2.x;
624  p3.z = p.z + 0.05;
625 
626  marker.points.push_back(p);
627  marker.points.push_back(p2);
628  marker.points.push_back(p3);
629 
630  std_msgs::ColorRGBA c;
631  c.r = x * 0.1;
632  c.g = y * 0.1;
633  c.b = z * 0.1;
634  c.a = 1.0;
635  marker.colors.push_back(c);
636  marker.colors.push_back(c);
637  marker.colors.push_back(c);
638  }
639  }
640  }
641 
642  g_marker_pub.publish(marker);
643  }
644 
645  x_pos += 3;
646 
647  {
648  visualization_msgs::Marker marker;
649  marker.header.frame_id = "/base_link";
650  marker.header.stamp = ros::Time::now();
651  marker.ns = "marker_test_mesh_color_change";
652  marker.id = 0;
653  marker.type = visualization_msgs::Marker::MESH_RESOURCE;
654  marker.action = visualization_msgs::Marker::ADD;
655  marker.pose.position.x = 0.0;
656  marker.pose.position.y = 0.0;
657  marker.pose.position.z = 0.0;
658  marker.pose.orientation.x = 0.0;
659  marker.pose.orientation.y = 0.0;
660  marker.pose.orientation.z = 0.0;
661  marker.pose.orientation.w = 1.0;
662  marker.pose.position.x = x_pos;
663  marker.scale.x = 1.0;
664  marker.scale.y = 1.0;
665  marker.scale.z = 1.0;
666  marker.color.r = float(counter % 255) / 255;
667  marker.color.g = float((counter*3) % 255) / 255;
668  marker.color.b = float((counter*10) % 255) / 255;
669  marker.color.a = 1.0;
670  marker.frame_locked = true;
671  marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
672  marker.mesh_use_embedded_materials = false;
673 
674  g_marker_pub.publish(marker);
675  }
676 
677  ++counter;
678 }
679 
680 void frameCallback(const ros::TimerEvent&)
681 {
682  static uint32_t counter = 0;
683 
684  static tf::TransformBroadcaster br;
685  tf::Transform t;
686 
687  t.setOrigin(tf::Vector3(0.0, 0.0, (counter % 1000) * 0.01));
688  t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
689  br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "base_link", "my_link"));
690 
691  t.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
692  t.setRotation(tf::createQuaternionFromRPY(M_PI*0.25, M_PI*0.25, 0.0));
693  br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "rotate_base_link", "base_link"));
694 
695  ++counter;
696 }
697 
698 int main(int argc, char** argv)
699 {
700  ros::init(argc, argv, "marker_test");
701  ros::NodeHandle n;
702 
703  g_marker_pub = n.advertise<visualization_msgs::Marker> ("visualization_marker", 0);
704  ros::Timer publish_timer = n.createTimer(ros::Duration(1), publishCallback);
705  ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
706 
707  tf::TransformBroadcaster tf_broadcaster;
708 
709  ros::Duration(0.1).sleep();
710 
711  ros::spin();
712 }
tf::createQuaternionFromRPY
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
tf::Transform::setRotation
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
ros::NodeHandle
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
frameCallback
void frameCallback(const ros::TimerEvent &)
Definition: marker_test.cpp:680
ros::Duration::sleep
bool sleep() const
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
transform_broadcaster.h
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
g_marker_pub
ros::Publisher g_marker_pub
Definition: marker_test.cpp:9
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::createTimer
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
tf::TransformBroadcaster::sendTransform
void sendTransform(const StampedTransform &transform)
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
z
TFSIMD_FORCE_INLINE const tfScalar & z() const
main
int main(int argc, char **argv)
Definition: marker_test.cpp:698
ros::Time::now
static Time now()
publishCallback
void publishCallback(const ros::TimerEvent &)
Definition: marker_test.cpp:53
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
emitRow
void emitRow(const std::string type_name, uint32_t type, int32_t x_pos, float r, float g, float b, ros::Duration lifetime, ros::Publisher &pub, bool frame_locked=true, std::string frame_id=std::string("/base_link"), float sx=1.0, float sy=1.0, float sz=1.0)
Definition: marker_test.cpp:11

rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51