44 void autoComplete( visualization_msgs::InteractiveMarker &msg,
bool enable_autocomplete_transparency )
47 if ( msg.controls.empty() )
59 if ( msg.pose.orientation.w == 0 && msg.pose.orientation.x == 0 &&
60 msg.pose.orientation.y == 0 && msg.pose.orientation.z == 0 )
62 msg.pose.orientation.w = 1;
66 tf::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
67 msg.pose.orientation.z, msg.pose.orientation.w );
69 msg.pose.orientation.x = int_marker_orientation.x();
70 msg.pose.orientation.y = int_marker_orientation.y();
71 msg.pose.orientation.z = int_marker_orientation.z();
72 msg.pose.orientation.w = int_marker_orientation.w();
75 for (
unsigned c=0; c<msg.controls.size(); c++ )
77 autoComplete( msg, msg.controls[c], enable_autocomplete_transparency);
85 int uniqueification_number = 0;
86 std::set<std::string> names;
87 for(
unsigned c = 0; c < msg.controls.size(); c++ )
89 std::string name = msg.controls[c].name;
90 while( names.find( name ) != names.end() )
93 ss << name <<
"_u" << uniqueification_number++;
96 msg.controls[c].name = name;
102 visualization_msgs::InteractiveMarkerControl &control,
bool enable_autocomplete_transparency)
105 if ( control.orientation.w == 0 && control.orientation.x == 0 &&
106 control.orientation.y == 0 && control.orientation.z == 0 )
108 control.orientation.w = 1;
112 if ( control.markers.empty() )
114 switch ( control.interaction_mode )
116 case visualization_msgs::InteractiveMarkerControl::NONE:
119 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
120 control.markers.reserve(2);
125 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
126 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
127 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
131 case visualization_msgs::InteractiveMarkerControl::BUTTON:
134 case visualization_msgs::InteractiveMarkerControl::MENU:
144 tf::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
145 msg.pose.orientation.z, msg.pose.orientation.w );
146 tf::Vector3 int_marker_position( msg.pose.position.x, msg.pose.position.y, msg.pose.position.z );
149 for (
unsigned m=0; m<control.markers.size(); m++ )
151 visualization_msgs::Marker &marker = control.markers[m];
153 if ( marker.scale.x == 0 )
157 if ( marker.scale.y == 0 )
161 if ( marker.scale.z == 0 )
166 marker.ns = msg.name;
169 if ( marker.pose.orientation.w == 0 && marker.pose.orientation.x == 0 &&
170 marker.pose.orientation.y == 0 && marker.pose.orientation.z == 0 )
172 marker.pose.orientation.w = 1;
176 tf::Quaternion marker_orientation( marker.pose.orientation.x, marker.pose.orientation.y,
177 marker.pose.orientation.z, marker.pose.orientation.w );
181 marker.pose.orientation.x = marker_orientation.x();
182 marker.pose.orientation.y = marker_orientation.y();
183 marker.pose.orientation.z = marker_orientation.z();
184 marker.pose.orientation.w = marker_orientation.w();
186 static volatile unsigned id = 0;
188 marker.ns = msg.name;
191 if ( !enable_autocomplete_transparency && marker.color.a > 0.0 )
193 marker.color.a = 1.0;
198 void makeArrow(
const visualization_msgs::InteractiveMarker &msg,
199 visualization_msgs::InteractiveMarkerControl &control,
float pos )
201 visualization_msgs::Marker marker;
204 marker.pose.orientation = control.orientation;
206 marker.type = visualization_msgs::Marker::ARROW;
207 marker.scale.x = msg.scale * 0.15;
208 marker.scale.y = msg.scale * 0.25;
209 marker.scale.z = msg.scale * 0.2;
213 float dist = fabs(pos);
214 float dir = pos > 0 ? 1 : -1;
216 float inner = 0.5 * dist;
217 float outer = inner + 0.4;
219 marker.points.resize(2);
220 marker.points[0].x = dir * msg.scale * inner;
221 marker.points[1].x = dir * msg.scale * outer;
223 control.markers.push_back( marker );
226 void makeDisc(
const visualization_msgs::InteractiveMarker &msg,
227 visualization_msgs::InteractiveMarkerControl &control,
float width )
229 visualization_msgs::Marker marker;
232 marker.pose.orientation = control.orientation;
234 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
235 marker.scale.x = msg.scale;
236 marker.scale.y = msg.scale;
237 marker.scale.z = msg.scale;
243 std::vector<geometry_msgs::Point> circle1, circle2;
244 circle1.reserve(steps);
245 circle2.reserve(steps);
247 geometry_msgs::Point v1,v2;
249 for (
int i=0; i<steps; i++ )
251 float a = float(i)/float(steps) * M_PI * 2.0;
256 v2.y = (1+width) * v1.y;
257 v2.z = (1+width) * v1.z;
259 circle1.push_back( v1 );
260 circle2.push_back( v2 );
263 marker.points.resize(6*steps);
265 std_msgs::ColorRGBA color;
266 color.r=color.g=color.b=color.a=1;
268 switch ( control.interaction_mode )
270 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
272 marker.colors.resize(2*steps);
273 std_msgs::ColorRGBA base_color = marker.color;
274 for (
int i=0; i<steps; i++ )
277 int i2 = (i+1) % steps;
278 int i3 = (i+2) % steps;
283 marker.points[p+0] = circle1[i1];
284 marker.points[p+1] = circle2[i2];
285 marker.points[p+2] = circle1[i2];
287 marker.points[p+3] = circle1[i2];
288 marker.points[p+4] = circle2[i2];
289 marker.points[p+5] = circle2[i3];
291 float t = 0.6 + 0.4 * (i%2);
292 color.r = base_color.r * t;
293 color.g = base_color.g * t;
294 color.b = base_color.b * t;
296 marker.colors[c] = color;
297 marker.colors[c+1] = color;
302 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
304 marker.colors.resize(2*steps);
305 std_msgs::ColorRGBA base_color = marker.color;
306 for (
int i=0; i<steps-1; i+=2 )
309 int i2 = (i+1) % steps;
310 int i3 = (i+2) % steps;
315 marker.points[p+0] = circle1[i1];
316 marker.points[p+1] = circle2[i2];
317 marker.points[p+2] = circle1[i2];
319 marker.points[p+3] = circle1[i2];
320 marker.points[p+4] = circle2[i2];
321 marker.points[p+5] = circle1[i3];
323 color.r = base_color.r * 0.6;
324 color.g = base_color.g * 0.6;
325 color.b = base_color.b * 0.6;
327 marker.colors[c] = color;
328 marker.colors[c+1] = color;
333 marker.points[p+0] = circle2[i1];
334 marker.points[p+1] = circle2[i2];
335 marker.points[p+2] = circle1[i1];
337 marker.points[p+3] = circle2[i2];
338 marker.points[p+4] = circle2[i3];
339 marker.points[p+5] = circle1[i3];
341 marker.colors[c] = base_color;
342 marker.colors[c+1] = base_color;
348 for (
int i=0; i<steps; i++ )
351 int i2 = (i+1) % steps;
355 marker.points[p+0] = circle1[i1];
356 marker.points[p+1] = circle2[i1];
357 marker.points[p+2] = circle1[i2];
359 marker.points[p+3] = circle2[i1];
360 marker.points[p+4] = circle2[i2];
361 marker.points[p+5] = circle1[i2];
366 control.markers.push_back(marker);
370 visualization_msgs::InteractiveMarkerControl &control, std::string text )
372 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
373 control.independent_marker_orientation =
false;
375 visualization_msgs::Marker marker;
377 float base_scale = 0.25 * msg.scale;
378 float base_z = 1.2 * msg.scale;
380 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
381 marker.scale.x = base_scale;
382 marker.scale.y = base_scale;
383 marker.scale.z = base_scale;
384 marker.color.r = 1.0;
385 marker.color.g = 1.0;
386 marker.color.b = 1.0;
387 marker.color.a = 1.0;
388 marker.pose.position.x = base_scale * -0.1;
389 marker.pose.position.z = base_z + base_scale * -0.1;
392 control.markers.push_back( marker );
398 geometry_msgs::Vector3 v;
401 tf::Vector3 bt_x_axis =
tf::Matrix3x3(bt_quat) * tf::Vector3(1,0,0);
404 x = fabs(bt_x_axis.x());
405 y = fabs(bt_x_axis.y());
406 z = fabs(bt_x_axis.z());
408 float max_xy = x>y ? x : y;
409 float max_yz = y>z ? y : z;
410 float max_xyz = max_xy > max_yz ? max_xy : max_yz;
412 marker.color.r = x / max_xyz;
413 marker.color.g = y / max_xyz;
414 marker.color.b = z / max_xyz;
415 marker.color.a = 0.5;
419 visualization_msgs::InteractiveMarkerControl
makeTitle(
const visualization_msgs::InteractiveMarker &msg )
421 visualization_msgs::Marker marker;
423 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
424 marker.scale.x = msg.scale * 0.15;
425 marker.scale.y = msg.scale * 0.15;
426 marker.scale.z = msg.scale * 0.15;
427 marker.color.r = 1.0;
428 marker.color.g = 1.0;
429 marker.color.b = 1.0;
430 marker.color.a = 1.0;
431 marker.pose.position.z = msg.scale * 1.4;
432 marker.text = msg.description;
434 visualization_msgs::InteractiveMarkerControl control;
435 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::NONE;
436 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
437 control.always_visible =
true;
438 control.markers.push_back( marker );
INTERACTIVE_MARKERS_PUBLIC void makeViewFacingButton(const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, std::string text)
make a box which shows the given text and is view facing
INTERACTIVE_MARKERS_PUBLIC visualization_msgs::InteractiveMarkerControl makeTitle(const visualization_msgs::InteractiveMarker &msg)
create a control which shows the description of the interactive marker
INTERACTIVE_MARKERS_PUBLIC void autoComplete(visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency=true)
fill in default values & insert default controls when none are specified.
INTERACTIVE_MARKERS_PUBLIC void uniqueifyControlNames(visualization_msgs::InteractiveMarker &msg)
Make sure all the control names are unique within the given msg.
INTERACTIVE_MARKERS_PUBLIC void assignDefaultColor(visualization_msgs::Marker &marker, const geometry_msgs::Quaternion &quat)
assign an RGB value to the given marker based on the given orientation
INTERACTIVE_MARKERS_PUBLIC void makeArrow(const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, float pos)
— marker helpers —
INTERACTIVE_MARKERS_PUBLIC void makeDisc(const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, float width=0.3)
make a default-style disc marker (e.g for rotating) based on the properties of the given interactive ...