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
00012 publishText(int id, float x, float y, const std::string & text)
00013 {
00014 visualization_msgs::Marker marker;
00015 marker.header.frame_id = "/base_link";
00016 marker.header.stamp = ros::Time::now();
00017 marker.ns = "mesh";
00018 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00019 marker.action = visualization_msgs::Marker::ADD;
00020 marker.pose.orientation.x = 0.0;
00021 marker.pose.orientation.y = 0.0;
00022 marker.pose.orientation.z = 0.0;
00023 marker.pose.orientation.w = 1.0;
00024 marker.color.r = 1;
00025 marker.color.g = 1;
00026 marker.color.b = 1;
00027 marker.color.a = 1;
00028 marker.scale.x = 0.2;
00029 marker.scale.y = 0.2;
00030 marker.scale.z = 0.2;
00031 marker.text = text;
00032 marker.id = id;
00033 marker.pose.position.x = x;
00034 marker.pose.position.y = y;
00035
00036 g_marker_pub.publish(marker);
00037 }
00038
00039 void
00040 publishMesh(
00041 int id, float x, float y,
00042 float r, float g, float b, float a,
00043 bool use_embedded_materials, bool mesh)
00044 {
00045 using visualization_msgs::Marker;
00046
00047 Marker marker;
00048 marker.header.frame_id = "/base_link";
00049 marker.header.stamp = ros::Time::now();
00050 marker.ns = "mesh";
00051 if (mesh) {
00052 marker.type = Marker::MESH_RESOURCE;
00053 } else {
00054 marker.type = Marker::SPHERE;
00055 }
00056 marker.action = Marker::ADD;
00057 marker.pose.orientation.x = 0.0;
00058 marker.pose.orientation.y = 0.0;
00059 marker.pose.orientation.z = 0.1;
00060 marker.pose.orientation.w = 1.0;
00061 marker.scale.x = 1;
00062 marker.scale.y = 1;
00063 marker.scale.z = 1;
00064 marker.color.r = r;
00065 marker.color.g = g;
00066 marker.color.b = b;
00067 marker.color.a = a;
00068 marker.frame_locked = true;
00069 marker.mesh_resource = "package://rviz/src/test/meshes/pr2-base.dae";
00070 marker.mesh_use_embedded_materials = use_embedded_materials;
00071 marker.id = id;
00072 marker.pose.position.x = x;
00073 marker.pose.position.y = y;
00074
00075 g_marker_pub.publish(marker);
00076 }
00077
00078 void
00079 publishMeshes()
00080 {
00081 ROS_INFO("Publishing");
00082
00083 int id = 0;
00084 float x = 0;
00085 float y = 0;
00086
00087 publishText(id, x - 1, y, "mesh, use_embedded_materials = true");
00088 id++;
00089 publishText(id, x, y - 1, "rbg: 0.0 0.0 0.0, a: 0");
00090 id++;
00091 publishMesh(id, x, y, 0, 0, 0, 0, true, true);
00092 id++; x++;
00093 publishText(id, x, y - 1, "rbg: 1.0 1.0 1.0, a: 1");
00094 id++;
00095 publishMesh(id, x, y, 1, 1, 1, 1, true, true);
00096 id++; x++;
00097 publishText(id, x, y - 1, "rbg: 1.0 1.0 1.0, a: 0.5");
00098 id++;
00099 publishMesh(id, x, y, 1, 1, 1, .5, true, true);
00100 id++; x++;
00101 publishText(id, x, y - 1, "rbg: 1.0 1.0 1.0, a: 0");
00102 id++;
00103 publishMesh(id, x, y, 1, 1, 1, 0, true, true);
00104 id++; x++;
00105 publishText(id, x, y - 1, "rbg: 1.0 0.0 0.0, a: 1");
00106 id++;
00107 publishMesh(id, x, y, 1, 0, 0, 1, true, true);
00108 id++; x++;
00109 publishText(id, x, y - 1, "rbg: 1.0 0.0 0.0, a: 0.5");
00110 id++;
00111 publishMesh(id, x, y, 1, 0, 0, .5, true, true);
00112 id++; x++;
00113 publishText(id, x, y - 1, "rbg: 1.0 0.0 0.0, a: 0");
00114 id++;
00115 publishMesh(id, x, y, 1, 0, 0, 0, true, true);
00116 id++; x++;
00117 publishText(id, x, y - 1, "rbg: 1.0 0.5 0.5, a: 1");
00118 id++;
00119 publishMesh(id, x, y, 1, .5, .5, 1, true, true);
00120 id++; x++;
00121 publishText(id, x, y - 1, "rbg: 1.0 0.5 0.5, a: 0.5");
00122 id++;
00123 publishMesh(id, x, y, 1, .5, .5, .5, true, true);
00124 id++; x++;
00125 publishText(id, x, y - 1, "rbg: 1.0 0.5 0.5, a: 0");
00126 id++;
00127 publishMesh(id, x, y, 1, .5, .5, 0, true, true);
00128 id++; x++;
00129
00130 y++; x = 0;
00131
00132 publishText(id, x - 1, y, "mesh, use_embedded_materials = false");
00133 id++;
00134 publishMesh(id, x, y, 0, 0, 0, 0, false, true);
00135 id++; x++;
00136 publishMesh(id, x, y, 1, 1, 1, 1, false, true);
00137 id++; x++;
00138 publishMesh(id, x, y, 1, 1, 1, .5, false, true);
00139 id++; x++;
00140 publishMesh(id, x, y, 1, 1, 1, 0, false, true);
00141 id++; x++;
00142 publishMesh(id, x, y, 1, 0, 0, 1, false, true);
00143 id++; x++;
00144 publishMesh(id, x, y, 1, 0, 0, .5, false, true);
00145 id++; x++;
00146 publishMesh(id, x, y, 1, 0, 0, 0, false, true);
00147 id++; x++;
00148 publishMesh(id, x, y, 1, .5, .5, 1, false, true);
00149 id++; x++;
00150 publishMesh(id, x, y, 1, .5, .5, .5, false, true);
00151 id++; x++;
00152 publishMesh(id, x, y, 1, .5, .5, 0, false, true);
00153 id++; x++;
00154
00155 y++; x = 0;
00156
00157 publishText(id, x - 1, y, "sphere, use_embedded_materials = true");
00158 id++;
00159 publishMesh(id, x, y, 0, 0, 0, 0, true, false);
00160 id++; x++;
00161 publishMesh(id, x, y, 1, 1, 1, 1, true, false);
00162 id++; x++;
00163 publishMesh(id, x, y, 1, 1, 1, .5, true, false);
00164 id++; x++;
00165 publishMesh(id, x, y, 1, 1, 1, 0, true, false);
00166 id++; x++;
00167 publishMesh(id, x, y, 1, 0, 0, 1, true, false);
00168 id++; x++;
00169 publishMesh(id, x, y, 1, 0, 0, .5, true, false);
00170 id++; x++;
00171 publishMesh(id, x, y, 1, 0, 0, 0, true, false);
00172 id++; x++;
00173 publishMesh(id, x, y, 1, .5, .5, 1, true, false);
00174 id++; x++;
00175 publishMesh(id, x, y, 1, .5, .5, .5, true, false);
00176 id++; x++;
00177 publishMesh(id, x, y, 1, .5, .5, 0, true, false);
00178 id++; x++;
00179
00180 y++; x = 0;
00181
00182 publishText(id, x - 1, y, "sphere, use_embedded_materials = false");
00183 id++;
00184 publishMesh(id, x, y, 0, 0, 0, 0, false, false);
00185 id++; x++;
00186 publishMesh(id, x, y, 1, 1, 1, 1, false, false);
00187 id++; x++;
00188 publishMesh(id, x, y, 1, 1, 1, .5, false, false);
00189 id++; x++;
00190 publishMesh(id, x, y, 1, 1, 1, 0, false, false);
00191 id++; x++;
00192 publishMesh(id, x, y, 1, 0, 0, 1, false, false);
00193 id++; x++;
00194 publishMesh(id, x, y, 1, 0, 0, .5, false, false);
00195 id++; x++;
00196 publishMesh(id, x, y, 1, 0, 0, 0, false, false);
00197 id++; x++;
00198 publishMesh(id, x, y, 1, .5, .5, 1, false, false);
00199 id++; x++;
00200 publishMesh(id, x, y, 1, .5, .5, .5, false, false);
00201 id++; x++;
00202 publishMesh(id, x, y, 1, .5, .5, 0, false, false);
00203 id++; x++;
00204 }
00205
00206 void publishCallback(const ros::TimerEvent&)
00207 {
00208 return publishMeshes();
00209 }
00210
00211 int main(int argc, char** argv)
00212 {
00213 ros::init(argc, argv, "mesh_marker_test");
00214 ros::NodeHandle n;
00215
00216 bool latch_only = false;
00217 if (argc == 2) {
00218 if (std::string(argv[1]) == "--latch-only") {
00219 latch_only = true;
00220 }
00221 }
00222
00223 g_marker_pub = n.advertise<visualization_msgs::Marker>("mesh_markers", 0, latch_only);
00224
00225
00226 ros::Timer publish_timer;
00227 if (latch_only) {
00228 ros::Duration(1.0).sleep();
00229 publishMeshes();
00230 } else {
00231 publish_timer = n.createTimer(ros::Duration(1), publishCallback);
00232 }
00233
00234 ros::spin();
00235 }