tools.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include "interactive_markers/tools.h"
31 
32 #include <tf2/LinearMath/Quaternion.h>
33 #include <tf2/LinearMath/Matrix3x3.h>
34 
35 #include <math.h>
36 #include <assert.h>
37 
38 #include <set>
39 #include <sstream>
40 
41 namespace interactive_markers
42 {
43 
44 void autoComplete( visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency )
45 {
46  // this is a 'delete' message. no need for action.
47  if ( msg.controls.empty() )
48  {
49  return;
50  }
51 
52  // default size
53  if ( msg.scale == 0 )
54  {
55  msg.scale = 1;
56  }
57 
58  // correct empty orientation, normalize
59  if ( msg.pose.orientation.w == 0 && msg.pose.orientation.x == 0 &&
60  msg.pose.orientation.y == 0 && msg.pose.orientation.z == 0 )
61  {
62  msg.pose.orientation.w = 1;
63  }
64 
65  //normalize quaternion
66  tf2::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
67  msg.pose.orientation.z, msg.pose.orientation.w );
68  int_marker_orientation.normalize();
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();
73 
74  // complete the controls
75  for ( unsigned c=0; c<msg.controls.size(); c++ )
76  {
77  autoComplete( msg, msg.controls[c], enable_autocomplete_transparency);
78  }
79 
80  uniqueifyControlNames( msg );
81 }
82 
83 void uniqueifyControlNames( visualization_msgs::InteractiveMarker& msg )
84 {
85  int uniqueification_number = 0;
86  std::set<std::string> names;
87  for( unsigned c = 0; c < msg.controls.size(); c++ )
88  {
89  std::string name = msg.controls[c].name;
90  while( names.find( name ) != names.end() )
91  {
92  std::stringstream ss;
93  ss << name << "_u" << uniqueification_number++;
94  name = ss.str();
95  }
96  msg.controls[c].name = name;
97  names.insert( name );
98  }
99 }
100 
101 void autoComplete( const visualization_msgs::InteractiveMarker &msg,
102  visualization_msgs::InteractiveMarkerControl &control, bool enable_autocomplete_transparency)
103 {
104  // correct empty orientation
105  if ( control.orientation.w == 0 && control.orientation.x == 0 &&
106  control.orientation.y == 0 && control.orientation.z == 0 )
107  {
108  control.orientation.w = 1;
109  }
110 
111  // add default control handles if there are none
112  if ( control.markers.empty() )
113  {
114  switch ( control.interaction_mode )
115  {
116  case visualization_msgs::InteractiveMarkerControl::NONE:
117  break;
118 
119  case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
120  control.markers.reserve(2);
121  makeArrow( msg, control, 1.0 );
122  makeArrow( msg, control, -1.0 );
123  break;
124 
125  case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
126  case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
127  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
128  makeDisc( msg, control );
129  break;
130 
131  case visualization_msgs::InteractiveMarkerControl::BUTTON:
132  break;
133 
134  case visualization_msgs::InteractiveMarkerControl::MENU:
135  makeViewFacingButton( msg, control, control.description );
136  break;
137 
138  default:
139  break;
140  }
141  }
142 
143  // get interactive marker pose for later
144  tf2::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
145  msg.pose.orientation.z, msg.pose.orientation.w );
146  tf2::Vector3 int_marker_position( msg.pose.position.x, msg.pose.position.y, msg.pose.position.z );
147 
148  // fill in missing pose information into the markers
149  for ( unsigned m=0; m<control.markers.size(); m++ )
150  {
151  visualization_msgs::Marker &marker = control.markers[m];
152 
153  if ( marker.scale.x == 0 )
154  {
155  marker.scale.x = 1;
156  }
157  if ( marker.scale.y == 0 )
158  {
159  marker.scale.y = 1;
160  }
161  if ( marker.scale.z == 0 )
162  {
163  marker.scale.z = 1;
164  }
165 
166  marker.ns = msg.name;
167 
168  // correct empty orientation
169  if ( marker.pose.orientation.w == 0 && marker.pose.orientation.x == 0 &&
170  marker.pose.orientation.y == 0 && marker.pose.orientation.z == 0 )
171  {
172  marker.pose.orientation.w = 1;
173  }
174 
175  //normalize orientation
176  tf2::Quaternion marker_orientation( marker.pose.orientation.x, marker.pose.orientation.y,
177  marker.pose.orientation.z, marker.pose.orientation.w );
178 
179  marker_orientation.normalize();
180 
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();
185 
186  static volatile unsigned id = 0;
187  marker.id = id++;
188  marker.ns = msg.name;
189 
190  // If transparency is disabled, set alpha to 1.0 for all semi-transparent markers
191  if ( !enable_autocomplete_transparency && marker.color.a > 0.0 )
192  {
193  marker.color.a = 1.0;
194  }
195  }
196 }
197 
198 void makeArrow( const visualization_msgs::InteractiveMarker &msg,
199  visualization_msgs::InteractiveMarkerControl &control, float pos )
200 {
201  visualization_msgs::Marker marker;
202 
203  // rely on the auto-completion for the correct orientation
204  marker.pose.orientation = control.orientation;
205 
206  marker.type = visualization_msgs::Marker::ARROW;
207  marker.scale.x = msg.scale * 0.15; // aleeper: changed from 0.3 due to Rviz fix
208  marker.scale.y = msg.scale * 0.25; // aleeper: changed from 0.5 due to Rviz fix
209  marker.scale.z = msg.scale * 0.2;
210 
211  assignDefaultColor(marker, control.orientation);
212 
213  float dist = fabs(pos);
214  float dir = pos > 0 ? 1 : -1;
215 
216  float inner = 0.5 * dist;
217  float outer = inner + 0.4;
218 
219  marker.points.resize(2);
220  marker.points[0].x = dir * msg.scale * inner;
221  marker.points[1].x = dir * msg.scale * outer;
222 
223  control.markers.push_back( marker );
224 }
225 
226 void makeDisc( const visualization_msgs::InteractiveMarker &msg,
227  visualization_msgs::InteractiveMarkerControl &control, float width )
228 {
229  visualization_msgs::Marker marker;
230 
231  // rely on the auto-completion for the correct orientation
232  marker.pose.orientation = control.orientation;
233 
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;
238 
239  assignDefaultColor(marker, control.orientation);
240 
241  // compute points on a circle in the y-z plane
242  int steps = 36;
243  std::vector<geometry_msgs::Point> circle1, circle2;
244  circle1.reserve(steps);
245  circle2.reserve(steps);
246 
247  geometry_msgs::Point v1,v2;
248 
249  for ( int i=0; i<steps; i++ )
250  {
251  float a = float(i)/float(steps) * M_PI * 2.0;
252 
253  v1.y = 0.5 * cos(a);
254  v1.z = 0.5 * sin(a);
255 
256  v2.y = (1+width) * v1.y;
257  v2.z = (1+width) * v1.z;
258 
259  circle1.push_back( v1 );
260  circle2.push_back( v2 );
261  }
262 
263  marker.points.resize(6*steps);
264 
265  std_msgs::ColorRGBA color;
266  color.r=color.g=color.b=color.a=1;
267 
268  switch ( control.interaction_mode )
269  {
270  case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
271  {
272  marker.colors.resize(2*steps);
273  std_msgs::ColorRGBA base_color = marker.color;
274  for ( int i=0; i<steps; i++ )
275  {
276  int i1 = i;
277  int i2 = (i+1) % steps;
278  int i3 = (i+2) % steps;
279 
280  int p = i*6;
281  int c = i*2;
282 
283  marker.points[p+0] = circle1[i1];
284  marker.points[p+1] = circle2[i2];
285  marker.points[p+2] = circle1[i2];
286 
287  marker.points[p+3] = circle1[i2];
288  marker.points[p+4] = circle2[i2];
289  marker.points[p+5] = circle2[i3];
290 
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;
295 
296  marker.colors[c] = color;
297  marker.colors[c+1] = color;
298  }
299  break;
300  }
301 
302  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
303  {
304  marker.colors.resize(2*steps);
305  std_msgs::ColorRGBA base_color = marker.color;
306  for ( int i=0; i<steps-1; i+=2 )
307  {
308  int i1 = i;
309  int i2 = (i+1) % steps;
310  int i3 = (i+2) % steps;
311 
312  int p = i * 6;
313  int c = i * 2;
314 
315  marker.points[p+0] = circle1[i1];
316  marker.points[p+1] = circle2[i2];
317  marker.points[p+2] = circle1[i2];
318 
319  marker.points[p+3] = circle1[i2];
320  marker.points[p+4] = circle2[i2];
321  marker.points[p+5] = circle1[i3];
322 
323  color.r = base_color.r * 0.6;
324  color.g = base_color.g * 0.6;
325  color.b = base_color.b * 0.6;
326 
327  marker.colors[c] = color;
328  marker.colors[c+1] = color;
329 
330  p += 6;
331  c += 2;
332 
333  marker.points[p+0] = circle2[i1];
334  marker.points[p+1] = circle2[i2];
335  marker.points[p+2] = circle1[i1];
336 
337  marker.points[p+3] = circle2[i2];
338  marker.points[p+4] = circle2[i3];
339  marker.points[p+5] = circle1[i3];
340 
341  marker.colors[c] = base_color;
342  marker.colors[c+1] = base_color;
343  }
344  break;
345  }
346 
347  default:
348  for ( int i=0; i<steps; i++ )
349  {
350  int i1 = i;
351  int i2 = (i+1) % steps;
352 
353  int p = i*6;
354 
355  marker.points[p+0] = circle1[i1];
356  marker.points[p+1] = circle2[i1];
357  marker.points[p+2] = circle1[i2];
358 
359  marker.points[p+3] = circle2[i1];
360  marker.points[p+4] = circle2[i2];
361  marker.points[p+5] = circle1[i2];
362  }
363  break;
364  }
365 
366  control.markers.push_back(marker);
367 }
368 
369 void makeViewFacingButton( const visualization_msgs::InteractiveMarker &msg,
370  visualization_msgs::InteractiveMarkerControl &control, std::string text )
371 {
372  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
373  control.independent_marker_orientation = false;
374 
375  visualization_msgs::Marker marker;
376 
377  float base_scale = 0.25 * msg.scale;
378  float base_z = 1.2 * msg.scale;
379 
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;
390  marker.text = text;
391 
392  control.markers.push_back( marker );
393 }
394 
395 
396 void assignDefaultColor(visualization_msgs::Marker &marker, const geometry_msgs::Quaternion &quat )
397 {
398  geometry_msgs::Vector3 v;
399 
400  tf2::Quaternion bt_quat( quat.x, quat.y, quat.z, quat.w );
401  tf2::Vector3 bt_x_axis = tf2::Matrix3x3(bt_quat) * tf2::Vector3(1,0,0);
402 
403  float x,y,z;
404  x = fabs(bt_x_axis.x());
405  y = fabs(bt_x_axis.y());
406  z = fabs(bt_x_axis.z());
407 
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;
411 
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;
416 }
417 
418 
419 visualization_msgs::InteractiveMarkerControl makeTitle( const visualization_msgs::InteractiveMarker &msg )
420 {
421  visualization_msgs::Marker marker;
422 
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;
433 
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 );
439 
440  autoComplete( msg, control );
441 
442  return control;
443 }
444 
445 }
interactive_markers::autoComplete
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.
Definition: tools.cpp:44
interactive_markers::makeDisc
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 ...
Definition: tools.cpp:226
tf2::Quaternion::normalize
Quaternion & normalize()
interactive_markers::uniqueifyControlNames
INTERACTIVE_MARKERS_PUBLIC void uniqueifyControlNames(visualization_msgs::InteractiveMarker &msg)
Make sure all the control names are unique within the given msg.
Definition: tools.cpp:83
Quaternion.h
interactive_markers::makeArrow
INTERACTIVE_MARKERS_PUBLIC void makeArrow(const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, float pos)
— marker helpers —
Definition: tools.cpp:198
interactive_markers::makeTitle
INTERACTIVE_MARKERS_PUBLIC visualization_msgs::InteractiveMarkerControl makeTitle(const visualization_msgs::InteractiveMarker &msg)
create a control which shows the description of the interactive marker
Definition: tools.cpp:419
interactive_markers::makeViewFacingButton
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
Definition: tools.cpp:369
tf2::Quaternion
interactive_markers
Definition: message_context.h:45
tools.h
interactive_markers::assignDefaultColor
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
Definition: tools.cpp:396
tf2::Matrix3x3
assert.h
Matrix3x3.h
t
geometry_msgs::TransformStamped t

interactive_markers
Author(s): David Gossow, William Woodall
autogenerated on Fri Oct 27 2023 02:31:54