mesh_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 publishMesh( int id, float x, float y, float r, float g, float b, float a, bool use_embedded_materials, bool mesh )
00012 {
00013   visualization_msgs::Marker marker;
00014   marker.header.frame_id = "/base_link";
00015   marker.header.stamp = ros::Time::now();
00016   marker.ns = "mesh";
00017   marker.type = mesh ? (int) visualization_msgs::Marker::MESH_RESOURCE : (int) visualization_msgs::Marker::SPHERE;
00018   marker.action = visualization_msgs::Marker::ADD;
00019   marker.pose.orientation.x = 0.0;
00020   marker.pose.orientation.y = 0.0;
00021   marker.pose.orientation.z = 0.1;
00022   marker.pose.orientation.w = 1.0;
00023   marker.scale.x = 1;
00024   marker.scale.y = 1;
00025   marker.scale.z = 1;
00026   marker.color.r = r;
00027   marker.color.g = g;
00028   marker.color.b = b;
00029   marker.color.a = a;
00030   marker.frame_locked = true;
00031   marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
00032   marker.mesh_use_embedded_materials = use_embedded_materials;
00033   marker.id = id;
00034   marker.pose.position.x = x;
00035   marker.pose.position.y = y;
00036 
00037   g_marker_pub.publish(marker);
00038 }
00039 
00040 void publishCallback(const ros::TimerEvent&)
00041 {
00042   static uint32_t counter = 0;
00043 
00044   ROS_INFO("Publishing");
00045 
00046   int id = 0;
00047   float x = 0;
00048   float y = 0;
00049 
00050   publishMesh( id, x, y, 1, 1, 1, 1, true, true);
00051   id++; x++;
00052   publishMesh( id, x, y, 1, 1, 1, .5, true, true);
00053   id++; x++;
00054   publishMesh( id, x, y, 1, 1, 1, 0, true, true);
00055   id++; x++;
00056   publishMesh( id, x, y, 1, 0, 0, 1, true, true);
00057   id++; x++;
00058   publishMesh( id, x, y, 1, 0, 0, .5, true, true);
00059   id++; x++;
00060   publishMesh( id, x, y, 1, 0, 0, 0, true, true);
00061   id++; x++;
00062   publishMesh( id, x, y, 1, .5, .5, 1, true, true);
00063   id++; x++;
00064   publishMesh( id, x, y, 1, .5, .5, .5, true, true);
00065   id++; x++;
00066   publishMesh( id, x, y, 1, .5, .5, 0, true, true);
00067   id++; x++;
00068 
00069   y++; x = 0;
00070 
00071   publishMesh( id, x, y, 1, 1, 1, 1, false, true);
00072   id++; x++;
00073   publishMesh( id, x, y, 1, 1, 1, .5, false, true);
00074   id++; x++;
00075   publishMesh( id, x, y, 1, 1, 1, 0, false, true);
00076   id++; x++;
00077   publishMesh( id, x, y, 1, 0, 0, 1, false, true);
00078   id++; x++;
00079   publishMesh( id, x, y, 1, 0, 0, .5, false, true);
00080   id++; x++;
00081   publishMesh( id, x, y, 1, 0, 0, 0, false, true);
00082   id++; x++;
00083   publishMesh( id, x, y, 1, .5, .5, 1, false, true);
00084   id++; x++;
00085   publishMesh( id, x, y, 1, .5, .5, .5, false, true);
00086   id++; x++;
00087   publishMesh( id, x, y, 1, .5, .5, 0, false, true);
00088   id++; x++;
00089 
00090   y++; x = 0;
00091 
00092   publishMesh( id, x, y, 1, 1, 1, 1, true, false);
00093   id++; x++;
00094   publishMesh( id, x, y, 1, 1, 1, .5, true, false);
00095   id++; x++;
00096   publishMesh( id, x, y, 1, 1, 1, 0, true, false);
00097   id++; x++;
00098   publishMesh( id, x, y, 1, 0, 0, 1, true, false);
00099   id++; x++;
00100   publishMesh( id, x, y, 1, 0, 0, .5, true, false);
00101   id++; x++;
00102   publishMesh( id, x, y, 1, 0, 0, 0, true, false);
00103   id++; x++;
00104   publishMesh( id, x, y, 1, .5, .5, 1, true, false);
00105   id++; x++;
00106   publishMesh( id, x, y, 1, .5, .5, .5, true, false);
00107   id++; x++;
00108   publishMesh( id, x, y, 1, .5, .5, 0, true, false);
00109   id++; x++;
00110 
00111   y++; x = 0;
00112 
00113   publishMesh( id, x, y, 0, 0, 0, 0, true, true);
00114   id++; x++;
00115 
00116   ++counter;
00117 }
00118 
00119 int main(int argc, char** argv)
00120 {
00121   ros::init(argc, argv, "mesh_marker_test");
00122   ros::NodeHandle n;
00123 
00124   g_marker_pub = n.advertise<visualization_msgs::Marker> ("mesh_markers", 0);
00125   ros::Timer publish_timer = n.createTimer(ros::Duration(1), publishCallback);
00126 
00127   ros::Duration(0.1).sleep();
00128 
00129   ros::spin();
00130 }


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