marker_test.cpp
Go to the documentation of this file.
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 //    ros_time.sec -=1;
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 = (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           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   x_pos += 3;
00591 
00592   {
00593     visualization_msgs::Marker marker;
00594     marker.header.frame_id = "/base_link";
00595     marker.header.stamp = ros::Time::now();
00596     marker.ns = "marker_test_mesh_color_change";
00597     marker.id = 0;
00598     marker.type = visualization_msgs::Marker::MESH_RESOURCE;
00599     marker.action = visualization_msgs::Marker::ADD;
00600     marker.pose.position.x = 0.0;
00601     marker.pose.position.y = 0.0;
00602     marker.pose.position.z = 0.0;
00603     marker.pose.orientation.x = 0.0;
00604     marker.pose.orientation.y = 0.0;
00605     marker.pose.orientation.z = 0.0;
00606     marker.pose.orientation.w = 1.0;
00607     marker.pose.position.x = x_pos;
00608     marker.scale.x = 1.0;
00609     marker.scale.y = 1.0;
00610     marker.scale.z = 1.0;
00611     marker.color.r = float(counter % 255) / 255;
00612     marker.color.g = float((counter*3) % 255) / 255;
00613     marker.color.b = float((counter*10) % 255) / 255;
00614     marker.color.a = 1.0;
00615     marker.frame_locked = true;
00616     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
00617     marker.mesh_use_embedded_materials = false;
00618 
00619     g_marker_pub.publish(marker);
00620   }
00621 
00622   ++counter;
00623 }
00624 
00625 void frameCallback(const ros::TimerEvent&)
00626 {
00627   static uint32_t counter = 0;
00628 
00629   static tf::TransformBroadcaster br;
00630   tf::Transform t;
00631 
00632   t.setOrigin(tf::Vector3(0.0, 0.0, (counter % 1000) * 0.01));
00633   t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00634   br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "base_link", "my_link"));
00635 
00636   t.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00637   t.setRotation(tf::createQuaternionFromRPY(M_PI*0.25, M_PI*0.25, 0.0));
00638   br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "rotate_base_link", "base_link"));
00639 
00640   ++counter;
00641 }
00642 
00643 int main(int argc, char** argv)
00644 {
00645   ros::init(argc, argv, "marker_test");
00646   ros::NodeHandle n;
00647 
00648   g_marker_pub = n.advertise<visualization_msgs::Marker> ("visualization_marker", 0);
00649   ros::Timer publish_timer = n.createTimer(ros::Duration(1), publishCallback);
00650   ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
00651 
00652   tf::TransformBroadcaster tf_broadcaster;
00653 
00654   ros::Duration(0.1).sleep();
00655 
00656   ros::spin();
00657 }


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32