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