3 #include "visualization_msgs/Marker.h" 4 #include "visualization_msgs/MarkerArray.h" 11 void publishText(
int&
id,
float x,
float y,
const std::string& text)
13 visualization_msgs::Marker marker;
14 marker.header.frame_id =
"/base_link";
17 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
18 marker.action = visualization_msgs::Marker::ADD;
19 marker.pose.orientation.x = 0.0;
20 marker.pose.orientation.y = 0.0;
21 marker.pose.orientation.z = 0.0;
22 marker.pose.orientation.w = 1.0;
30 marker.pose.position.x = x;
31 marker.pose.position.y = y;
43 bool use_embedded_materials,
46 using visualization_msgs::Marker;
49 marker.header.frame_id =
"/base_link";
55 marker.type = Marker::MESH_RESOURCE;
56 marker.mesh_resource =
"package://rviz/src/test/meshes/pr2-base.dae";
57 marker.mesh_use_embedded_materials = use_embedded_materials;
60 marker.type = Marker::MESH_RESOURCE;
61 marker.mesh_resource =
"package://rviz/src/test/meshes/frame.dae";
62 marker.mesh_use_embedded_materials = use_embedded_materials;
65 marker.type = Marker::SPHERE;
68 marker.action = Marker::ADD;
69 marker.pose.orientation.x = 0.0;
70 marker.pose.orientation.y = 0.0;
71 marker.pose.orientation.z = 0.0995;
72 marker.pose.orientation.w = 0.995;
80 marker.frame_locked =
true;
82 marker.pose.position.x = x;
83 marker.pose.position.y = y;
88 void publishMeshes(
int&
id,
float x,
float r,
float g,
float b,
float a)
92 snprintf(buffer,
sizeof(buffer),
"rbg: %.1f %.1f %.1f, a: %.1f", r, g, b, a);
114 publishText(
id, x, y++,
"mesh1, use_embedded_materials = true");
115 publishText(
id, x, y++,
"mesh1, use_embedded_materials = false");
116 publishText(
id, x, y++,
"mesh2, use_embedded_materials = true");
117 publishText(
id, x, y++,
"mesh2, use_embedded_materials = false");
118 publishText(
id, x, y++,
"sphere, use_embedded_materials = true");
119 publishText(
id, x, y++,
"sphere, use_embedded_materials = false");
135 static float rgba[4] = {0, 0, 0, 0};
142 if (rgba[index] > 1.01)
143 rgba[index--] = 0.0f;
154 int main(
int argc,
char** argv)
156 ros::init(argc, argv,
"mesh_marker_test");
159 bool latch_only =
false;
162 if (std::string(argv[1]) ==
"--latch-only")
168 g_marker_pub = n.
advertise<visualization_msgs::Marker>(
"mesh_markers", 0, latch_only);
void publishMeshes(int &id, float x, float r, float g, float b, float a)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void publish(const boost::shared_ptr< M > &message) const
void publishCallback(const ros::TimerEvent &)
ros::Publisher g_marker_pub
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishMesh(int &id, float x, float y, float r, float g, float b, float a, bool use_embedded_materials, int mesh)
void publishText(int &id, float x, float y, const std::string &text)
int main(int argc, char **argv)