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 = float(i+5) / 10.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 = (i > int((count / 12) % 5));
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 c.a = 1.0;
00220 marker.colors.push_back(c);
00221 }
00222 }
00223 }
00224 g_marker_pub.publish(marker);
00225 }
00226
00227 x_pos += 3;
00228
00229 {
00230 visualization_msgs::Marker marker;
00231 marker.header.frame_id = "/base_link";
00232 marker.header.stamp = ros::Time::now();
00233 marker.ns = "marker_test_point_list_alpha_per";
00234 marker.id = 0;
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 = 0.0;
00247 marker.color.a = 1.0;
00248 marker.frame_locked = true;
00249
00250 for( int type = visualization_msgs::Marker::CUBE_LIST; type <= visualization_msgs::Marker::POINTS; type++ )
00251 {
00252 marker.id = type;
00253 marker.pose.position.x += 0.5;
00254 marker.type = type;
00255
00256 for (int y = 0; y < 10; ++y)
00257 {
00258 geometry_msgs::Point p;
00259 p.x = 0;
00260 p.y = y * 0.1f;
00261 p.z = 0;
00262
00263 marker.points.push_back(p);
00264
00265 std_msgs::ColorRGBA c;
00266 c.r = 1;
00267 c.g = 1;
00268 c.b = 1;
00269 c.a = (float)y * 0.1 + 0.1;
00270 marker.colors.push_back(c);
00271 }
00272 g_marker_pub.publish(marker);
00273 }
00274 }
00275
00276 x_pos += 3;
00277
00278 {
00279 visualization_msgs::Marker marker;
00280 marker.header.frame_id = "/base_link";
00281 marker.header.stamp = ros::Time::now();
00282 marker.ns = "marker_test_sphere_list";
00283 marker.id = 0;
00284 marker.type = visualization_msgs::Marker::SPHERE_LIST;
00285 marker.action = visualization_msgs::Marker::ADD;
00286 marker.pose.orientation.x = 0.0;
00287 marker.pose.orientation.y = 0.0;
00288 marker.pose.orientation.z = 0.0;
00289 marker.pose.orientation.w = 1.0;
00290 marker.pose.position.x = x_pos;
00291 marker.scale.x = 0.05;
00292 marker.scale.y = 0.05;
00293 marker.scale.z = 0.05;
00294 marker.color.r = 1.0;
00295 marker.color.g = 1.0;
00296 marker.color.b = 1.0;
00297 marker.color.a = 1.0;
00298 marker.frame_locked = true;
00299
00300 for (int x = 0; x < 10; ++x)
00301 {
00302 for (int y = 0; y < 10; ++y)
00303 {
00304 for (int z = 0; z < 1; ++z)
00305 {
00306 geometry_msgs::Point p;
00307 p.x = x * 0.1f;
00308 p.y = y * 0.1f;
00309 p.z = z * 0.1f;
00310
00311 marker.points.push_back(p);
00312
00313 std_msgs::ColorRGBA c;
00314 c.r = x * 0.1;
00315 c.g = y * 0.1;
00316 c.b = 0.5;
00317 c.a = 1.0;
00318 marker.colors.push_back(c);
00319 }
00320 }
00321 }
00322 g_marker_pub.publish(marker);
00323 }
00324
00325 x_pos += 3;
00326
00327 {
00328 visualization_msgs::Marker marker;
00329 marker.header.frame_id = "/base_link";
00330 marker.header.stamp = ros::Time::now();
00331 marker.ns = "marker_test_points";
00332 marker.id = 0;
00333 marker.type = visualization_msgs::Marker::POINTS;
00334 marker.action = visualization_msgs::Marker::ADD;
00335 marker.pose.orientation.x = 0.0;
00336 marker.pose.orientation.y = 0.0;
00337 marker.pose.orientation.z = 0.0;
00338 marker.pose.orientation.w = 1.0;
00339 marker.pose.position.x = x_pos;
00340 marker.scale.x = 0.02;
00341 marker.scale.y = 0.02;
00342 marker.scale.z = 0.02;
00343 marker.color.r = 1.0;
00344 marker.color.g = 0.0;
00345 marker.color.b = 1.0;
00346 marker.color.a = 1.0;
00347 marker.frame_locked = true;
00348
00349 for (int x = 0; x < 10; ++x)
00350 {
00351 for (int y = 0; y < 10; ++y)
00352 {
00353 for (int z = 0; z < 10; ++z)
00354 {
00355 geometry_msgs::Point p;
00356 p.x = x * 0.1f;
00357 p.y = y * 0.1f;
00358 p.z = z * 0.1f;
00359
00360 marker.points.push_back(p);
00361 }
00362 }
00363 }
00364 g_marker_pub.publish(marker);
00365 }
00366
00367 x_pos += 3;
00368
00369 {
00370 visualization_msgs::Marker marker;
00371 marker.header.frame_id = "/base_link";
00372 marker.header.stamp = ros::Time::now();
00373 marker.ns = "marker_test_points_color_per";
00374 marker.id = 0;
00375 marker.type = visualization_msgs::Marker::POINTS;
00376 marker.action = visualization_msgs::Marker::ADD;
00377 marker.pose.orientation.x = 0.0;
00378 marker.pose.orientation.y = 0.0;
00379 marker.pose.orientation.z = 0.0;
00380 marker.pose.orientation.w = 1.0;
00381 marker.pose.position.x = x_pos;
00382 marker.scale.x = 0.02;
00383 marker.scale.y = 0.02;
00384 marker.scale.z = 0.02;
00385 marker.color.r = 1.0;
00386 marker.color.g = 0.0;
00387 marker.color.b = 1.0;
00388 marker.color.a = 1.0;
00389 marker.frame_locked = true;
00390
00391 for (int x = 0; x < 10; ++x)
00392 {
00393 for (int y = 0; y < 10; ++y)
00394 {
00395 for (int z = 0; z < 10; ++z)
00396 {
00397 geometry_msgs::Point p;
00398 p.x = x * 0.1f;
00399 p.y = y * 0.1f;
00400 p.z = z * 0.1f;
00401
00402 marker.points.push_back(p);
00403
00404 std_msgs::ColorRGBA c;
00405 c.r = x * 0.1;
00406 c.g = y * 0.1;
00407 c.b = z * 0.1;
00408 c.a = 1.0;
00409 marker.colors.push_back(c);
00410 }
00411 }
00412 }
00413 g_marker_pub.publish(marker);
00414 }
00415
00416 x_pos += 3;
00417
00418 {
00419 int count = 10;
00420 visualization_msgs::Marker marker;
00421 marker.header.frame_id = "/base_link";
00422 marker.header.stamp = ros::Time::now();
00423 marker.ns = "marker_test_line_list";
00424 marker.id = 0;
00425 marker.type = visualization_msgs::Marker::LINE_LIST;
00426 marker.action = visualization_msgs::Marker::ADD;
00427 marker.pose.position.x = 0.0;
00428 marker.pose.position.y = 0.0;
00429 marker.pose.position.z = 0.0;
00430 marker.pose.orientation.x = 0.0;
00431 marker.pose.orientation.y = 0.0;
00432 marker.pose.orientation.z = 0.0;
00433 marker.pose.orientation.w = 1.0;
00434 marker.pose.position.x = x_pos;
00435 marker.scale.x = 0.1;
00436 marker.color.r = 1.0;
00437 marker.color.a = 1.0;
00438 marker.frame_locked = true;
00439
00440 for (int i = 0; i < count; ++i)
00441 {
00442 geometry_msgs::Point p1, p2;
00443 p1.x = 0;
00444 p1.y = (i - count / 2) * 2;
00445 p1.z = 0;
00446 p2.x = 0;
00447 p2.y = (i - count / 2) * 2;
00448 p2.z = 1;
00449 marker.points.push_back(p1);
00450 marker.points.push_back(p2);
00451 }
00452 g_marker_pub.publish(marker);
00453 }
00454
00455 x_pos += 3;
00456
00457 {
00458 int count = 10;
00459 visualization_msgs::Marker marker;
00460 marker.header.frame_id = "/base_link";
00461 marker.header.stamp = ros::Time::now();
00462 marker.ns = "marker_test_line_list_color_per";
00463 marker.id = 0;
00464 marker.type = visualization_msgs::Marker::LINE_LIST;
00465 marker.action = visualization_msgs::Marker::ADD;
00466 marker.pose.position.x = 0.0;
00467 marker.pose.position.y = 0.0;
00468 marker.pose.position.z = 0.0;
00469 marker.pose.orientation.x = 0.0;
00470 marker.pose.orientation.y = 0.0;
00471 marker.pose.orientation.z = 0.0;
00472 marker.pose.orientation.w = 1.0;
00473 marker.pose.position.x = x_pos;
00474 marker.scale.x = 0.1;
00475 marker.color.r = 1.0;
00476 marker.color.a = 1.0;
00477 marker.frame_locked = true;
00478
00479 for (int i = 0; i < count; ++i)
00480 {
00481 geometry_msgs::Point p1, p2;
00482 p1.x = 0;
00483 p1.y = (i - count / 2) * 2;
00484 p1.z = 0;
00485 p2.x = 0;
00486 p2.y = (i - count / 2) * 2;
00487 p2.z = 1;
00488 marker.points.push_back(p1);
00489 marker.points.push_back(p2);
00490
00491 std_msgs::ColorRGBA c;
00492 float pct = (float)i / (float)count;
00493 c.r = pct * 1.0 + (1 - pct) * 0.0;
00494 c.g = pct * 0.0 + (1 - pct) * 0.0;
00495 c.b = pct * 0.0 + (1 - pct) * 1.0;
00496 c.a = 1.0;
00497
00498 marker.colors.push_back(c);
00499 marker.colors.push_back(c);
00500 }
00501 g_marker_pub.publish(marker);
00502 }
00503
00504 x_pos += 3;
00505
00506 {
00507 visualization_msgs::Marker marker;
00508 marker.header.frame_id = "/base_link";
00509 marker.header.stamp = ros::Time::now();
00510 marker.ns = "marker_test_line_strip";
00511 marker.id = 0;
00512 marker.type = visualization_msgs::Marker::LINE_STRIP;
00513 marker.action = visualization_msgs::Marker::ADD;
00514 marker.pose.position.x = 0.0;
00515 marker.pose.position.y = 0.0;
00516 marker.pose.position.z = 0.0;
00517 marker.pose.orientation.x = 0.0;
00518 marker.pose.orientation.y = 0.0;
00519 marker.pose.orientation.z = 0.0;
00520 marker.pose.orientation.w = 1.0;
00521 marker.pose.position.x = x_pos;
00522 marker.scale.x = 0.1;
00523 marker.color.g = 1.0;
00524 marker.color.a = 1.0;
00525 marker.frame_locked = true;
00526
00527 for (int i = -5; i < 5; ++i)
00528 {
00529 geometry_msgs::Point p;
00530 p.x = 1 + (i % 2);
00531 p.y = (i * 2);
00532 p.z = 0;
00533 marker.points.push_back(p);
00534 }
00535
00536 g_marker_pub.publish(marker);
00537 }
00538
00539 x_pos += 3;
00540
00541 {
00542 visualization_msgs::Marker marker;
00543 marker.header.frame_id = "/base_link";
00544 marker.header.stamp = ros::Time::now();
00545 marker.ns = "marker_test_line_strip_color_per";
00546 marker.id = 0;
00547 marker.type = visualization_msgs::Marker::LINE_STRIP;
00548 marker.action = visualization_msgs::Marker::ADD;
00549 marker.pose.position.x = 0.0;
00550 marker.pose.position.y = 0.0;
00551 marker.pose.position.z = 0.0;
00552 marker.pose.orientation.x = 0.0;
00553 marker.pose.orientation.y = 0.0;
00554 marker.pose.orientation.z = 0.0;
00555 marker.pose.orientation.w = 1.0;
00556 marker.pose.position.x = x_pos;
00557 marker.scale.x = 0.1;
00558 marker.color.g = 1.0;
00559 marker.color.a = 1.0;
00560 marker.frame_locked = true;
00561
00562 for (int i = -5; i < 5; ++i)
00563 {
00564 geometry_msgs::Point p;
00565 p.x = 1 + (i % 2);
00566 p.y = (i * 2);
00567 p.z = 0;
00568 marker.points.push_back(p);
00569
00570 std_msgs::ColorRGBA c;
00571 float pct = (i + 5) / 10.0;
00572 c.r = pct * 0.0 + (1 - pct) * 0.0;
00573 c.g = pct * 1.0 + (1 - pct) * 0.0;
00574 c.b = pct * 0.0 + (1 - pct) * 1.0;
00575 c.a = 1.0;
00576
00577 marker.colors.push_back(c);
00578 }
00579
00580 g_marker_pub.publish(marker);
00581 }
00582
00583 x_pos += 3;
00584
00585 {
00586 visualization_msgs::Marker marker;
00587 marker.header.frame_id = "/base_link";
00588 marker.header.stamp = ros::Time::now();
00589 marker.ns = "marker_test_triangle_list";
00590 marker.id = 0;
00591 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00592 marker.action = visualization_msgs::Marker::ADD;
00593 marker.pose.position.x = 0.0;
00594 marker.pose.position.y = 0.0;
00595 marker.pose.position.z = 0.0;
00596 marker.pose.orientation.x = 0.0;
00597 marker.pose.orientation.y = 0.0;
00598 marker.pose.orientation.z = 0.0;
00599 marker.pose.orientation.w = 1.0;
00600 marker.pose.position.x = x_pos;
00601 marker.scale.x = 1.0;
00602 marker.scale.y = 1.0;
00603 marker.scale.z = 1.0;
00604 marker.color.g = 1.0;
00605 marker.color.a = 1.0;
00606 marker.frame_locked = true;
00607
00608 for (int x = 0; x < 10; ++x)
00609 {
00610 for (int y = 0; y < 10; ++y)
00611 {
00612 for (int z = 0; z < 10; ++z)
00613 {
00614 geometry_msgs::Point p;
00615 p.x = x * 0.1f;
00616 p.y = y * 0.1f;
00617 p.z = z * 0.1f;
00618
00619 geometry_msgs::Point p2 = p;
00620 p2.x = p.x + 0.05;
00621
00622 geometry_msgs::Point p3 = p;
00623 p3.x = p2.x;
00624 p3.z = p.z + 0.05;
00625
00626 marker.points.push_back(p);
00627 marker.points.push_back(p2);
00628 marker.points.push_back(p3);
00629
00630 std_msgs::ColorRGBA c;
00631 c.r = x * 0.1;
00632 c.g = y * 0.1;
00633 c.b = z * 0.1;
00634 c.a = 1.0;
00635 marker.colors.push_back(c);
00636 marker.colors.push_back(c);
00637 marker.colors.push_back(c);
00638 }
00639 }
00640 }
00641
00642 g_marker_pub.publish(marker);
00643 }
00644
00645 x_pos += 3;
00646
00647 {
00648 visualization_msgs::Marker marker;
00649 marker.header.frame_id = "/base_link";
00650 marker.header.stamp = ros::Time::now();
00651 marker.ns = "marker_test_mesh_color_change";
00652 marker.id = 0;
00653 marker.type = visualization_msgs::Marker::MESH_RESOURCE;
00654 marker.action = visualization_msgs::Marker::ADD;
00655 marker.pose.position.x = 0.0;
00656 marker.pose.position.y = 0.0;
00657 marker.pose.position.z = 0.0;
00658 marker.pose.orientation.x = 0.0;
00659 marker.pose.orientation.y = 0.0;
00660 marker.pose.orientation.z = 0.0;
00661 marker.pose.orientation.w = 1.0;
00662 marker.pose.position.x = x_pos;
00663 marker.scale.x = 1.0;
00664 marker.scale.y = 1.0;
00665 marker.scale.z = 1.0;
00666 marker.color.r = float(counter % 255) / 255;
00667 marker.color.g = float((counter*3) % 255) / 255;
00668 marker.color.b = float((counter*10) % 255) / 255;
00669 marker.color.a = 1.0;
00670 marker.frame_locked = true;
00671 marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
00672 marker.mesh_use_embedded_materials = false;
00673
00674 g_marker_pub.publish(marker);
00675 }
00676
00677 ++counter;
00678 }
00679
00680 void frameCallback(const ros::TimerEvent&)
00681 {
00682 static uint32_t counter = 0;
00683
00684 static tf::TransformBroadcaster br;
00685 tf::Transform t;
00686
00687 t.setOrigin(tf::Vector3(0.0, 0.0, (counter % 1000) * 0.01));
00688 t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00689 br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "base_link", "my_link"));
00690
00691 t.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00692 t.setRotation(tf::createQuaternionFromRPY(M_PI*0.25, M_PI*0.25, 0.0));
00693 br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "rotate_base_link", "base_link"));
00694
00695 ++counter;
00696 }
00697
00698 int main(int argc, char** argv)
00699 {
00700 ros::init(argc, argv, "marker_test");
00701 ros::NodeHandle n;
00702
00703 g_marker_pub = n.advertise<visualization_msgs::Marker> ("visualization_marker", 0);
00704 ros::Timer publish_timer = n.createTimer(ros::Duration(1), publishCallback);
00705 ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
00706
00707 tf::TransformBroadcaster tf_broadcaster;
00708
00709 ros::Duration(0.1).sleep();
00710
00711 ros::spin();
00712 }