Merge pull request #7 from aleeper/move_rotate_3d

new layout and 3D controls in basic_controls

Author: David Gossow

interactive_marker_tutorials/python/basic_controls.py

@@ -35,13 +35,13 @@
 
 from interactive_markers.interactive_marker_server import *
 from interactive_markers.menu_handler import *
+from geometry_msgs.msg import Point
 from tf.broadcaster import TransformBroadcaster
 
 from random import random
 from math import sin
 
 server = None
-marker_pos = 0
 menu_handler = MenuHandler()
 br = None
 counter = 0
@@ -131,98 +131,108 @@ def saveMarker( int_marker ):
 #####################################################################
 # Marker Creation
 
-def make6DofMarker( fixed ):
-    global marker_pos
+def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "/base_link"
-    int_marker.pose.position.y = -3.0 * marker_pos
-    marker_pos += 1
+    int_marker.pose.position = position
     int_marker.scale = 1
 
     int_marker.name = "simple_6dof"
     int_marker.description = "Simple 6-DOF Control"
 
     # insert a box
     makeBoxControl(int_marker)
+    int_marker.controls[0].interaction_mode = interaction_mode
 
     if fixed:
         int_marker.name += "_fixed"
         int_marker.description += "\n(fixed orientation)"
 
-    control = InteractiveMarkerControl()
-    control.orientation.w = 1
-    control.orientation.x = 1
-    control.orientation.y = 0
-    control.orientation.z = 0
-    control.name = "rotate_x"
-    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
-    if fixed:
-        control.orientation_mode = InteractiveMarkerControl.FIXED
-    int_marker.controls.append(control)
+    if interaction_mode != InteractiveMarkerControl.NONE:
+        control_modes_dict = { 
+                          InteractiveMarkerControl.MOVE_3D : "MOVE_3D",
+                          InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D",
+                          InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" }
+        int_marker.name += "_" + control_modes_dict[interaction_mode]
+        int_marker.description = "3D Control"
+        if show_6dof: 
+          int_marker.description += " + 6-DOF controls"
+        int_marker.description += "\n" + control_modes_dict[interaction_mode]
+    
+    if show_6dof: 
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 1
+        control.orientation.y = 0
+        control.orientation.z = 0
+        control.name = "rotate_x"
+        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
 
-    control = InteractiveMarkerControl()
-    control.orientation.w = 1
-    control.orientation.x = 1
-    control.orientation.y = 0
-    control.orientation.z = 0
-    control.name = "move_x"
-    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-    if fixed:
-        control.orientation_mode = InteractiveMarkerControl.FIXED
-    int_marker.controls.append(control)
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 1
+        control.orientation.y = 0
+        control.orientation.z = 0
+        control.name = "move_x"
+        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
 
-    control = InteractiveMarkerControl()
-    control.orientation.w = 1
-    control.orientation.x = 0
-    control.orientation.y = 1
-    control.orientation.z = 0
-    control.name = "rotate_z"
-    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
-    if fixed:
-        control.orientation_mode = InteractiveMarkerControl.FIXED
-    int_marker.controls.append(control)
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 0
+        control.orientation.y = 1
+        control.orientation.z = 0
+        control.name = "rotate_z"
+        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
 
-    control = InteractiveMarkerControl()
-    control.orientation.w = 1
-    control.orientation.x = 0
-    control.orientation.y = 1
-    control.orientation.z = 0
-    control.name = "move_z"
-    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-    if fixed:
-        control.orientation_mode = InteractiveMarkerControl.FIXED
-    int_marker.controls.append(control)
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 0
+        control.orientation.y = 1
+        control.orientation.z = 0
+        control.name = "move_z"
+        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
 
-    control = InteractiveMarkerControl()
-    control.orientation.w = 1
-    control.orientation.x = 0
-    control.orientation.y = 0
-    control.orientation.z = 1
-    control.name = "rotate_y"
-    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
-    if fixed:
-        control.orientation_mode = InteractiveMarkerControl.FIXED
-    int_marker.controls.append(control)
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 0
+        control.orientation.y = 0
+        control.orientation.z = 1
+        control.name = "rotate_y"
+        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
 
-    control = InteractiveMarkerControl()
-    control.orientation.w = 1
-    control.orientation.x = 0
-    control.orientation.y = 0
-    control.orientation.z = 1
-    control.name = "move_y"
-    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-    if fixed:
-        control.orientation_mode = InteractiveMarkerControl.FIXED
-    int_marker.controls.append(control)
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 0
+        control.orientation.y = 0
+        control.orientation.z = 1
+        control.name = "move_y"
+        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
 
     server.insert(int_marker, processFeedback)
+    menu_handler.apply( server, int_marker.name )
 
-def makeRandomDofMarker():
-    global marker_pos
+def makeRandomDofMarker( position ):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "/base_link"
-    int_marker.pose.position.y = -3.0 * marker_pos
-    marker_pos += 1
+    int_marker.pose.position = position
     int_marker.scale = 1
 
     int_marker.name = "6dof_random_axes"
@@ -240,16 +250,14 @@ def makeRandomDofMarker():
         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
         int_marker.controls.append(copy.deepcopy(control))
         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-        int_marker.controls.append(control)
+        int_marker.controls.append(copy.deepcopy(control))
 
     server.insert(int_marker, processFeedback)
 
-def makeViewFacingMarker():
-    global marker_pos
+def makeViewFacingMarker(position):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "/base_link"
-    int_marker.pose.position.y = -3.0 * marker_pos
-    marker_pos += 1
+    int_marker.pose.position = position
     int_marker.scale = 1
 
     int_marker.name = "view_facing"
@@ -276,12 +284,10 @@ def makeViewFacingMarker():
 
     server.insert(int_marker, processFeedback)
 
-def makeQuadrocopterMarker():
-    global marker_pos
+def makeQuadrocopterMarker(position):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "/base_link"
-    int_marker.pose.position.y = -3.0 * marker_pos
-    marker_pos += 1
+    int_marker.pose.position = position
     int_marker.scale = 1
 
     int_marker.name = "quadrocopter"
@@ -301,12 +307,10 @@ def makeQuadrocopterMarker():
 
     server.insert(int_marker, processFeedback)
 
-def makeChessPieceMarker():
-    global marker_pos
+def makeChessPieceMarker(position):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "/base_link"
-    int_marker.pose.position.y = -3.0 * marker_pos
-    marker_pos += 1
+    int_marker.pose.position = position
     int_marker.scale = 1
 
     int_marker.name = "chess_piece"
@@ -331,12 +335,10 @@ def makeChessPieceMarker():
     # set different callback for POSE_UPDATE feedback
     server.setCallback(int_marker.name, alignMarker, InteractiveMarkerFeedback.POSE_UPDATE )
 
-def makePanTiltMarker():
-    global marker_pos
+def makePanTiltMarker(position):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "/base_link"
-    int_marker.pose.position.y = -3.0 * marker_pos
-    marker_pos += 1
+    int_marker.pose.position = position
     int_marker.scale = 1
 
     int_marker.name = "pan_tilt"
@@ -364,12 +366,10 @@ def makePanTiltMarker():
 
     server.insert(int_marker, processFeedback)
 
-def makeMenuMarker():
-    global marker_pos
+def makeMenuMarker(position):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "/base_link"
-    int_marker.pose.position.y = -3.0 * marker_pos
-    marker_pos += 1
+    int_marker.pose.position = position
     int_marker.scale = 1
 
     int_marker.name = "context_menu"
@@ -391,12 +391,10 @@ def makeMenuMarker():
     server.insert(int_marker, processFeedback)
     menu_handler.apply( server, int_marker.name )
 
-def makeMovingMarker():
-    global marker_pos
+def makeMovingMarker(position):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "/moving_frame"
-    int_marker.pose.position.y = -3.0 * marker_pos
-    marker_pos += 1
+    int_marker.pose.position = position
     int_marker.scale = 1
 
     int_marker.name = "moving"
@@ -432,16 +430,33 @@ def makeMovingMarker():
     sub_menu_handle = menu_handler.insert( "Submenu" )
     menu_handler.insert( "First Entry", parent=sub_menu_handle, callback=processFeedback )
     menu_handler.insert( "Second Entry", parent=sub_menu_handle, callback=processFeedback )
-
-    make6DofMarker( False )
-    make6DofMarker( True )
-    makeRandomDofMarker( )
-    makeViewFacingMarker( )
-    makeQuadrocopterMarker( )
-    makeChessPieceMarker( )
-    makePanTiltMarker( )
-    makeMenuMarker( )
-    makeMovingMarker( )
+  
+    
+    position = Point(-3, 3, 0)
+    make6DofMarker( False, InteractiveMarkerControl.NONE, position, True)
+    position = Point( 0, 3, 0)
+    make6DofMarker( True, InteractiveMarkerControl.NONE, position, True)
+    position = Point( 3, 3, 0)
+    makeRandomDofMarker( position )
+    position = Point(-3, 0, 0)
+    make6DofMarker( False, InteractiveMarkerControl.ROTATE_3D, position, False)
+    position = Point( 0, 0, 0)
+    make6DofMarker( False, InteractiveMarkerControl.MOVE_ROTATE_3D, position, True )
+    position = Point( 3, 0, 0)
+    make6DofMarker( False, InteractiveMarkerControl.MOVE_3D, position, False)
+    position = Point(-3, -3, 0)
+    makeViewFacingMarker( position )
+    position = Point( 0, -3, 0)
+    makeQuadrocopterMarker( position )
+    position = Point( 3, -3, 0)
+    makeChessPieceMarker( position )
+    position = Point(-3, -6, 0)
+    makePanTiltMarker( position )
+    position = Point( 0, -6, 0)
+    makeMovingMarker( position )
+    position = Point( 3, -6, 0)
+    makeMenuMarker( position )
+    
 
     server.applyChanges()