interactive_marker_tutorials: basic_controls.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 Copyright (c) 2011, Willow Garage, Inc.
00005 All rights reserved.
00006 
00007 Redistribution and use in source and binary forms, with or without
00008 modification, are permitted provided that the following conditions are met:
00009 
00010     * Redistributions of source code must retain the above copyright
00011       notice, this list of conditions and the following disclaimer.
00012     * Redistributions in binary form must reproduce the above copyright
00013       notice, this list of conditions and the following disclaimer in the
00014       documentation and/or other materials provided with the distribution.
00015     * Neither the name of the Willow Garage, Inc. nor the names of its
00016       contributors may be used to endorse or promote products derived from
00017       this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025 SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS
00026 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 POSSIBILITY OF SUCH DAMAGE.
00030 """
00031 
00032 import rospy
00033 import copy
00034 
00035 from interactive_markers.interactive_marker_server import *
00036 from interactive_markers.menu_handler import *
00037 from visualization_msgs.msg import *
00038 from geometry_msgs.msg import Point
00039 from tf.broadcaster import TransformBroadcaster
00040 
00041 from random import random
00042 from math import sin
00043 
00044 server = None
00045 menu_handler = MenuHandler()
00046 br = None
00047 counter = 0
00048 
00049 def frameCallback( msg ):
00050     global counter, br
00051     time = rospy.Time.now()
00052     br.sendTransform( (0, 0, sin(counter/140.0)*2.0), (0, 0, 0, 1.0), time, "base_link", "moving_frame" )
00053     counter += 1
00054 
00055 def processFeedback( feedback ):
00056     s = "Feedback from marker '" + feedback.marker_name
00057     s += "' / control '" + feedback.control_name + "'"
00058 
00059     mp = ""
00060     if feedback.mouse_point_valid:
00061         mp = " at " + str(feedback.mouse_point.x)
00062         mp += ", " + str(feedback.mouse_point.y)
00063         mp += ", " + str(feedback.mouse_point.z)
00064         mp += " in frame " + feedback.header.frame_id
00065 
00066     if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
00067         rospy.loginfo( s + ": button click" + mp + "." )
00068     elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
00069         rospy.loginfo( s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + "." )
00070     elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
00071         rospy.loginfo( s + ": pose changed")
00072 # TODO
00073 #          << "\nposition = "
00074 #          << feedback.pose.position.x
00075 #          << ", " << feedback.pose.position.y
00076 #          << ", " << feedback.pose.position.z
00077 #          << "\norientation = "
00078 #          << feedback.pose.orientation.w
00079 #          << ", " << feedback.pose.orientation.x
00080 #          << ", " << feedback.pose.orientation.y
00081 #          << ", " << feedback.pose.orientation.z
00082 #          << "\nframe: " << feedback.header.frame_id
00083 #          << " time: " << feedback.header.stamp.sec << "sec, "
00084 #          << feedback.header.stamp.nsec << " nsec" )
00085     elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
00086         rospy.loginfo( s + ": mouse down" + mp + "." )
00087     elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
00088         rospy.loginfo( s + ": mouse up" + mp + "." )
00089     server.applyChanges()
00090 
00091 def alignMarker( feedback ):
00092     pose = feedback.pose
00093 
00094     pose.position.x = round(pose.position.x-0.5)+0.5
00095     pose.position.y = round(pose.position.y-0.5)+0.5
00096 
00097     rospy.loginfo( feedback.marker_name + ": aligning position = " + str(feedback.pose.position.x) + "," + str(feedback.pose.position.y) + "," + str(feedback.pose.position.z) + " to " +
00098                                                                      str(pose.position.x) + "," + str(pose.position.y) + "," + str(pose.position.z) )
00099 
00100     server.setPose( feedback.marker_name, pose )
00101     server.applyChanges()
00102 
00103 def rand( min_, max_ ):
00104     return min_ + random()*(max_-min_)
00105 
00106 def makeBox( msg ):
00107     marker = Marker()
00108 
00109     marker.type = Marker.CUBE
00110     marker.scale.x = msg.scale * 0.45
00111     marker.scale.y = msg.scale * 0.45
00112     marker.scale.z = msg.scale * 0.45
00113     marker.color.r = 0.5
00114     marker.color.g = 0.5
00115     marker.color.b = 0.5
00116     marker.color.a = 1.0
00117 
00118     return marker
00119 
00120 def makeBoxControl( msg ):
00121     control =  InteractiveMarkerControl()
00122     control.always_visible = True
00123     control.markers.append( makeBox(msg) )
00124     msg.controls.append( control )
00125     return control
00126 
00127 def saveMarker( int_marker ):
00128   server.insert(int_marker, processFeedback)
00129 
00130 
00131 #####################################################################
00132 # Marker Creation
00133 
00134 def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
00135     int_marker = InteractiveMarker()
00136     int_marker.header.frame_id = "base_link"
00137     int_marker.pose.position = position
00138     int_marker.scale = 1
00139 
00140     int_marker.name = "simple_6dof"
00141     int_marker.description = "Simple 6-DOF Control"
00142 
00143     # insert a box
00144     makeBoxControl(int_marker)
00145     int_marker.controls[0].interaction_mode = interaction_mode
00146 
00147     if fixed:
00148         int_marker.name += "_fixed"
00149         int_marker.description += "\n(fixed orientation)"
00150 
00151     if interaction_mode != InteractiveMarkerControl.NONE:
00152         control_modes_dict = { 
00153                           InteractiveMarkerControl.MOVE_3D : "MOVE_3D",
00154                           InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D",
00155                           InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" }
00156         int_marker.name += "_" + control_modes_dict[interaction_mode]
00157         int_marker.description = "3D Control"
00158         if show_6dof: 
00159           int_marker.description += " + 6-DOF controls"
00160         int_marker.description += "\n" + control_modes_dict[interaction_mode]
00161     
00162     if show_6dof: 
00163         control = InteractiveMarkerControl()
00164         control.orientation.w = 1
00165         control.orientation.x = 1
00166         control.orientation.y = 0
00167         control.orientation.z = 0
00168         control.name = "rotate_x"
00169         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00170         if fixed:
00171             control.orientation_mode = InteractiveMarkerControl.FIXED
00172         int_marker.controls.append(control)
00173 
00174         control = InteractiveMarkerControl()
00175         control.orientation.w = 1
00176         control.orientation.x = 1
00177         control.orientation.y = 0
00178         control.orientation.z = 0
00179         control.name = "move_x"
00180         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00181         if fixed:
00182             control.orientation_mode = InteractiveMarkerControl.FIXED
00183         int_marker.controls.append(control)
00184 
00185         control = InteractiveMarkerControl()
00186         control.orientation.w = 1
00187         control.orientation.x = 0
00188         control.orientation.y = 1
00189         control.orientation.z = 0
00190         control.name = "rotate_z"
00191         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00192         if fixed:
00193             control.orientation_mode = InteractiveMarkerControl.FIXED
00194         int_marker.controls.append(control)
00195 
00196         control = InteractiveMarkerControl()
00197         control.orientation.w = 1
00198         control.orientation.x = 0
00199         control.orientation.y = 1
00200         control.orientation.z = 0
00201         control.name = "move_z"
00202         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00203         if fixed:
00204             control.orientation_mode = InteractiveMarkerControl.FIXED
00205         int_marker.controls.append(control)
00206 
00207         control = InteractiveMarkerControl()
00208         control.orientation.w = 1
00209         control.orientation.x = 0
00210         control.orientation.y = 0
00211         control.orientation.z = 1
00212         control.name = "rotate_y"
00213         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00214         if fixed:
00215             control.orientation_mode = InteractiveMarkerControl.FIXED
00216         int_marker.controls.append(control)
00217 
00218         control = InteractiveMarkerControl()
00219         control.orientation.w = 1
00220         control.orientation.x = 0
00221         control.orientation.y = 0
00222         control.orientation.z = 1
00223         control.name = "move_y"
00224         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00225         if fixed:
00226             control.orientation_mode = InteractiveMarkerControl.FIXED
00227         int_marker.controls.append(control)
00228 
00229     server.insert(int_marker, processFeedback)
00230     menu_handler.apply( server, int_marker.name )
00231 
00232 def makeRandomDofMarker( position ):
00233     int_marker = InteractiveMarker()
00234     int_marker.header.frame_id = "base_link"
00235     int_marker.pose.position = position
00236     int_marker.scale = 1
00237 
00238     int_marker.name = "6dof_random_axes"
00239     int_marker.description = "6-DOF\n(Arbitrary Axes)"
00240 
00241     makeBoxControl(int_marker)
00242 
00243     control = InteractiveMarkerControl()
00244 
00245     for i in range(3):
00246         control.orientation.w = rand(-1,1)
00247         control.orientation.x = rand(-1,1)
00248         control.orientation.y = rand(-1,1)
00249         control.orientation.z = rand(-1,1)
00250         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00251         int_marker.controls.append(copy.deepcopy(control))
00252         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00253         int_marker.controls.append(copy.deepcopy(control))
00254 
00255     server.insert(int_marker, processFeedback)
00256 
00257 def makeViewFacingMarker(position):
00258     int_marker = InteractiveMarker()
00259     int_marker.header.frame_id = "base_link"
00260     int_marker.pose.position = position
00261     int_marker.scale = 1
00262 
00263     int_marker.name = "view_facing"
00264     int_marker.description = "View Facing 6-DOF"
00265 
00266     # make a control that rotates around the view axis
00267     control = InteractiveMarkerControl()
00268     control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
00269     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00270     control.orientation.w = 1
00271     control.name = "rotate"
00272     int_marker.controls.append(control)
00273 
00274     # create a box in the center which should not be view facing,
00275     # but move in the camera plane.
00276     control = InteractiveMarkerControl()
00277     control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
00278     control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
00279     control.independent_marker_orientation = True
00280     control.name = "move"
00281     control.markers.append( makeBox(int_marker) )
00282     control.always_visible = True
00283     int_marker.controls.append(control)
00284 
00285     server.insert(int_marker, processFeedback)
00286 
00287 def makeQuadrocopterMarker(position):
00288     int_marker = InteractiveMarker()
00289     int_marker.header.frame_id = "base_link"
00290     int_marker.pose.position = position
00291     int_marker.scale = 1
00292 
00293     int_marker.name = "quadrocopter"
00294     int_marker.description = "Quadrocopter"
00295 
00296     makeBoxControl(int_marker)
00297 
00298     control = InteractiveMarkerControl()
00299     control.orientation.w = 1
00300     control.orientation.x = 0
00301     control.orientation.y = 1
00302     control.orientation.z = 0
00303     control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
00304     int_marker.controls.append(copy.deepcopy(control))
00305     control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00306     int_marker.controls.append(control)
00307 
00308     server.insert(int_marker, processFeedback)
00309 
00310 def makeChessPieceMarker(position):
00311     int_marker = InteractiveMarker()
00312     int_marker.header.frame_id = "base_link"
00313     int_marker.pose.position = position
00314     int_marker.scale = 1
00315 
00316     int_marker.name = "chess_piece"
00317     int_marker.description = "Chess Piece\n(2D Move + Alignment)"
00318 
00319     control = InteractiveMarkerControl()
00320     control.orientation.w = 1
00321     control.orientation.x = 0
00322     control.orientation.y = 1
00323     control.orientation.z = 0
00324     control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
00325     int_marker.controls.append(copy.deepcopy(control))
00326 
00327     # make a box which also moves in the plane
00328     control.markers.append( makeBox(int_marker) )
00329     control.always_visible = True
00330     int_marker.controls.append(control)
00331 
00332     # we want to use our special callback function
00333     server.insert(int_marker, processFeedback)
00334 
00335     # set different callback for POSE_UPDATE feedback
00336     server.setCallback(int_marker.name, alignMarker, InteractiveMarkerFeedback.POSE_UPDATE )
00337 
00338 def makePanTiltMarker(position):
00339     int_marker = InteractiveMarker()
00340     int_marker.header.frame_id = "base_link"
00341     int_marker.pose.position = position
00342     int_marker.scale = 1
00343 
00344     int_marker.name = "pan_tilt"
00345     int_marker.description = "Pan / Tilt"
00346 
00347     makeBoxControl(int_marker)
00348 
00349     control = InteractiveMarkerControl()
00350     control.orientation.w = 1
00351     control.orientation.x = 0
00352     control.orientation.y = 1
00353     control.orientation.z = 0
00354     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00355     control.orientation_mode = InteractiveMarkerControl.FIXED
00356     int_marker.controls.append(control)
00357 
00358     control = InteractiveMarkerControl()
00359     control.orientation.w = 1
00360     control.orientation.x = 0
00361     control.orientation.y = 0
00362     control.orientation.z = 1
00363     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00364     control.orientation_mode = InteractiveMarkerControl.INHERIT
00365     int_marker.controls.append(control)
00366 
00367     server.insert(int_marker, processFeedback)
00368 
00369 def makeMenuMarker(position):
00370     int_marker = InteractiveMarker()
00371     int_marker.header.frame_id = "base_link"
00372     int_marker.pose.position = position
00373     int_marker.scale = 1
00374 
00375     int_marker.name = "context_menu"
00376     int_marker.description = "Context Menu\n(Right Click)"
00377 
00378     # make one control using default visuals
00379     control = InteractiveMarkerControl()
00380     control.interaction_mode = InteractiveMarkerControl.MENU
00381     control.description="Options"
00382     control.name = "menu_only_control"
00383     int_marker.controls.append(copy.deepcopy(control))
00384 
00385     # make one control showing a box
00386     marker = makeBox( int_marker )
00387     control.markers.append( marker )
00388     control.always_visible = True
00389     int_marker.controls.append(control)
00390 
00391     server.insert(int_marker, processFeedback)
00392     menu_handler.apply( server, int_marker.name )
00393 
00394 def makeMovingMarker(position):
00395     int_marker = InteractiveMarker()
00396     int_marker.header.frame_id = "moving_frame"
00397     int_marker.pose.position = position
00398     int_marker.scale = 1
00399 
00400     int_marker.name = "moving"
00401     int_marker.description = "Marker Attached to a\nMoving Frame"
00402 
00403     control = InteractiveMarkerControl()
00404     control.orientation.w = 1
00405     control.orientation.x = 1
00406     control.orientation.y = 0
00407     control.orientation.z = 0
00408     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00409     int_marker.controls.append(copy.deepcopy(control))
00410 
00411     control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
00412     control.always_visible = True
00413     control.markers.append( makeBox(int_marker) )
00414     int_marker.controls.append(control)
00415 
00416     server.insert(int_marker, processFeedback)
00417 
00418 
00419 if __name__=="__main__":
00420     rospy.init_node("basic_controls")
00421     br = TransformBroadcaster()
00422     
00423     # create a timer to update the published transforms
00424     rospy.Timer(rospy.Duration(0.01), frameCallback)
00425 
00426     server = InteractiveMarkerServer("basic_controls")
00427 
00428     menu_handler.insert( "First Entry", callback=processFeedback )
00429     menu_handler.insert( "Second Entry", callback=processFeedback )
00430     sub_menu_handle = menu_handler.insert( "Submenu" )
00431     menu_handler.insert( "First Entry", parent=sub_menu_handle, callback=processFeedback )
00432     menu_handler.insert( "Second Entry", parent=sub_menu_handle, callback=processFeedback )
00433   
00434     
00435     position = Point(-3, 3, 0)
00436     make6DofMarker( False, InteractiveMarkerControl.NONE, position, True)
00437     position = Point( 0, 3, 0)
00438     make6DofMarker( True, InteractiveMarkerControl.NONE, position, True)
00439     position = Point( 3, 3, 0)
00440     makeRandomDofMarker( position )
00441     position = Point(-3, 0, 0)
00442     make6DofMarker( False, InteractiveMarkerControl.ROTATE_3D, position, False)
00443     position = Point( 0, 0, 0)
00444     make6DofMarker( False, InteractiveMarkerControl.MOVE_ROTATE_3D, position, True )
00445     position = Point( 3, 0, 0)
00446     make6DofMarker( False, InteractiveMarkerControl.MOVE_3D, position, False)
00447     position = Point(-3, -3, 0)
00448     makeViewFacingMarker( position )
00449     position = Point( 0, -3, 0)
00450     makeQuadrocopterMarker( position )
00451     position = Point( 3, -3, 0)
00452     makeChessPieceMarker( position )
00453     position = Point(-3, -6, 0)
00454     makePanTiltMarker( position )
00455     position = Point( 0, -6, 0)
00456     makeMovingMarker( position )
00457     position = Point( 3, -6, 0)
00458     makeMenuMarker( position )
00459     
00460 
00461     server.applyChanges()
00462 
00463     rospy.spin()
00464