basic_controls.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4 Copyright (c) 2011, Willow Garage, Inc.
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10  * Redistributions of source code must retain the above copyright
11  notice, this list of conditions and the following disclaimer.
12  * Redistributions in binary form must reproduce the above copyright
13  notice, this list of conditions and the following disclaimer in the
14  documentation and/or other materials provided with the distribution.
15  * Neither the name of the Willow Garage, Inc. nor the names of its
16  contributors may be used to endorse or promote products derived from
17  this software without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS
26 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 POSSIBILITY OF SUCH DAMAGE.
30 """
31 
32 import rospy
33 import copy
34 
37 from visualization_msgs.msg import *
38 from geometry_msgs.msg import Point
39 from tf.broadcaster import TransformBroadcaster
40 
41 from random import random
42 from math import sin
43 
44 server = None
45 menu_handler = MenuHandler()
46 br = None
47 counter = 0
48 
49 def frameCallback( msg ):
50  global counter, br
51  time = rospy.Time.now()
52  br.sendTransform( (0, 0, sin(counter/140.0)*2.0), (0, 0, 0, 1.0), time, "base_link", "moving_frame" )
53  counter += 1
54 
55 def processFeedback( feedback ):
56  s = "Feedback from marker '" + feedback.marker_name
57  s += "' / control '" + feedback.control_name + "'"
58 
59  mp = ""
60  if feedback.mouse_point_valid:
61  mp = " at " + str(feedback.mouse_point.x)
62  mp += ", " + str(feedback.mouse_point.y)
63  mp += ", " + str(feedback.mouse_point.z)
64  mp += " in frame " + feedback.header.frame_id
65 
66  if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
67  rospy.loginfo( s + ": button click" + mp + "." )
68  elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
69  rospy.loginfo( s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + "." )
70  elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
71  rospy.loginfo( s + ": pose changed")
72 # TODO
73 # << "\nposition = "
74 # << feedback.pose.position.x
75 # << ", " << feedback.pose.position.y
76 # << ", " << feedback.pose.position.z
77 # << "\norientation = "
78 # << feedback.pose.orientation.w
79 # << ", " << feedback.pose.orientation.x
80 # << ", " << feedback.pose.orientation.y
81 # << ", " << feedback.pose.orientation.z
82 # << "\nframe: " << feedback.header.frame_id
83 # << " time: " << feedback.header.stamp.sec << "sec, "
84 # << feedback.header.stamp.nsec << " nsec" )
85  elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
86  rospy.loginfo( s + ": mouse down" + mp + "." )
87  elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
88  rospy.loginfo( s + ": mouse up" + mp + "." )
89  server.applyChanges()
90 
91 def alignMarker( feedback ):
92  pose = feedback.pose
93 
94  pose.position.x = round(pose.position.x-0.5)+0.5
95  pose.position.y = round(pose.position.y-0.5)+0.5
96 
97  rospy.loginfo( feedback.marker_name + ": aligning position = " + str(feedback.pose.position.x) + "," + str(feedback.pose.position.y) + "," + str(feedback.pose.position.z) + " to " +
98  str(pose.position.x) + "," + str(pose.position.y) + "," + str(pose.position.z) )
99 
100  server.setPose( feedback.marker_name, pose )
101  server.applyChanges()
102 
103 def rand( min_, max_ ):
104  return min_ + random()*(max_-min_)
105 
106 def makeBox( msg ):
107  marker = Marker()
108 
109  marker.type = Marker.CUBE
110  marker.scale.x = msg.scale * 0.45
111  marker.scale.y = msg.scale * 0.45
112  marker.scale.z = msg.scale * 0.45
113  marker.color.r = 0.5
114  marker.color.g = 0.5
115  marker.color.b = 0.5
116  marker.color.a = 1.0
117 
118  return marker
119 
120 def makeBoxControl( msg ):
121  control = InteractiveMarkerControl()
122  control.always_visible = True
123  control.markers.append( makeBox(msg) )
124  msg.controls.append( control )
125  return control
126 
127 def saveMarker( int_marker ):
128  server.insert(int_marker, processFeedback)
129 
130 
131 #####################################################################
132 # Marker Creation
133 
134 def normalizeQuaternion( quaternion_msg ):
135  norm = quaternion_msg.x**2 + quaternion_msg.y**2 + quaternion_msg.z**2 + quaternion_msg.w**2
136  s = norm**(-0.5)
137  quaternion_msg.x *= s
138  quaternion_msg.y *= s
139  quaternion_msg.z *= s
140  quaternion_msg.w *= s
141 
142 def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
143  int_marker = InteractiveMarker()
144  int_marker.header.frame_id = "base_link"
145  int_marker.pose.position = position
146  int_marker.scale = 1
147 
148  int_marker.name = "simple_6dof"
149  int_marker.description = "Simple 6-DOF Control"
150 
151  # insert a box
152  makeBoxControl(int_marker)
153  int_marker.controls[0].interaction_mode = interaction_mode
154 
155  if fixed:
156  int_marker.name += "_fixed"
157  int_marker.description += "\n(fixed orientation)"
158 
159  if interaction_mode != InteractiveMarkerControl.NONE:
160  control_modes_dict = {
161  InteractiveMarkerControl.MOVE_3D : "MOVE_3D",
162  InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D",
163  InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" }
164  int_marker.name += "_" + control_modes_dict[interaction_mode]
165  int_marker.description = "3D Control"
166  if show_6dof:
167  int_marker.description += " + 6-DOF controls"
168  int_marker.description += "\n" + control_modes_dict[interaction_mode]
169 
170  if show_6dof:
171  control = InteractiveMarkerControl()
172  control.orientation.w = 1
173  control.orientation.x = 1
174  control.orientation.y = 0
175  control.orientation.z = 0
176  normalizeQuaternion(control.orientation)
177  control.name = "rotate_x"
178  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
179  if fixed:
180  control.orientation_mode = InteractiveMarkerControl.FIXED
181  int_marker.controls.append(control)
182 
183  control = InteractiveMarkerControl()
184  control.orientation.w = 1
185  control.orientation.x = 1
186  control.orientation.y = 0
187  control.orientation.z = 0
188  normalizeQuaternion(control.orientation)
189  control.name = "move_x"
190  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
191  if fixed:
192  control.orientation_mode = InteractiveMarkerControl.FIXED
193  int_marker.controls.append(control)
194 
195  control = InteractiveMarkerControl()
196  control.orientation.w = 1
197  control.orientation.x = 0
198  control.orientation.y = 1
199  control.orientation.z = 0
200  normalizeQuaternion(control.orientation)
201  control.name = "rotate_z"
202  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
203  if fixed:
204  control.orientation_mode = InteractiveMarkerControl.FIXED
205  int_marker.controls.append(control)
206 
207  control = InteractiveMarkerControl()
208  control.orientation.w = 1
209  control.orientation.x = 0
210  control.orientation.y = 1
211  control.orientation.z = 0
212  normalizeQuaternion(control.orientation)
213  control.name = "move_z"
214  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
215  if fixed:
216  control.orientation_mode = InteractiveMarkerControl.FIXED
217  int_marker.controls.append(control)
218 
219  control = InteractiveMarkerControl()
220  control.orientation.w = 1
221  control.orientation.x = 0
222  control.orientation.y = 0
223  control.orientation.z = 1
224  normalizeQuaternion(control.orientation)
225  control.name = "rotate_y"
226  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
227  if fixed:
228  control.orientation_mode = InteractiveMarkerControl.FIXED
229  int_marker.controls.append(control)
230 
231  control = InteractiveMarkerControl()
232  control.orientation.w = 1
233  control.orientation.x = 0
234  control.orientation.y = 0
235  control.orientation.z = 1
236  normalizeQuaternion(control.orientation)
237  control.name = "move_y"
238  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
239  if fixed:
240  control.orientation_mode = InteractiveMarkerControl.FIXED
241  int_marker.controls.append(control)
242 
243  server.insert(int_marker, processFeedback)
244  menu_handler.apply( server, int_marker.name )
245 
246 def makeRandomDofMarker( position ):
247  int_marker = InteractiveMarker()
248  int_marker.header.frame_id = "base_link"
249  int_marker.pose.position = position
250  int_marker.scale = 1
251 
252  int_marker.name = "6dof_random_axes"
253  int_marker.description = "6-DOF\n(Arbitrary Axes)"
254 
255  makeBoxControl(int_marker)
256 
257  control = InteractiveMarkerControl()
258 
259  for i in range(3):
260  control.orientation.w = rand(-1,1)
261  control.orientation.x = rand(-1,1)
262  control.orientation.y = rand(-1,1)
263  control.orientation.z = rand(-1,1)
264  normalizeQuaternion(control.orientation)
265  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
266  int_marker.controls.append(copy.deepcopy(control))
267  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
268  int_marker.controls.append(copy.deepcopy(control))
269 
270  server.insert(int_marker, processFeedback)
271 
272 def makeViewFacingMarker(position):
273  int_marker = InteractiveMarker()
274  int_marker.header.frame_id = "base_link"
275  int_marker.pose.position = position
276  int_marker.scale = 1
277 
278  int_marker.name = "view_facing"
279  int_marker.description = "View Facing 6-DOF"
280 
281  # make a control that rotates around the view axis
282  control = InteractiveMarkerControl()
283  control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
284  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
285  control.orientation.w = 1
286  control.name = "rotate"
287  int_marker.controls.append(control)
288 
289  # create a box in the center which should not be view facing,
290  # but move in the camera plane.
291  control = InteractiveMarkerControl()
292  control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
293  control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
294  control.independent_marker_orientation = True
295  control.name = "move"
296  control.markers.append( makeBox(int_marker) )
297  control.always_visible = True
298  int_marker.controls.append(control)
299 
300  server.insert(int_marker, processFeedback)
301 
303  int_marker = InteractiveMarker()
304  int_marker.header.frame_id = "base_link"
305  int_marker.pose.position = position
306  int_marker.scale = 1
307 
308  int_marker.name = "quadrocopter"
309  int_marker.description = "Quadrocopter"
310 
311  makeBoxControl(int_marker)
312 
313  control = InteractiveMarkerControl()
314  control.orientation.w = 1
315  control.orientation.x = 0
316  control.orientation.y = 1
317  control.orientation.z = 0
318  normalizeQuaternion(control.orientation)
319  control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
320  int_marker.controls.append(copy.deepcopy(control))
321  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
322  int_marker.controls.append(control)
323 
324  server.insert(int_marker, processFeedback)
325 
326 def makeChessPieceMarker(position):
327  int_marker = InteractiveMarker()
328  int_marker.header.frame_id = "base_link"
329  int_marker.pose.position = position
330  int_marker.scale = 1
331 
332  int_marker.name = "chess_piece"
333  int_marker.description = "Chess Piece\n(2D Move + Alignment)"
334 
335  control = InteractiveMarkerControl()
336  control.orientation.w = 1
337  control.orientation.x = 0
338  control.orientation.y = 1
339  control.orientation.z = 0
340  normalizeQuaternion(control.orientation)
341  control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
342  int_marker.controls.append(copy.deepcopy(control))
343 
344  # make a box which also moves in the plane
345  control.markers.append( makeBox(int_marker) )
346  control.always_visible = True
347  int_marker.controls.append(control)
348 
349  # we want to use our special callback function
350  server.insert(int_marker, processFeedback)
351 
352  # set different callback for POSE_UPDATE feedback
353  server.setCallback(int_marker.name, alignMarker, InteractiveMarkerFeedback.POSE_UPDATE )
354 
355 def makePanTiltMarker(position):
356  int_marker = InteractiveMarker()
357  int_marker.header.frame_id = "base_link"
358  int_marker.pose.position = position
359  int_marker.scale = 1
360 
361  int_marker.name = "pan_tilt"
362  int_marker.description = "Pan / Tilt"
363 
364  makeBoxControl(int_marker)
365 
366  control = InteractiveMarkerControl()
367  control.orientation.w = 1
368  control.orientation.x = 0
369  control.orientation.y = 1
370  control.orientation.z = 0
371  normalizeQuaternion(control.orientation)
372  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
373  control.orientation_mode = InteractiveMarkerControl.FIXED
374  int_marker.controls.append(control)
375 
376  control = InteractiveMarkerControl()
377  control.orientation.w = 1
378  control.orientation.x = 0
379  control.orientation.y = 0
380  control.orientation.z = 1
381  normalizeQuaternion(control.orientation)
382  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
383  control.orientation_mode = InteractiveMarkerControl.INHERIT
384  int_marker.controls.append(control)
385 
386  server.insert(int_marker, processFeedback)
387 
388 def makeMenuMarker(position):
389  int_marker = InteractiveMarker()
390  int_marker.header.frame_id = "base_link"
391  int_marker.pose.position = position
392  int_marker.scale = 1
393 
394  int_marker.name = "context_menu"
395  int_marker.description = "Context Menu\n(Right Click)"
396 
397  # make one control using default visuals
398  control = InteractiveMarkerControl()
399  control.interaction_mode = InteractiveMarkerControl.MENU
400  control.description="Options"
401  control.name = "menu_only_control"
402  int_marker.controls.append(copy.deepcopy(control))
403 
404  # make one control showing a box
405  marker = makeBox( int_marker )
406  control.markers.append( marker )
407  control.always_visible = True
408  int_marker.controls.append(control)
409 
410  server.insert(int_marker, processFeedback)
411  menu_handler.apply( server, int_marker.name )
412 
413 def makeMovingMarker(position):
414  int_marker = InteractiveMarker()
415  int_marker.header.frame_id = "moving_frame"
416  int_marker.pose.position = position
417  int_marker.scale = 1
418 
419  int_marker.name = "moving"
420  int_marker.description = "Marker Attached to a\nMoving Frame"
421 
422  control = InteractiveMarkerControl()
423  control.orientation.w = 1
424  control.orientation.x = 1
425  control.orientation.y = 0
426  control.orientation.z = 0
427  normalizeQuaternion(control.orientation)
428  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
429  int_marker.controls.append(copy.deepcopy(control))
430 
431  control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
432  control.always_visible = True
433  control.markers.append( makeBox(int_marker) )
434  int_marker.controls.append(control)
435 
436  server.insert(int_marker, processFeedback)
437 
438 
439 if __name__=="__main__":
440  rospy.init_node("basic_controls")
441  br = TransformBroadcaster()
442 
443  # create a timer to update the published transforms
444  rospy.Timer(rospy.Duration(0.01), frameCallback)
445 
446  server = InteractiveMarkerServer("basic_controls")
447 
448  menu_handler.insert( "First Entry", callback=processFeedback )
449  menu_handler.insert( "Second Entry", callback=processFeedback )
450  sub_menu_handle = menu_handler.insert( "Submenu" )
451  menu_handler.insert( "First Entry", parent=sub_menu_handle, callback=processFeedback )
452  menu_handler.insert( "Second Entry", parent=sub_menu_handle, callback=processFeedback )
453 
454 
455  position = Point(-3, 3, 0)
456  make6DofMarker( False, InteractiveMarkerControl.NONE, position, True)
457  position = Point( 0, 3, 0)
458  make6DofMarker( True, InteractiveMarkerControl.NONE, position, True)
459  position = Point( 3, 3, 0)
460  makeRandomDofMarker( position )
461  position = Point(-3, 0, 0)
462  make6DofMarker( False, InteractiveMarkerControl.ROTATE_3D, position, False)
463  position = Point( 0, 0, 0)
464  make6DofMarker( False, InteractiveMarkerControl.MOVE_ROTATE_3D, position, True )
465  position = Point( 3, 0, 0)
466  make6DofMarker( False, InteractiveMarkerControl.MOVE_3D, position, False)
467  position = Point(-3, -3, 0)
468  makeViewFacingMarker( position )
469  position = Point( 0, -3, 0)
470  makeQuadrocopterMarker( position )
471  position = Point( 3, -3, 0)
472  makeChessPieceMarker( position )
473  position = Point(-3, -6, 0)
474  makePanTiltMarker( position )
475  position = Point( 0, -6, 0)
476  makeMovingMarker( position )
477  position = Point( 3, -6, 0)
478  makeMenuMarker( position )
479 
480 
481  server.applyChanges()
482 
483  rospy.spin()
484 
def rand(min_, max_)
def makeMovingMarker(position)
def make6DofMarker(fixed, interaction_mode, position, show_6dof=False)
def makeMenuMarker(position)
def frameCallback(msg)
def makeBoxControl(msg)
def makeRandomDofMarker(position)
def makeQuadrocopterMarker(position)
def makeChessPieceMarker(position)
def processFeedback(feedback)
def makeViewFacingMarker(position)
def makePanTiltMarker(position)
def saveMarker(int_marker)
def alignMarker(feedback)
def normalizeQuaternion(quaternion_msg)
Marker Creation.


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Sat May 16 2020 03:49:16