interactive_marker_helpers.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 // author: Adam Leeper
31 
32 //#include <interactive_marker_helpers/interactive_marker_helpers.h>
33 #include <jsk_interactive_marker/interactive_marker_helpers.h>
34 
35 #include <tf/tf.h>
36 // #include <object_manipulator/tools/msg_helpers.h>
37 
38 namespace im_helpers {
39 
40 geometry_msgs::Pose createPoseMsg(const tf::Transform &transform)
41 {
42  geometry_msgs::Pose pose;
43  tf::Vector3 pos = transform.getOrigin();
44  tf::Quaternion rot = transform.getRotation();
45  pose.position.x = pos[0];
46  pose.position.y = pos[1];
47  pose.position.z = pos[2];
48  pose.orientation.x = rot.getX();
49  pose.orientation.y = rot.getY();
50  pose.orientation.z = rot.getZ();
51  pose.orientation.w = rot.getW();
52  return pose;
53 }
54 
55 visualization_msgs::InteractiveMarker makeEmptyMarker( const char *frame_id )
56 {
57  visualization_msgs::InteractiveMarker int_marker;
58  int_marker.header.frame_id = frame_id;
59  int_marker.scale = 1;
60 
61  return int_marker;
62 }
63 
64 visualization_msgs::Marker makeBox( float scale )
65 {
66  visualization_msgs::Marker marker;
67 
68  marker.type = visualization_msgs::Marker::CUBE;
69  marker.scale.x = scale;
70  marker.scale.y = scale;
71  marker.scale.z = scale;
72  marker.color.r = 1.0;
73  marker.color.g = 1.0;
74  marker.color.b = 1.0;
75  marker.color.a = 1.0;
76 
77  return marker;
78 }
79 
80 visualization_msgs::Marker makeSphere( float scale )
81 {
82  visualization_msgs::Marker marker;
83 
84  marker.type = visualization_msgs::Marker::SPHERE;
85  marker.scale.x = scale;
86  marker.scale.y = scale;
87  marker.scale.z = scale;
88  marker.color.r = 1.0;
89  marker.color.g = 1.0;
90  marker.color.b = 1.0;
91  marker.color.a = 1.0;
92 
93  return marker;
94 }
95 
96 void add3Dof2DControl( visualization_msgs::InteractiveMarker &msg, bool fixed)
97 {
98  visualization_msgs::InteractiveMarkerControl control;
99 
100  if(fixed)
101  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
102 
103  control.orientation.w = 1;
104  control.orientation.x = 1;
105  control.orientation.y = 0;
106  control.orientation.z = 0;
107  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
108  // msg.controls.push_back(control);
109  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
110  msg.controls.push_back(control);
111 
112  control.orientation.w = 1;
113  control.orientation.x = 0;
114  control.orientation.y = 1;
115  control.orientation.z = 0;
116  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
117  msg.controls.push_back(control);
118  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
119  // msg.controls.push_back(control);
120 
121  control.orientation.w = 1;
122  control.orientation.x = 0;
123  control.orientation.y = 0;
124  control.orientation.z = 1;
125  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
126  // msg.controls.push_back(control);
127  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
128  msg.controls.push_back(control);
129 
130 }
131 
132 void add6DofControl( visualization_msgs::InteractiveMarker &msg, bool fixed)
133 {
134  visualization_msgs::InteractiveMarkerControl control;
135 
136  if(fixed)
137  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
138 
139  control.orientation.w = 1;
140  control.orientation.x = 1;
141  control.orientation.y = 0;
142  control.orientation.z = 0;
143  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
144  msg.controls.push_back(control);
145  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
146  msg.controls.push_back(control);
147 
148  control.orientation.w = 1;
149  control.orientation.x = 0;
150  control.orientation.y = 1;
151  control.orientation.z = 0;
152  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
153  msg.controls.push_back(control);
154  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
155  msg.controls.push_back(control);
156 
157  control.orientation.w = 1;
158  control.orientation.x = 0;
159  control.orientation.y = 0;
160  control.orientation.z = 1;
161  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
162  msg.controls.push_back(control);
163  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
164  msg.controls.push_back(control);
165 }
166 
167  void addVisible6DofControl( visualization_msgs::InteractiveMarker &msg, bool fixed, bool visible)
168  {
169  visualization_msgs::InteractiveMarkerControl control;
170 
171  if(visible)
172  control.always_visible = true;
173 
174  if(fixed)
175  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
176 
177  control.orientation.w = 1;
178  control.orientation.x = 1;
179  control.orientation.y = 0;
180  control.orientation.z = 0;
181  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
182  msg.controls.push_back(control);
183  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
184  msg.controls.push_back(control);
185 
186  control.orientation.w = 1;
187  control.orientation.x = 0;
188  control.orientation.y = 1;
189  control.orientation.z = 0;
190  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
191  msg.controls.push_back(control);
192  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
193  msg.controls.push_back(control);
194 
195  control.orientation.w = 1;
196  control.orientation.x = 0;
197  control.orientation.y = 0;
198  control.orientation.z = 1;
199  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
200  msg.controls.push_back(control);
201  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
202  msg.controls.push_back(control);
203 }
204 
205 
206 visualization_msgs::InteractiveMarkerControl& makeBoxControl( visualization_msgs::InteractiveMarker &msg )
207 {
208  visualization_msgs::InteractiveMarkerControl control;
209  control.always_visible = true;
210  control.markers.push_back( makeBox(msg.scale) );
211  msg.controls.push_back( control );
212 
213  return msg.controls.back();
214 }
215 
216 visualization_msgs::InteractiveMarkerControl& makeSphereControl( visualization_msgs::InteractiveMarker &msg )
217 {
218  visualization_msgs::InteractiveMarkerControl control;
219  control.always_visible = true;
220  control.markers.push_back( makeSphere(msg.scale) );
221  msg.controls.push_back( control );
222 
223  return msg.controls.back();
224 }
225 
226 visualization_msgs::MenuEntry makeMenuEntry(const char *title)
227 {
228  visualization_msgs::MenuEntry m;
229  m.title = title;
230  m.command = title;
231  return m;
232 }
233 
234 visualization_msgs::MenuEntry makeMenuEntry(const char *title, const char *command, int type )
235 {
236  visualization_msgs::MenuEntry m;
237  m.title = title;
238  m.command = command;
239  m.command_type = type;
240  return m;
241 }
242 
243 visualization_msgs::InteractiveMarker makePostureMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
244  float scale, bool fixed, bool view_facing )
245 {
246  visualization_msgs::InteractiveMarker int_marker;
247  int_marker.header = stamped.header;
248  int_marker.name = name;
249  int_marker.scale = scale;
250  int_marker.pose = stamped.pose;
251 
252  visualization_msgs::InteractiveMarkerControl control;
253  control.orientation.w = 1;
254  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
255  int_marker.controls.push_back(control);
256 
257  return int_marker;
258 }
259 
260 visualization_msgs::InteractiveMarker makeHeadGoalMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
261  float scale)
262 {
263  visualization_msgs::InteractiveMarker int_marker;
264  int_marker.header = stamped.header;
265  int_marker.name = name;
266  int_marker.scale = scale;
267  int_marker.pose = stamped.pose;
268 
269  visualization_msgs::InteractiveMarkerControl control;
270  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
271  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
272  control.orientation.w = 1;
273  control.markers.push_back( makeSphere(scale*0.7) );
274  int_marker.controls.push_back(control);
275  control.markers.clear();
276 
277  add6DofControl(int_marker);
278 
279  return int_marker;
280 }
281 
282 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
283  const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color, bool use_color )
284 {
285  visualization_msgs::InteractiveMarker int_marker;
286  int_marker.header = stamped.header;
287  int_marker.pose = stamped.pose;
288  int_marker.name = name;
289  int_marker.scale = scale;
290 
291  visualization_msgs::Marker mesh;
292  if (use_color) mesh.color = color;
293  mesh.mesh_resource = mesh_resource;
294  mesh.mesh_use_embedded_materials = !use_color;
295  mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
296  mesh.scale.x = scale;
297  mesh.scale.y = scale;
298  mesh.scale.z = scale;
299 
300  visualization_msgs::InteractiveMarkerControl control;
301  control.markers.push_back( mesh );
302  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
303  int_marker.controls.push_back( control );
304 
305  return int_marker;
306 }
307 
308 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
309  const geometry_msgs::PoseStamped &stamped, float scale)
310 {
311  std_msgs::ColorRGBA color;
312  return makeMeshMarker( name, mesh_resource, stamped, scale, color, false);
313 }
314 
315 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
316  const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color)
317 {
318  return makeMeshMarker( name, mesh_resource, stamped, scale, color, true);
319 }
320 
321 visualization_msgs::InteractiveMarker makeButtonBox( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing )
322 {
323  visualization_msgs::InteractiveMarker int_marker;
324  int_marker.header = stamped.header;
325  int_marker.name = name;
326  int_marker.scale = scale;
327  int_marker.pose = stamped.pose;
328  //int_marker.description = "This is the marker.";
329 
330  visualization_msgs::InteractiveMarkerControl &control = makeBoxControl(int_marker);
331  //control.description = "This is the control";
332  control.always_visible = false;
333  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
334 
335  return int_marker;
336 }
337 
338 visualization_msgs::InteractiveMarker makeButtonSphere( const char *name, const geometry_msgs::PoseStamped &stamped,
339  float scale, bool fixed, bool view_facing)
340 {
341  std_msgs::ColorRGBA color;
342  return makeButtonSphere(name, stamped, scale, fixed, view_facing, color);
343 }
344 
345 visualization_msgs::InteractiveMarker makeButtonSphere( const char *name, const geometry_msgs::PoseStamped &stamped,
346  float scale, bool fixed, bool view_facing,
347  std_msgs::ColorRGBA color)
348 {
349  visualization_msgs::InteractiveMarker int_marker;
350  int_marker.header = stamped.header;
351  int_marker.name = name;
352  int_marker.scale = scale;
353  int_marker.pose = stamped.pose;
354  //int_marker.description = "This is the marker.";
355 
356  visualization_msgs::InteractiveMarkerControl &control = makeSphereControl(int_marker);
357  //control.description = "This is the control";
358  control.always_visible = false;
359  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
360  control.markers.back().color = color;
361 
362  return int_marker;
363 }
364 
365 visualization_msgs::InteractiveMarker makeListControl( const char *name, const geometry_msgs::PoseStamped &stamped, int num, int total, float scale)
366 {
367 
368  visualization_msgs::InteractiveMarker int_marker;
369  int_marker.header = stamped.header;
370  int_marker.name = name;
371  int_marker.scale = scale;
372  int_marker.pose = stamped.pose;
373 
374  visualization_msgs::InteractiveMarkerControl control;
375 
376 // control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
377  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
378  std::stringstream ss;
379  ss << "pose(" << num << "/" << total << ")";
380  interactive_markers::makeViewFacingButton(int_marker, control, ss.str());
381 
382 // control.orientation.w = 1;
383 // control.orientation.x = 0;
384 // control.orientation.y = -1;
385 // control.orientation.z = 0;
386 
387 // interactive_markers::makeArrow( int_marker, control, 0.3*scale );
388 // control.markers.back().color.r = 1.0;
389 // control.markers.back().color.g = 0.0;
390 // control.markers.back().color.b = 0.0;
391 // control.name = "right";
392 // int_marker.controls.push_back(control);
393 //
394 // control.markers.clear();
395 // interactive_markers::makeArrow( int_marker, control, -0.3*scale );
396 // control.markers.back().color.r = 0.0;
397 // control.markers.back().color.g = 1.0;
398 // control.markers.back().color.b = 0.0;
399 // control.name = "left";
400  int_marker.controls.push_back(control);
401 
402  return int_marker;
403 
404 
405 // visualization_msgs::InteractiveMarker int_marker;
406 // int_marker.header = stamped.header;
407 // int_marker.name = name;
408 // int_marker.scale = scale;
409 // int_marker.pose = stamped.pose;
410 // //int_marker.description = "This is the marker.";
411 //
412 // visualization_msgs::InteractiveMarkerControl &control = makeSphereControl(int_marker);
413 // //control.description = "This is the control";
414 // control.always_visible = false;
415 // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
416 //
417 // return int_marker;
418 }
419 
420 visualization_msgs::InteractiveMarker make6DofMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing )
421 {
422  visualization_msgs::InteractiveMarker int_marker;
423  int_marker.header = stamped.header;
424  int_marker.name = name;
425  int_marker.scale = scale;
426  int_marker.pose = stamped.pose;
427 
428  if ( view_facing )
429  {
430  visualization_msgs::InteractiveMarkerControl control;
431  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
432  //control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE;
433  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
434  control.orientation.w = 1;
435  int_marker.controls.push_back(control);
436 
437  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
438  control.markers.push_back( makeSphere(scale*0.5) );
439  int_marker.controls.push_back(control);
440  }
441  else
442  {
443  add6DofControl(int_marker, fixed);
444  }
445 
446  return int_marker;
447 }
448 
449 visualization_msgs::InteractiveMarker makePlanarMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed )
450 {
451  visualization_msgs::InteractiveMarker int_marker;
452  int_marker.header = stamped.header;
453  int_marker.name = name;
454  int_marker.scale = scale;
455  int_marker.pose = stamped.pose;
456 
457  visualization_msgs::InteractiveMarkerControl control;
458  control.orientation_mode = fixed ? (uint8_t)visualization_msgs::InteractiveMarkerControl::FIXED : (uint8_t)visualization_msgs::InteractiveMarkerControl::INHERIT;
459  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE;
460  control.orientation.w = 1;
461  control.orientation.x = 0;
462  control.orientation.y = 1;
463  control.orientation.z = 0;
464  int_marker.controls.push_back(control);
465 
466  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
467  interactive_markers::makeArrow( int_marker, control, 0 );
468  control.markers.back().pose.orientation.w = 0;
469  control.markers.back().pose.orientation.x = 0;
470  control.markers.back().pose.orientation.y = 1;
471  control.markers.back().pose.orientation.z = 0;
472  control.markers.back().color.r = 0.0;
473  control.markers.back().color.g = 1.0;
474  control.markers.back().color.b = 0.0;
475  int_marker.controls.push_back(control);
476 
477  return int_marker;
478 }
479 
480 visualization_msgs::InteractiveMarker makeElevatorMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
481 {
482  visualization_msgs::InteractiveMarker int_marker;
483  int_marker.header = stamped.header;
484  int_marker.name = name;
485  int_marker.scale = scale;
486  int_marker.pose = stamped.pose;
487 
488  visualization_msgs::InteractiveMarkerControl control;
489 
490  if(fixed)
491  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
492 
493  control.orientation.w = 1;
494  control.orientation.x = 0;
495  control.orientation.y = -1;
496  control.orientation.z = 0;
497  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
498 
499  interactive_markers::makeArrow( int_marker, control, 0.3 );
500  control.markers.back().color.r = 0.0;
501  control.markers.back().color.g = 1.0;
502  control.markers.back().color.b = 0.0;
503  control.name = "up";
504  int_marker.controls.push_back(control);
505 
506  control.markers.clear();
507  interactive_markers::makeArrow( int_marker, control, -0.3 );
508  control.markers.back().color.r = 1.0;
509  control.markers.back().color.g = 0.0;
510  control.markers.back().color.b = 0.0;
511  control.name = "down";
512  int_marker.controls.push_back(control);
513 
514  return int_marker;
515 }
516 
517 visualization_msgs::InteractiveMarker makeProjectorMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale)
518 {
519  visualization_msgs::InteractiveMarker int_marker;
520  int_marker.header = stamped.header;
521  int_marker.name = name;
522  int_marker.scale = scale;
523  int_marker.pose = stamped.pose;
524 
525  visualization_msgs::InteractiveMarkerControl control;
526  control.orientation.w = 1;
527  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
528  visualization_msgs::Marker marker;
529  marker.type = visualization_msgs::Marker::CYLINDER;
530  marker.scale.x = 0.03;
531  marker.scale.y = 0.03;
532  marker.scale.z = 0.04;
533  marker.color.r = 1.0;
534  marker.color.a = 0.8;
535  control.markers.push_back(marker);
536  int_marker.controls.push_back(control);
537 
538  return int_marker;
539 }
540 
541 
542 visualization_msgs::InteractiveMarker makeBaseMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
543 {
544  visualization_msgs::InteractiveMarker int_marker;
545  int_marker.header = stamped.header;
546  int_marker.name = name;
547  int_marker.scale = scale;
548  int_marker.pose = stamped.pose;
549 
550  visualization_msgs::InteractiveMarkerControl control;
551 
552  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
553  control.orientation.w = 1;
554  control.orientation.y = -1;
555  int_marker.controls.push_back(control);
556 
557 
558  if(fixed)
559  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
560 
561  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
562  control.orientation.w = 1;
563  control.orientation.y = 0;
564 
565  control.markers.clear();
566  interactive_markers::makeArrow( int_marker, control, 0.9 );
567  control.markers.back().color.r = 1.0;
568  control.markers.back().color.g = 0.0;
569  control.markers.back().color.b = 0.0;
570  control.name = "forward";
571  int_marker.controls.push_back(control);
572 
573  control.markers.clear();
574  interactive_markers::makeArrow( int_marker, control, -0.9 );
575  control.markers.back().color.r = 1.0;
576  control.markers.back().color.g = 0.0;
577  control.markers.back().color.b = 0.0;
578  control.name = "back";
579  int_marker.controls.push_back(control);
580 
581  control.orientation.z = 1;
582  control.markers.clear();
583  interactive_markers::makeArrow( int_marker, control, 0.9 );
584  control.markers.back().color.r = 1.0;
585  control.markers.back().color.g = 0.0;
586  control.markers.back().color.b = 0.0;
587  control.name = "left";
588  int_marker.controls.push_back(control);
589 
590  control.markers.clear();
591  interactive_markers::makeArrow( int_marker, control, -0.9 );
592  control.markers.back().color.r = 1.0;
593  control.markers.back().color.g = 0.0;
594  control.markers.back().color.b = 0.0;
595  control.name = "right";
596  int_marker.controls.push_back(control);
597 
598  control.markers.clear();
599  tf::quaternionTFToMsg( tf::Quaternion(tf::Vector3(0,0,1), 135*M_PI/180.0), control.orientation);
600  interactive_markers::makeArrow( int_marker, control, 1.0 );
601  control.markers.back().pose.position.x = 0.7;
602  control.markers.back().color.r = 1.0;
603  control.markers.back().color.g = 1.0;
604  control.markers.back().color.b = 0.0;
605  control.name = "rotate left";
606  int_marker.controls.push_back(control);
607 
608  control.markers.clear();
609  tf::quaternionTFToMsg( tf::Quaternion(tf::Vector3(0,0,1), -135*M_PI/180.0), control.orientation);
610  interactive_markers::makeArrow( int_marker, control, 1.0 );
611  control.markers.back().pose.position.x = 0.7;
612  control.markers.back().color.r = 1.0;
613  control.markers.back().color.g = 1.0;
614  control.markers.back().color.b = 0.0;
615  control.name = "rotate right";
616  int_marker.controls.push_back(control);
617  return int_marker;
618 }
619 
620 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
621  float scale, float angle, bool view_facing)
622 {
623  std_msgs::ColorRGBA color;
624  return makeGripperMarker( name, stamped, scale, angle, view_facing, color, false);
625 }
626 
627 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
628  float scale, float angle, bool view_facing, std_msgs::ColorRGBA color )
629 {
630  return makeGripperMarker( name, stamped, scale, angle, view_facing, color, true);
631 }
632 
633 
634 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
635  float scale, float angle, bool view_facing, std_msgs::ColorRGBA color, bool use_color )
636 {
637  visualization_msgs::InteractiveMarker int_marker;
638  int_marker.header = stamped.header;
639  int_marker.name = name;
640  int_marker.scale = 1.0; //scale;
641  int_marker.pose = stamped.pose;
642 
643 // add6DofControl(int_marker, false);
644 
645  visualization_msgs::InteractiveMarkerControl control;
646 
647  visualization_msgs::Marker mesh;
648  mesh.mesh_use_embedded_materials = !use_color;
649  mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
650  mesh.scale.x = scale;
651  mesh.scale.y = scale;
652  mesh.scale.z = scale;
653  mesh.color = color;
654 
655  tf::Transform T1, T2;
656  tf::Transform T_proximal, T_distal;
657 
658  T1.setOrigin(tf::Vector3(0.07691, 0.01, 0));
659  T1.setRotation(tf::Quaternion(tf::Vector3(0,0,1), angle));
660  T2.setOrigin(tf::Vector3(0.09137, 0.00495, 0));
661  T2.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -angle));
662  T_proximal = T1;
663  T_distal = T1 * T2;
664 
665  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae";
666  mesh.pose.orientation.w = 1;
667  control.markers.push_back( mesh );
668  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae";
669  mesh.pose = createPoseMsg(T_proximal);
670  control.markers.push_back( mesh );
671  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae";
672  mesh.pose = createPoseMsg(T_distal);
673  control.markers.push_back( mesh );
674 
675  T1.setOrigin(tf::Vector3(0.07691, -0.01, 0));
676  T1.setRotation(tf::Quaternion(tf::Vector3(1,0,0), M_PI)*tf::Quaternion(tf::Vector3(0,0,1), angle));
677  T2.setOrigin(tf::Vector3(0.09137, 0.00495, 0));
678  T2.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -angle));
679  T_proximal = T1;
680  T_distal = T1 * T2;
681 
682  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae";
683  mesh.pose = createPoseMsg(T_proximal);
684  control.markers.push_back( mesh );
685  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae";
686  mesh.pose = createPoseMsg(T_distal);
687  control.markers.push_back( mesh );
688 
689  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
690  control.always_visible = true;
691  int_marker.controls.push_back( control );
692 
693  return int_marker;
694 }
695 
696 
697 visualization_msgs::InteractiveMarker makeGraspMarker( const char * name, const geometry_msgs::PoseStamped &stamped, float scale, PoseState pose_state)
698 {
699  visualization_msgs::InteractiveMarker int_marker;
700  int_marker.header = stamped.header;
701  int_marker.header.stamp = ros::Time(0);
702  int_marker.name = name;
703  int_marker.scale = scale;
704  int_marker.pose = stamped.pose;
705 
706  visualization_msgs::InteractiveMarkerControl control;
707  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
708  control.orientation.w = 1;
709  visualization_msgs::Marker marker;
710  marker.type = visualization_msgs::Marker::ARROW;
711  //marker.header = int_marker.header;
712  //marker.pose = int_marker.pose;
713  marker.scale.x = 0.025*scale;
714  marker.scale.y = 0.0025*scale;
715  marker.scale.z = 0.0025*scale;
716  marker.color.r = 1.0;
717  marker.color.a = 1.0;
718  control.markers.push_back(marker);
719  marker.type = visualization_msgs::Marker::CUBE;
720  marker.scale.x = 0.015*scale;
721  marker.scale.y = 0.04*scale;
722  marker.scale.z = 0.015*scale;
723  switch (pose_state)
724  {
725  case UNTESTED:
726  marker.color.g = 0.5;
727  marker.color.b = 0.5;
728  marker.color.r = 0.5;
729  break;
730  case VALID:
731  marker.color.g = 1.0;
732  marker.color.r = 0.0;
733  break;
734  case INVALID:
735  marker.color.g = 0.0;
736  marker.color.r = 1.0;
737  break;
738  }
739  control.markers.push_back(marker);
740 // marker.points[0] = object_manipulator::msg::createPointMsg(0,0,0);
741 // marker.points[1] = object_manipulator::msg::createPointMsg(0,0.03,0);
742 // marker.color.r = 0;
743 // marker.color.g = 1.0;
744 // control.markers.push_back(marker);
745 
746  int_marker.controls.push_back(control);
747 
748  return int_marker;
749 }
750 
751 
752 visualization_msgs::InteractiveMarker makePosedMultiMeshMarker( const char * name, const geometry_msgs::PoseStamped &stamped,
753  const std::vector< geometry_msgs::PoseStamped> &mesh_poses,
754  const std::vector<std::string> &mesh_paths, const float &scale,
755  const bool button_only)
756 {
757  //ROS_ERROR("Start create robot marker...");
758  visualization_msgs::InteractiveMarker int_marker;
759  int_marker.header = stamped.header;
760  int_marker.name = name;
761  int_marker.scale = scale;
762  int_marker.pose = stamped.pose;
763 
764  bool fixed = false;
765  visualization_msgs::InteractiveMarkerControl control;
766  if( !button_only )
767  {
768  control.orientation_mode = fixed ? (uint8_t)visualization_msgs::InteractiveMarkerControl::FIXED : (uint8_t)visualization_msgs::InteractiveMarkerControl::INHERIT;
769  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
770  control.orientation.w = 1;
771  control.orientation.x = 0;
772  control.orientation.y = 1;
773  control.orientation.z = 0;
774  int_marker.controls.push_back(control);
775  }
776 
777  control.markers.clear();
778  if (button_only) {
779  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
780  }
781  else {
782  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
783  }
784  control.always_visible = true;
785  for(size_t i = 0; i < mesh_poses.size(); i++)
786  {
787  visualization_msgs::Marker mesh;
788  mesh.color = std_msgs::ColorRGBA();
789  mesh.mesh_resource = mesh_paths[i];
790  mesh.mesh_use_embedded_materials = true;
791  mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
792  mesh.scale.x = mesh.scale.y = mesh.scale.z = scale;
793  mesh.pose = mesh_poses[i].pose;
794  control.markers.push_back( mesh );
795  }
796  int_marker.controls.push_back( control );
797 
798  return int_marker;
799 
800 }
801 
802 visualization_msgs::InteractiveMarker makeFollowerMultiMeshMarker( const char * name, const geometry_msgs::PoseStamped &stamped,
803  const std::vector<std::string> &mesh_frames,
804  const std::vector<std::string> &mesh_paths,
805  const float &scale)
806 {
807  visualization_msgs::InteractiveMarker int_marker;
808  int_marker.header = stamped.header;
809  int_marker.name = name;
810  int_marker.scale = scale;
811  int_marker.pose = stamped.pose;
812 
813  if(mesh_frames.size() != mesh_paths.size())
814  {
815  ROS_ERROR("The number of mesh frames and mesh paths is not equal!");
816  return int_marker;
817  }
818 
819  visualization_msgs::InteractiveMarkerControl control;
820 
821  control.markers.clear();
822  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
823  control.always_visible = true;
824  for(size_t i = 0; i < mesh_frames.size(); i++)
825  {
826  visualization_msgs::Marker mesh;
827  mesh.color = std_msgs::ColorRGBA();
828  mesh.mesh_resource = mesh_paths[i];
829  mesh.mesh_use_embedded_materials = true;
830  mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
831  mesh.scale.x = mesh.scale.y = mesh.scale.z = scale;
832  mesh.pose.orientation.w = 1;
833  mesh.header.frame_id = mesh_frames[i];
834  control.markers.push_back( mesh );
835  mesh.frame_locked = true;
836  }
837  int_marker.controls.push_back( control );
838 
839  return int_marker;
840 }
841 
842 } // namespace im_helpers
im_helpers::makeMeshMarker
visualization_msgs::InteractiveMarker makeMeshMarker(const std::string &name, const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, float scale)
Definition: interactive_marker_helpers.cpp:308
tf::Transform::getRotation
Quaternion getRotation() const
rot
rot
im_helpers::createPoseMsg
geometry_msgs::Pose createPoseMsg(const tf::Transform &transform)
Definition: interactive_marker_helpers.cpp:40
im_helpers::PoseState
PoseState
Definition: interactive_marker_helpers.h:46
msg
msg
mesh_resource
mesh_resource
scale
scale
marker
marker
pos
pos
im_helpers::makePosedMultiMeshMarker
visualization_msgs::InteractiveMarker makePosedMultiMeshMarker(const char *name, const geometry_msgs::PoseStamped &stamped, const std::vector< geometry_msgs::PoseStamped > &mesh_poses, const std::vector< std::string > &mesh_paths, const float &scale, const bool button_only=true)
Definition: interactive_marker_helpers.cpp:752
M_PI
#define M_PI
im_helpers::makeBox
visualization_msgs::Marker makeBox(float scale)
Definition: interactive_marker_helpers.cpp:64
angle
GLfloat angle
command
ROSLIB_DECL std::string command(const std::string &cmd)
im_helpers::makeSphere
visualization_msgs::Marker makeSphere(float scale)
Definition: interactive_marker_helpers.cpp:80
im_helpers::makeGripperMarker
visualization_msgs::InteractiveMarker makeGripperMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, float angle, bool view_facing)
Definition: interactive_marker_helpers.cpp:620
im_helpers::add3Dof2DControl
void add3Dof2DControl(visualization_msgs::InteractiveMarker &msg, bool fixed=false)
Definition: interactive_marker_helpers.cpp:96
im_helpers::makeBoxControl
visualization_msgs::InteractiveMarkerControl & makeBoxControl(visualization_msgs::InteractiveMarker &msg)
Definition: interactive_marker_helpers.cpp:206
im_helpers::makePlanarMarker
visualization_msgs::InteractiveMarker makePlanarMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
Definition: interactive_marker_helpers.cpp:449
im_helpers::makeFollowerMultiMeshMarker
visualization_msgs::InteractiveMarker makeFollowerMultiMeshMarker(const char *name, const geometry_msgs::PoseStamped &stamped, const std::vector< std::string > &mesh_frames, const std::vector< std::string > &mesh_paths, const float &scale)
Definition: interactive_marker_helpers.cpp:802
im_helpers::makeProjectorMarker
visualization_msgs::InteractiveMarker makeProjectorMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale)
Definition: interactive_marker_helpers.cpp:517
im_helpers::VALID
@ VALID
Definition: interactive_marker_helpers.h:46
im_helpers::add6DofControl
void add6DofControl(visualization_msgs::InteractiveMarker &msg, bool fixed=false)
Definition: interactive_marker_helpers.cpp:132
im_helpers::addVisible6DofControl
void addVisible6DofControl(visualization_msgs::InteractiveMarker &msg, bool fixed=false, bool visible=true)
Definition: interactive_marker_helpers.cpp:167
pose
pose
im_helpers
Definition: interactive_marker_helpers.h:44
im_helpers::makeBaseMarker
visualization_msgs::InteractiveMarker makeBaseMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
Definition: interactive_marker_helpers.cpp:542
tf::Transform::setRotation
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
im_helpers::makeButtonBox
visualization_msgs::InteractiveMarker makeButtonBox(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing)
Definition: interactive_marker_helpers.cpp:321
im_helpers::makeListControl
visualization_msgs::InteractiveMarker makeListControl(const char *name, const geometry_msgs::PoseStamped &stamped, int num, int total, float scale)
Definition: interactive_marker_helpers.cpp:365
tf::Transform
im_helpers::makeElevatorMarker
visualization_msgs::InteractiveMarker makeElevatorMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
Definition: interactive_marker_helpers.cpp:480
interactive_markers::makeArrow
INTERACTIVE_MARKERS_PUBLIC void makeArrow(const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, float pos)
visible
void visible(int state)
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
im_helpers::UNTESTED
@ UNTESTED
Definition: interactive_marker_helpers.h:46
im_helpers::makeGraspMarker
visualization_msgs::InteractiveMarker makeGraspMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, PoseState pose_state)
Definition: interactive_marker_helpers.cpp:697
im_helpers::makeMenuEntry
visualization_msgs::MenuEntry makeMenuEntry(const char *title)
Definition: interactive_marker_helpers.cpp:226
num
num
im_helpers::make6DofMarker
visualization_msgs::InteractiveMarker make6DofMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing)
Definition: interactive_marker_helpers.cpp:420
tf.h
dummy_camera.frame_id
frame_id
Definition: dummy_camera.py:10
ros::Time
interactive_marker_helpers.h
interactive_markers::makeViewFacingButton
INTERACTIVE_MARKERS_PUBLIC void makeViewFacingButton(const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, std::string text)
tf::quaternionTFToMsg
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
ROS_ERROR
#define ROS_ERROR(...)
im_helpers::makeHeadGoalMarker
visualization_msgs::InteractiveMarker makeHeadGoalMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale)
Definition: interactive_marker_helpers.cpp:260
im_helpers::makeSphereControl
visualization_msgs::InteractiveMarkerControl & makeSphereControl(visualization_msgs::InteractiveMarker &msg)
Definition: interactive_marker_helpers.cpp:216
im_helpers::makeEmptyMarker
visualization_msgs::InteractiveMarker makeEmptyMarker(const char *frame_id="")
Definition: interactive_marker_helpers.cpp:55
im_helpers::makePostureMarker
visualization_msgs::InteractiveMarker makePostureMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing)
Definition: interactive_marker_helpers.cpp:243
title
title
type
type
tf::Quaternion
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
im_helpers::INVALID
@ INVALID
Definition: interactive_marker_helpers.h:46
im_helpers::makeButtonSphere
visualization_msgs::InteractiveMarker makeButtonSphere(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing)
Definition: interactive_marker_helpers.cpp:338

jsk_interactive_marker
Author(s): furuta
autogenerated on Fri Aug 2 2024 08:50:24