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
12 publishText(int id, float x, float y, const std::string & text)
13 {
14  visualization_msgs::Marker marker;
15  marker.header.frame_id = "/base_link";
16  marker.header.stamp = ros::Time::now();
17  marker.ns = "mesh";
18  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
19  marker.action = visualization_msgs::Marker::ADD;
20  marker.pose.orientation.x = 0.0;
21  marker.pose.orientation.y = 0.0;
22  marker.pose.orientation.z = 0.0;
23  marker.pose.orientation.w = 1.0;
24  marker.color.r = 1;
25  marker.color.g = 1;
26  marker.color.b = 1;
27  marker.color.a = 1;
28  marker.scale.x = 0.2;
29  marker.scale.y = 0.2;
30  marker.scale.z = 0.2;
31  marker.text = text;
32  marker.id = id;
33  marker.pose.position.x = x;
34  marker.pose.position.y = y;
35 
36  g_marker_pub.publish(marker);
37 }
38 
39 void
41  int id, float x, float y,
42  float r, float g, float b, float a,
43  bool use_embedded_materials, bool mesh)
44 {
45  using visualization_msgs::Marker;
46 
47  Marker marker;
48  marker.header.frame_id = "/base_link";
49  marker.header.stamp = ros::Time::now();
50  marker.ns = "mesh";
51  if (mesh) {
52  marker.type = Marker::MESH_RESOURCE;
53  } else {
54  marker.type = Marker::SPHERE;
55  }
56  marker.action = Marker::ADD;
57  marker.pose.orientation.x = 0.0;
58  marker.pose.orientation.y = 0.0;
59  marker.pose.orientation.z = 0.1;
60  marker.pose.orientation.w = 1.0;
61  marker.scale.x = 1;
62  marker.scale.y = 1;
63  marker.scale.z = 1;
64  marker.color.r = r;
65  marker.color.g = g;
66  marker.color.b = b;
67  marker.color.a = a;
68  marker.frame_locked = true;
69  marker.mesh_resource = "package://rviz/src/test/meshes/pr2-base.dae";
70  marker.mesh_use_embedded_materials = use_embedded_materials;
71  marker.id = id;
72  marker.pose.position.x = x;
73  marker.pose.position.y = y;
74 
75  g_marker_pub.publish(marker);
76 }
77 
78 void
80 {
81  ROS_INFO("Publishing");
82 
83  int id = 0;
84  float x = 0;
85  float y = 0;
86 
87  publishText(id, x - 1, y, "mesh, use_embedded_materials = true");
88  id++;
89  publishText(id, x, y - 1, "rbg: 0.0 0.0 0.0, a: 0");
90  id++;
91  publishMesh(id, x, y, 0, 0, 0, 0, true, true);
92  id++; x++;
93  publishText(id, x, y - 1, "rbg: 1.0 1.0 1.0, a: 1");
94  id++;
95  publishMesh(id, x, y, 1, 1, 1, 1, true, true);
96  id++; x++;
97  publishText(id, x, y - 1, "rbg: 1.0 1.0 1.0, a: 0.5");
98  id++;
99  publishMesh(id, x, y, 1, 1, 1, .5, true, true);
100  id++; x++;
101  publishText(id, x, y - 1, "rbg: 1.0 1.0 1.0, a: 0");
102  id++;
103  publishMesh(id, x, y, 1, 1, 1, 0, true, true);
104  id++; x++;
105  publishText(id, x, y - 1, "rbg: 1.0 0.0 0.0, a: 1");
106  id++;
107  publishMesh(id, x, y, 1, 0, 0, 1, true, true);
108  id++; x++;
109  publishText(id, x, y - 1, "rbg: 1.0 0.0 0.0, a: 0.5");
110  id++;
111  publishMesh(id, x, y, 1, 0, 0, .5, true, true);
112  id++; x++;
113  publishText(id, x, y - 1, "rbg: 1.0 0.0 0.0, a: 0");
114  id++;
115  publishMesh(id, x, y, 1, 0, 0, 0, true, true);
116  id++; x++;
117  publishText(id, x, y - 1, "rbg: 1.0 0.5 0.5, a: 1");
118  id++;
119  publishMesh(id, x, y, 1, .5, .5, 1, true, true);
120  id++; x++;
121  publishText(id, x, y - 1, "rbg: 1.0 0.5 0.5, a: 0.5");
122  id++;
123  publishMesh(id, x, y, 1, .5, .5, .5, true, true);
124  id++; x++;
125  publishText(id, x, y - 1, "rbg: 1.0 0.5 0.5, a: 0");
126  id++;
127  publishMesh(id, x, y, 1, .5, .5, 0, true, true);
128  id++; x++;
129 
130  y++; x = 0;
131 
132  publishText(id, x - 1, y, "mesh, use_embedded_materials = false");
133  id++;
134  publishMesh(id, x, y, 0, 0, 0, 0, false, true);
135  id++; x++;
136  publishMesh(id, x, y, 1, 1, 1, 1, false, true);
137  id++; x++;
138  publishMesh(id, x, y, 1, 1, 1, .5, false, true);
139  id++; x++;
140  publishMesh(id, x, y, 1, 1, 1, 0, false, true);
141  id++; x++;
142  publishMesh(id, x, y, 1, 0, 0, 1, false, true);
143  id++; x++;
144  publishMesh(id, x, y, 1, 0, 0, .5, false, true);
145  id++; x++;
146  publishMesh(id, x, y, 1, 0, 0, 0, false, true);
147  id++; x++;
148  publishMesh(id, x, y, 1, .5, .5, 1, false, true);
149  id++; x++;
150  publishMesh(id, x, y, 1, .5, .5, .5, false, true);
151  id++; x++;
152  publishMesh(id, x, y, 1, .5, .5, 0, false, true);
153  id++; x++;
154 
155  y++; x = 0;
156 
157  publishText(id, x - 1, y, "sphere, use_embedded_materials = true");
158  id++;
159  publishMesh(id, x, y, 0, 0, 0, 0, true, false);
160  id++; x++;
161  publishMesh(id, x, y, 1, 1, 1, 1, true, false);
162  id++; x++;
163  publishMesh(id, x, y, 1, 1, 1, .5, true, false);
164  id++; x++;
165  publishMesh(id, x, y, 1, 1, 1, 0, true, false);
166  id++; x++;
167  publishMesh(id, x, y, 1, 0, 0, 1, true, false);
168  id++; x++;
169  publishMesh(id, x, y, 1, 0, 0, .5, true, false);
170  id++; x++;
171  publishMesh(id, x, y, 1, 0, 0, 0, true, false);
172  id++; x++;
173  publishMesh(id, x, y, 1, .5, .5, 1, true, false);
174  id++; x++;
175  publishMesh(id, x, y, 1, .5, .5, .5, true, false);
176  id++; x++;
177  publishMesh(id, x, y, 1, .5, .5, 0, true, false);
178  id++; x++;
179 
180  y++; x = 0;
181 
182  publishText(id, x - 1, y, "sphere, use_embedded_materials = false");
183  id++;
184  publishMesh(id, x, y, 0, 0, 0, 0, false, false);
185  id++; x++;
186  publishMesh(id, x, y, 1, 1, 1, 1, false, false);
187  id++; x++;
188  publishMesh(id, x, y, 1, 1, 1, .5, false, false);
189  id++; x++;
190  publishMesh(id, x, y, 1, 1, 1, 0, false, false);
191  id++; x++;
192  publishMesh(id, x, y, 1, 0, 0, 1, false, false);
193  id++; x++;
194  publishMesh(id, x, y, 1, 0, 0, .5, false, false);
195  id++; x++;
196  publishMesh(id, x, y, 1, 0, 0, 0, false, false);
197  id++; x++;
198  publishMesh(id, x, y, 1, .5, .5, 1, false, false);
199  id++; x++;
200  publishMesh(id, x, y, 1, .5, .5, .5, false, false);
201  id++; x++;
202  publishMesh(id, x, y, 1, .5, .5, 0, false, false);
203  id++; x++;
204 }
205 
207 {
208  return publishMeshes();
209 }
210 
211 int main(int argc, char** argv)
212 {
213  ros::init(argc, argv, "mesh_marker_test");
214  ros::NodeHandle n;
215 
216  bool latch_only = false;
217  if (argc == 2) {
218  if (std::string(argv[1]) == "--latch-only") {
219  latch_only = true;
220  }
221  }
222 
223  g_marker_pub = n.advertise<visualization_msgs::Marker>("mesh_markers", 0, /*latch*/ latch_only);
224 
225 
226  ros::Timer publish_timer;
227  if (latch_only) {
228  ros::Duration(1.0).sleep();
229  publishMeshes();
230  } else {
231  publish_timer = n.createTimer(ros::Duration(1), publishCallback);
232  }
233 
234  ros::spin();
235 }
void publish(const boost::shared_ptr< M > &message) const
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void publishMesh(int id, float x, float y, float r, float g, float b, float a, bool use_embedded_materials, bool mesh)
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSCPP_DECL void spin(Spinner &spinner)
void publishCallback(const ros::TimerEvent &)
#define ROS_INFO(...)
void publishMeshes()
ros::Publisher g_marker_pub
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static Time now()
void publishText(int id, float x, float y, const std::string &text)
int main(int argc, char **argv)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51