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://rviz/src/test/meshes/pr2-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 }