mesh_marker_test.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 
3 #include "visualization_msgs/Marker.h"
4 #include "visualization_msgs/MarkerArray.h"
5 
7 #include <tf/tf.h>
8 
10 
11 void publishText(int& id, float x, float y, const std::string& text)
12 {
13  visualization_msgs::Marker marker;
14  marker.header.frame_id = "/base_link";
15  marker.header.stamp = ros::Time::now();
16  marker.ns = "mesh";
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;
23  marker.color.r = 1;
24  marker.color.g = 1;
25  marker.color.b = 1;
26  marker.color.a = 1;
27  marker.scale.z = 0.2;
28  marker.text = text;
29  marker.id = ++id;
30  marker.pose.position.x = x;
31  marker.pose.position.y = y;
32 
33  g_marker_pub.publish(marker);
34 }
35 
36 void publishMesh(int& id,
37  float x,
38  float y,
39  float r,
40  float g,
41  float b,
42  float a,
43  bool use_embedded_materials,
44  int mesh)
45 {
46  using visualization_msgs::Marker;
47 
48  Marker marker;
49  marker.header.frame_id = "/base_link";
50  marker.header.stamp = ros::Time::now();
51  marker.ns = "mesh";
52  switch (mesh)
53  {
54  case 0:
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;
58  break;
59  case 1:
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;
63  break;
64  case 2:
65  marker.type = Marker::SPHERE;
66  break;
67  }
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;
73  marker.scale.x = 1;
74  marker.scale.y = 1;
75  marker.scale.z = 1;
76  marker.color.r = r;
77  marker.color.g = g;
78  marker.color.b = b;
79  marker.color.a = a;
80  marker.frame_locked = true;
81  marker.id = ++id;
82  marker.pose.position.x = x;
83  marker.pose.position.y = y;
84 
85  g_marker_pub.publish(marker);
86 }
87 
88 void publishMeshes(int& id, float x, float r, float g, float b, float a)
89 {
90  int y = -5;
91  char buffer[30];
92  snprintf(buffer, sizeof(buffer), "rbg: %.1f %.1f %.1f, a: %.1f", r, g, b, a);
93  publishText(id, x, y - 1, buffer);
94 
95  publishMesh(id, x, y++, r, g, b, a, true, 0);
96  publishMesh(id, x, y++, r, g, b, a, false, 0);
97 
98  publishMesh(id, x, y++, r, g, b, a, true, 1);
99  publishMesh(id, x, y++, r, g, b, a, false, 1);
100 
101  publishMesh(id, x, y++, r, g, b, a, true, 2);
102  publishMesh(id, x, y++, r, g, b, a, false, 2);
103 }
104 
106 {
107  ROS_INFO("Publishing");
108 
109  int id = 0;
110  float x = -6;
111  float y = -5;
112 
113  // column headers
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");
120 
121  publishMeshes(id, ++x, 0, 0, 0, 0);
122 
123  publishMeshes(id, ++x, 1, 1, 1, 1);
124  publishMeshes(id, ++x, 1, 1, 1, 0.5);
125  publishMeshes(id, ++x, 1, 1, 1, 0.0);
126 
127  publishMeshes(id, ++x, 1, 0, 0, 1);
128  publishMeshes(id, ++x, 1, 0, 0, 0.5);
129  publishMeshes(id, ++x, 1, 0, 0, 0);
130 
131  publishMeshes(id, ++x, 1, .5, .5, 1);
132  publishMeshes(id, ++x, 1, .5, .5, 0.5);
133  publishMeshes(id, ++x, 1, .5, .5, 0);
134 
135  static float rgba[4] = {0, 0, 0, 0};
136  publishMeshes(id, ++x, rgba[0], rgba[1], rgba[2], rgba[3]);
137  // evolve colors over time
138  int index = 3;
139  while (index >= 0)
140  {
141  rgba[index] += 0.2;
142  if (rgba[index] > 1.01)
143  rgba[index--] = 0.0f;
144  else
145  break;
146  }
147 }
148 
149 void publishCallback(const ros::TimerEvent& /*unused*/)
150 {
151  return publishMeshes();
152 }
153 
154 int main(int argc, char** argv)
155 {
156  ros::init(argc, argv, "mesh_marker_test");
157  ros::NodeHandle n;
158 
159  bool latch_only = false;
160  if (argc == 2)
161  {
162  if (std::string(argv[1]) == "--latch-only")
163  {
164  latch_only = true;
165  }
166  }
167 
168  g_marker_pub = n.advertise<visualization_msgs::Marker>("mesh_markers", 0, /*latch*/ latch_only);
169 
170 
171  ros::Timer publish_timer;
172  if (latch_only)
173  {
174  ros::Duration(1.0).sleep();
175  publishMeshes();
176  }
177  else
178  {
179  publish_timer = n.createTimer(ros::Duration(1), publishCallback);
180  }
181 
182  ros::spin();
183 }
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 &)
#define ROS_INFO(...)
ros::Publisher g_marker_pub
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
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)
static Time now()
bool sleep() const
int main(int argc, char **argv)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Fri Aug 12 2022 02:06:09