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

rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Dec 13 2024 03:31:02