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 = 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 }


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:15