00001 #include "ros/ros.h"
00002
00003 #include "visualization_msgs/Marker.h"
00004 #include "visualization_msgs/MarkerArray.h"
00005
00006 #include <tf/transform_broadcaster.h>
00007 #include <tf/tf.h>
00008
00009 ros::Publisher g_marker_pub;
00010
00011 void emitRow(const std::string type_name, uint32_t type, int32_t x_pos, float r, float g, float b,
00012 ros::Duration lifetime, ros::Publisher& pub, bool frame_locked = true, std::string frame_id = std::string("/base_link"),
00013 float sx = 1.0, float sy = 1.0, float sz = 1.0)
00014 {
00015 static uint32_t count = 0;
00016 for (int i = -5; i < 5; ++i)
00017 {
00018 visualization_msgs::Marker marker;
00019 marker.header.frame_id = frame_id;
00020 ros::Time ros_time = ros::Time::now();
00021
00022 marker.header.stamp = ros_time;
00023 marker.ns = "marker_test_" + type_name;
00024 marker.id = i;
00025 marker.type = type;
00026 marker.action = visualization_msgs::Marker::ADD;
00027 marker.pose.position.x = x_pos;
00028 marker.pose.position.y = (i * 2);
00029 marker.pose.position.z = 0;
00030 marker.pose.orientation.x = 0.0;
00031 marker.pose.orientation.y = 0.0;
00032 marker.pose.orientation.z = 0.0;
00033 marker.pose.orientation.w = 1.0;
00034 marker.scale.x = sx;
00035 marker.scale.y = sy;
00036 marker.scale.z = sz;
00037 marker.color.r = r;
00038 marker.color.g = g;
00039 marker.color.b = b;
00040 marker.color.a = 1.0;
00041
00042 marker.lifetime = lifetime;
00043 marker.frame_locked = frame_locked;
00044 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";
00045 marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
00046 marker.mesh_use_embedded_materials = count % 2 == 0;
00047 pub.publish(marker);
00048 }
00049
00050 ++count;
00051 }
00052
00053 void publishCallback(const ros::TimerEvent&)
00054 {
00055 static uint32_t counter = 0;
00056
00057 ROS_INFO("Publishing");
00058 int32_t x_pos = -15;
00059 emitRow("arrows", visualization_msgs::Marker::ARROW, x_pos, 1.0, 0.0, 0.0, ros::Duration(), g_marker_pub);
00060 x_pos += 3;
00061 emitRow("cubes", visualization_msgs::Marker::CUBE, x_pos, 0.0, 1.0, 0.0, ros::Duration(), g_marker_pub);
00062 x_pos += 3;
00063 emitRow("cubes_frame_locked", visualization_msgs::Marker::CUBE, x_pos, 1.0, 1.0, 0.0, ros::Duration(), g_marker_pub, true, "/my_link");
00064 x_pos += 3;
00065 emitRow("spheres", visualization_msgs::Marker::SPHERE, x_pos, 0.0, 0.0, 1.0, ros::Duration(), g_marker_pub);
00066 x_pos += 3;
00067 emitRow("cylinder", visualization_msgs::Marker::CYLINDER, x_pos, 1.0, 0.0, 0.0, ros::Duration(), g_marker_pub);
00068 x_pos += 3;
00069 emitRow("arrows_with_lifetime", visualization_msgs::Marker::ARROW, x_pos, 0.0, 1.0, 0.0, ros::Duration(0.6),
00070 g_marker_pub);
00071 x_pos += 3;
00072 emitRow("cubes_with_lifetime", visualization_msgs::Marker::CUBE, x_pos, 0.0, 0.0, 1.0, ros::Duration(0.7),
00073 g_marker_pub);
00074 x_pos += 3;
00075 emitRow("spheres_with_lifetime", visualization_msgs::Marker::SPHERE, x_pos, 1.0, 0.0, 0.0, ros::Duration(0.8),
00076 g_marker_pub);
00077 x_pos += 3;
00078 emitRow("cylinder_with_lifetime", visualization_msgs::Marker::CYLINDER, x_pos, 0.0, 1.0, 0.0, ros::Duration(0.9),
00079 g_marker_pub);
00080 x_pos += 3;
00081 emitRow("text_view_facing", visualization_msgs::Marker::TEXT_VIEW_FACING, x_pos, 1.0, 1.0, 1.0, ros::Duration(),
00082 g_marker_pub, false, "/base_link", 1.0, 1.0, 0.2);
00083 x_pos += 3;
00084 emitRow("mesh_resource", visualization_msgs::Marker::MESH_RESOURCE, x_pos, 0.0, 1.0, 1.0, ros::Duration(),
00085 g_marker_pub);
00086 x_pos += 3;
00087
00088 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);
00089 x_pos += 3;
00090
00091 {
00092 for (int i = -5; i < 5; ++i)
00093 {
00094 visualization_msgs::Marker marker;
00095 marker.header.frame_id = "/base_link";
00096 marker.header.stamp = ros::Time::now();
00097 marker.ns = "marker_test_arrow_by_points";
00098 marker.id = i;
00099 marker.type = visualization_msgs::Marker::ARROW;
00100 marker.action = visualization_msgs::Marker::ADD;
00101 marker.pose.orientation.x = 0.0;
00102 marker.pose.orientation.y = 0.0;
00103 marker.pose.orientation.z = 0.0;
00104 marker.pose.orientation.w = 1.0;
00105 marker.pose.position.x = x_pos;
00106 marker.pose.position.y = i * 2;
00107 marker.scale.x = 0.25;
00108 marker.scale.y = 0.5;
00109 marker.color.r = 0.0;
00110 marker.color.g = 1.0;
00111 marker.color.b = 1.0;
00112 marker.color.a = 1.0;
00113 marker.frame_locked = true;
00114
00115 if (counter % 2 == 0)
00116 {
00117 marker.points.resize(1);
00118 marker.points[0].x = 0.0f;
00119 marker.points[0].y = 0.0f;
00120 marker.points[0].z = 0.0f;
00121 }
00122 else
00123 {
00124 marker.points.resize(2);
00125 marker.points[0].x = 0.0f;
00126 marker.points[0].y = 0.0f;
00127 marker.points[0].z = 0.0f;
00128 marker.points[1].x = 1.0f;
00129 marker.points[1].y = 0.0f;
00130 marker.points[1].z = 0.0f;
00131 }
00132 g_marker_pub.publish(marker);
00133 }
00134 }
00135
00136 x_pos += 3;
00137
00138 {
00139 visualization_msgs::Marker marker;
00140 marker.header.frame_id = "/base_link";
00141 marker.header.stamp = ros::Time::now();
00142 marker.ns = "marker_test_cube_list";
00143 marker.id = 0;
00144 marker.type = visualization_msgs::Marker::CUBE_LIST;
00145 marker.action = visualization_msgs::Marker::ADD;
00146 marker.pose.orientation.x = 0.0;
00147 marker.pose.orientation.y = 0.0;
00148 marker.pose.orientation.z = 0.0;
00149 marker.pose.orientation.w = 1.0;
00150 marker.pose.position.x = x_pos;
00151 marker.scale.x = 0.05;
00152 marker.scale.y = 0.05;
00153 marker.scale.z = 0.05;
00154 marker.color.r = 1.0;
00155 marker.color.g = 1.0;
00156 marker.color.b = 0.0;
00157 marker.color.a = 1.0;
00158 marker.frame_locked = true;
00159
00160 for (int x = 0; x < 10; ++x)
00161 {
00162 for (int y = 0; y < 10; ++y)
00163 {
00164 for (int z = 0; z < 10; ++z)
00165 {
00166 geometry_msgs::Point p;
00167 p.x = x * 0.1f;
00168 p.y = y * 0.1f;
00169 p.z = z * 0.1f;
00170
00171 marker.points.push_back(p);
00172 }
00173 }
00174 }
00175 g_marker_pub.publish(marker);
00176 }
00177
00178 x_pos += 3;
00179
00180 {
00181 visualization_msgs::Marker marker;
00182 marker.header.frame_id = "/base_link";
00183 marker.header.stamp = ros::Time::now();
00184 marker.ns = "marker_test_cube_list_color_per";
00185 marker.id = 0;
00186 marker.type = visualization_msgs::Marker::CUBE_LIST;
00187 marker.action = visualization_msgs::Marker::ADD;
00188 marker.pose.orientation.x = 0.0;
00189 marker.pose.orientation.y = 0.0;
00190 marker.pose.orientation.z = 0.0;
00191 marker.pose.orientation.w = 1.0;
00192 marker.pose.position.x = x_pos;
00193 marker.scale.x = 0.05;
00194 marker.scale.y = 0.05;
00195 marker.scale.z = 0.05;
00196 marker.color.r = 1.0;
00197 marker.color.g = 1.0;
00198 marker.color.b = 0.0;
00199 marker.color.a = 1.0;
00200 marker.frame_locked = true;
00201
00202 for (int x = 0; x < 10; ++x)
00203 {
00204 for (int y = 0; y < 10; ++y)
00205 {
00206 for (int z = 0; z < 10; ++z)
00207 {
00208 geometry_msgs::Point p;
00209 p.x = x * 0.1f;
00210 p.y = y * 0.1f;
00211 p.z = z * 0.1f;
00212
00213 marker.points.push_back(p);
00214
00215 std_msgs::ColorRGBA c;
00216 c.r = x * 0.1;
00217 c.g = y * 0.1;
00218 c.b = z * 0.1;
00219 marker.colors.push_back(c);
00220 }
00221 }
00222 }
00223 g_marker_pub.publish(marker);
00224 }
00225
00226 x_pos += 3;
00227
00228 {
00229 visualization_msgs::Marker marker;
00230 marker.header.frame_id = "/base_link";
00231 marker.header.stamp = ros::Time::now();
00232 marker.ns = "marker_test_sphere_list";
00233 marker.id = 0;
00234 marker.type = visualization_msgs::Marker::SPHERE_LIST;
00235 marker.action = visualization_msgs::Marker::ADD;
00236 marker.pose.orientation.x = 0.0;
00237 marker.pose.orientation.y = 0.0;
00238 marker.pose.orientation.z = 0.0;
00239 marker.pose.orientation.w = 1.0;
00240 marker.pose.position.x = x_pos;
00241 marker.scale.x = 0.05;
00242 marker.scale.y = 0.05;
00243 marker.scale.z = 0.05;
00244 marker.color.r = 1.0;
00245 marker.color.g = 1.0;
00246 marker.color.b = 1.0;
00247 marker.color.a = 1.0;
00248 marker.frame_locked = true;
00249
00250 for (int x = 0; x < 10; ++x)
00251 {
00252 for (int y = 0; y < 10; ++y)
00253 {
00254 for (int z = 0; z < 1; ++z)
00255 {
00256 geometry_msgs::Point p;
00257 p.x = x * 0.1f;
00258 p.y = y * 0.1f;
00259 p.z = z * 0.1f;
00260
00261 marker.points.push_back(p);
00262
00263 std_msgs::ColorRGBA c;
00264 c.r = x * 0.1;
00265 c.g = y * 0.1;
00266 c.b = 0.5;
00267 marker.colors.push_back(c);
00268 }
00269 }
00270 }
00271 g_marker_pub.publish(marker);
00272 }
00273
00274 x_pos += 3;
00275
00276 {
00277 visualization_msgs::Marker marker;
00278 marker.header.frame_id = "/base_link";
00279 marker.header.stamp = ros::Time::now();
00280 marker.ns = "marker_test_points";
00281 marker.id = 0;
00282 marker.type = visualization_msgs::Marker::POINTS;
00283 marker.action = visualization_msgs::Marker::ADD;
00284 marker.pose.orientation.x = 0.0;
00285 marker.pose.orientation.y = 0.0;
00286 marker.pose.orientation.z = 0.0;
00287 marker.pose.orientation.w = 1.0;
00288 marker.pose.position.x = x_pos;
00289 marker.scale.x = 0.02;
00290 marker.scale.y = 0.02;
00291 marker.scale.z = 0.02;
00292 marker.color.r = 1.0;
00293 marker.color.g = 0.0;
00294 marker.color.b = 1.0;
00295 marker.color.a = 1.0;
00296 marker.frame_locked = true;
00297
00298 for (int x = 0; x < 10; ++x)
00299 {
00300 for (int y = 0; y < 10; ++y)
00301 {
00302 for (int z = 0; z < 10; ++z)
00303 {
00304 geometry_msgs::Point p;
00305 p.x = x * 0.1f;
00306 p.y = y * 0.1f;
00307 p.z = z * 0.1f;
00308
00309 marker.points.push_back(p);
00310 }
00311 }
00312 }
00313 g_marker_pub.publish(marker);
00314 }
00315
00316 x_pos += 3;
00317
00318 {
00319 visualization_msgs::Marker marker;
00320 marker.header.frame_id = "/base_link";
00321 marker.header.stamp = ros::Time::now();
00322 marker.ns = "marker_test_points_color_per";
00323 marker.id = 0;
00324 marker.type = visualization_msgs::Marker::POINTS;
00325 marker.action = visualization_msgs::Marker::ADD;
00326 marker.pose.orientation.x = 0.0;
00327 marker.pose.orientation.y = 0.0;
00328 marker.pose.orientation.z = 0.0;
00329 marker.pose.orientation.w = 1.0;
00330 marker.pose.position.x = x_pos;
00331 marker.scale.x = 0.02;
00332 marker.scale.y = 0.02;
00333 marker.scale.z = 0.02;
00334 marker.color.r = 1.0;
00335 marker.color.g = 0.0;
00336 marker.color.b = 1.0;
00337 marker.color.a = 1.0;
00338 marker.frame_locked = true;
00339
00340 for (int x = 0; x < 10; ++x)
00341 {
00342 for (int y = 0; y < 10; ++y)
00343 {
00344 for (int z = 0; z < 10; ++z)
00345 {
00346 geometry_msgs::Point p;
00347 p.x = x * 0.1f;
00348 p.y = y * 0.1f;
00349 p.z = z * 0.1f;
00350
00351 marker.points.push_back(p);
00352
00353 std_msgs::ColorRGBA c;
00354 c.r = x * 0.1;
00355 c.g = y * 0.1;
00356 c.b = z * 0.1;
00357 marker.colors.push_back(c);
00358 }
00359 }
00360 }
00361 g_marker_pub.publish(marker);
00362 }
00363
00364 x_pos += 3;
00365
00366 {
00367 int count = 10;
00368 visualization_msgs::Marker marker;
00369 marker.header.frame_id = "/base_link";
00370 marker.header.stamp = ros::Time::now();
00371 marker.ns = "marker_test_line_list";
00372 marker.id = 0;
00373 marker.type = visualization_msgs::Marker::LINE_LIST;
00374 marker.action = visualization_msgs::Marker::ADD;
00375 marker.pose.position.x = 0.0;
00376 marker.pose.position.y = 0.0;
00377 marker.pose.position.z = 0.0;
00378 marker.pose.orientation.x = 0.0;
00379 marker.pose.orientation.y = 0.0;
00380 marker.pose.orientation.z = 0.0;
00381 marker.pose.orientation.w = 1.0;
00382 marker.pose.position.x = x_pos;
00383 marker.scale.x = 0.1;
00384 marker.color.r = 1.0;
00385 marker.color.a = 1.0;
00386 marker.frame_locked = true;
00387
00388 for (int i = 0; i < count; ++i)
00389 {
00390 geometry_msgs::Point p1, p2;
00391 p1.x = 0;
00392 p1.y = (i - count / 2) * 2;
00393 p1.z = 0;
00394 p2.x = 0;
00395 p2.y = (i - count / 2) * 2;
00396 p2.z = 1;
00397 marker.points.push_back(p1);
00398 marker.points.push_back(p2);
00399 }
00400 g_marker_pub.publish(marker);
00401 }
00402
00403 x_pos += 3;
00404
00405 {
00406 int count = 10;
00407 visualization_msgs::Marker marker;
00408 marker.header.frame_id = "/base_link";
00409 marker.header.stamp = ros::Time::now();
00410 marker.ns = "marker_test_line_list_color_per";
00411 marker.id = 0;
00412 marker.type = visualization_msgs::Marker::LINE_LIST;
00413 marker.action = visualization_msgs::Marker::ADD;
00414 marker.pose.position.x = 0.0;
00415 marker.pose.position.y = 0.0;
00416 marker.pose.position.z = 0.0;
00417 marker.pose.orientation.x = 0.0;
00418 marker.pose.orientation.y = 0.0;
00419 marker.pose.orientation.z = 0.0;
00420 marker.pose.orientation.w = 1.0;
00421 marker.pose.position.x = x_pos;
00422 marker.scale.x = 0.1;
00423 marker.color.r = 1.0;
00424 marker.color.a = 1.0;
00425 marker.frame_locked = true;
00426
00427 for (int i = 0; i < count; ++i)
00428 {
00429 geometry_msgs::Point p1, p2;
00430 p1.x = 0;
00431 p1.y = (i - count / 2) * 2;
00432 p1.z = 0;
00433 p2.x = 0;
00434 p2.y = (i - count / 2) * 2;
00435 p2.z = 1;
00436 marker.points.push_back(p1);
00437 marker.points.push_back(p2);
00438
00439 std_msgs::ColorRGBA c;
00440 float pct = (float)i / (float)count;
00441 c.r = pct * 1.0 + (1 - pct) * 0.0;
00442 c.g = pct * 0.0 + (1 - pct) * 0.0;
00443 c.b = pct * 0.0 + (1 - pct) * 1.0;
00444
00445 marker.colors.push_back(c);
00446 marker.colors.push_back(c);
00447 }
00448 g_marker_pub.publish(marker);
00449 }
00450
00451 x_pos += 3;
00452
00453 {
00454 visualization_msgs::Marker marker;
00455 marker.header.frame_id = "/base_link";
00456 marker.header.stamp = ros::Time::now();
00457 marker.ns = "marker_test_line_strip";
00458 marker.id = 0;
00459 marker.type = visualization_msgs::Marker::LINE_STRIP;
00460 marker.action = visualization_msgs::Marker::ADD;
00461 marker.pose.position.x = 0.0;
00462 marker.pose.position.y = 0.0;
00463 marker.pose.position.z = 0.0;
00464 marker.pose.orientation.x = 0.0;
00465 marker.pose.orientation.y = 0.0;
00466 marker.pose.orientation.z = 0.0;
00467 marker.pose.orientation.w = 1.0;
00468 marker.pose.position.x = x_pos;
00469 marker.scale.x = 0.1;
00470 marker.color.g = 1.0;
00471 marker.color.a = 1.0;
00472 marker.frame_locked = true;
00473
00474 for (int i = -5; i < 5; ++i)
00475 {
00476 geometry_msgs::Point p;
00477 p.x = 1 + (i % 2);
00478 p.y = (i * 2);
00479 p.z = 0;
00480 marker.points.push_back(p);
00481 }
00482
00483 g_marker_pub.publish(marker);
00484 }
00485
00486 x_pos += 3;
00487
00488 {
00489 visualization_msgs::Marker marker;
00490 marker.header.frame_id = "/base_link";
00491 marker.header.stamp = ros::Time::now();
00492 marker.ns = "marker_test_line_strip_color_per";
00493 marker.id = 0;
00494 marker.type = visualization_msgs::Marker::LINE_STRIP;
00495 marker.action = visualization_msgs::Marker::ADD;
00496 marker.pose.position.x = 0.0;
00497 marker.pose.position.y = 0.0;
00498 marker.pose.position.z = 0.0;
00499 marker.pose.orientation.x = 0.0;
00500 marker.pose.orientation.y = 0.0;
00501 marker.pose.orientation.z = 0.0;
00502 marker.pose.orientation.w = 1.0;
00503 marker.pose.position.x = x_pos;
00504 marker.scale.x = 0.1;
00505 marker.color.g = 1.0;
00506 marker.color.a = 1.0;
00507 marker.frame_locked = true;
00508
00509 for (int i = -5; i < 5; ++i)
00510 {
00511 geometry_msgs::Point p;
00512 p.x = 1 + (i % 2);
00513 p.y = (i * 2);
00514 p.z = 0;
00515 marker.points.push_back(p);
00516
00517 std_msgs::ColorRGBA c;
00518 float pct = (i + 5) / 10.0;
00519 c.r = pct * 0.0 + (1 - pct) * 0.0;
00520 c.g = pct * 1.0 + (1 - pct) * 0.0;
00521 c.b = pct * 0.0 + (1 - pct) * 1.0;
00522
00523 marker.colors.push_back(c);
00524 }
00525
00526 g_marker_pub.publish(marker);
00527 }
00528
00529 x_pos += 3;
00530
00531 {
00532 visualization_msgs::Marker marker;
00533 marker.header.frame_id = "/base_link";
00534 marker.header.stamp = ros::Time::now();
00535 marker.ns = "marker_test_triangle_list";
00536 marker.id = 0;
00537 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00538 marker.action = visualization_msgs::Marker::ADD;
00539 marker.pose.position.x = 0.0;
00540 marker.pose.position.y = 0.0;
00541 marker.pose.position.z = 0.0;
00542 marker.pose.orientation.x = 0.0;
00543 marker.pose.orientation.y = 0.0;
00544 marker.pose.orientation.z = 0.0;
00545 marker.pose.orientation.w = 1.0;
00546 marker.pose.position.x = x_pos;
00547 marker.scale.x = 1.0;
00548 marker.scale.y = 1.0;
00549 marker.scale.z = 1.0;
00550 marker.color.g = 1.0;
00551 marker.color.a = 1.0;
00552 marker.frame_locked = true;
00553
00554 for (int x = 0; x < 10; ++x)
00555 {
00556 for (int y = 0; y < 10; ++y)
00557 {
00558 for (int z = 0; z < 10; ++z)
00559 {
00560 geometry_msgs::Point p;
00561 p.x = x * 0.1f;
00562 p.y = y * 0.1f;
00563 p.z = z * 0.1f;
00564
00565 geometry_msgs::Point p2 = p;
00566 p2.x = p.x + 0.05;
00567
00568 geometry_msgs::Point p3 = p;
00569 p3.x = p2.x;
00570 p3.z = p.z + 0.05;
00571
00572 marker.points.push_back(p);
00573 marker.points.push_back(p2);
00574 marker.points.push_back(p3);
00575
00576 std_msgs::ColorRGBA c;
00577 c.r = x * 0.1;
00578 c.g = y * 0.1;
00579 c.b = z * 0.1;
00580 marker.colors.push_back(c);
00581 marker.colors.push_back(c);
00582 marker.colors.push_back(c);
00583 }
00584 }
00585 }
00586
00587 g_marker_pub.publish(marker);
00588 }
00589
00590 ++counter;
00591 }
00592
00593 void frameCallback(const ros::TimerEvent&)
00594 {
00595 static uint32_t counter = 0;
00596
00597 static tf::TransformBroadcaster br;
00598 tf::Transform t;
00599
00600 t.setOrigin(tf::Vector3(0.0, 0.0, (counter % 1000) * 0.01));
00601 t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00602 br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "base_link", "my_link"));
00603
00604 t.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00605 t.setRotation(tf::createQuaternionFromRPY(M_PI*0.25, M_PI*0.25, 0.0));
00606 br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "rotate_base_link", "base_link"));
00607
00608 ++counter;
00609 }
00610
00611 int main(int argc, char** argv)
00612 {
00613 ros::init(argc, argv, "marker_test");
00614 ros::NodeHandle n;
00615
00616 g_marker_pub = n.advertise<visualization_msgs::Marker> ("visualization_marker", 0);
00617 ros::Timer publish_timer = n.createTimer(ros::Duration(1), publishCallback);
00618 ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
00619
00620 tf::TransformBroadcaster tf_broadcaster;
00621
00622 ros::Duration(0.1).sleep();
00623
00624 ros::spin();
00625 }