interactive_marker_tutorials: interactive_marker_tutorials: basic

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 roslib; roslib.load_manifest("interactive_markers")
00033 import rospy
00034 import copy
00035 
00036 from interactive_markers.interactive_marker_server import *
00037 from interactive_markers.menu_handler import *
00038 from tf.broadcaster import TransformBroadcaster
00039 
00040 from random import random
00041 from math import sin
00042 
00043 server = None
00044 marker_pos = 0
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 ):
00135     global marker_pos
00136     int_marker = InteractiveMarker()
00137     int_marker.header.frame_id = "/base_link"
00138     int_marker.pose.position.y = -3.0 * marker_pos
00139     marker_pos += 1
00140     int_marker.scale = 1
00141 
00142     int_marker.name = "simple_6dof"
00143     int_marker.description = "Simple 6-DOF Control"
00144 
00145     # insert a box
00146     makeBoxControl(int_marker)
00147 
00148     if fixed:
00149         int_marker.name += "_fixed"
00150         int_marker.description += "\n(fixed orientation)"
00151 
00152     control = InteractiveMarkerControl()
00153     control.orientation.w = 1
00154     control.orientation.x = 1
00155     control.orientation.y = 0
00156     control.orientation.z = 0
00157     control.name = "rotate_x"
00158     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00159     if fixed:
00160         control.orientation_mode = InteractiveMarkerControl.FIXED
00161     int_marker.controls.append(control)
00162 
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 = "move_x"
00169     control.interaction_mode = InteractiveMarkerControl.MOVE_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 = 0
00177     control.orientation.y = 1
00178     control.orientation.z = 0
00179     control.name = "rotate_z"
00180     control.interaction_mode = InteractiveMarkerControl.ROTATE_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 = "move_z"
00191     control.interaction_mode = InteractiveMarkerControl.MOVE_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 = 0
00200     control.orientation.z = 1
00201     control.name = "rotate_y"
00202     control.interaction_mode = InteractiveMarkerControl.ROTATE_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 = "move_y"
00213     control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00214     if fixed:
00215         control.orientation_mode = InteractiveMarkerControl.FIXED
00216     int_marker.controls.append(control)
00217 
00218     server.insert(int_marker, processFeedback)
00219 
00220 def makeRandomDofMarker():
00221     global marker_pos
00222     int_marker = InteractiveMarker()
00223     int_marker.header.frame_id = "/base_link"
00224     int_marker.pose.position.y = -3.0 * marker_pos
00225     marker_pos += 1
00226     int_marker.scale = 1
00227 
00228     int_marker.name = "6dof_random_axes"
00229     int_marker.description = "6-DOF\n(Arbitrary Axes)"
00230 
00231     makeBoxControl(int_marker)
00232 
00233     control = InteractiveMarkerControl()
00234 
00235     for i in range(3):
00236         control.orientation.w = rand(-1,1)
00237         control.orientation.x = rand(-1,1)
00238         control.orientation.y = rand(-1,1)
00239         control.orientation.z = rand(-1,1)
00240         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00241         int_marker.controls.append(copy.deepcopy(control))
00242         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00243         int_marker.controls.append(control)
00244 
00245     server.insert(int_marker, processFeedback)
00246 
00247 def makeViewFacingMarker():
00248     global marker_pos
00249     int_marker = InteractiveMarker()
00250     int_marker.header.frame_id = "/base_link"
00251     int_marker.pose.position.y = -3.0 * marker_pos
00252     marker_pos += 1
00253     int_marker.scale = 1
00254 
00255     int_marker.name = "view_facing"
00256     int_marker.description = "View Facing 6-DOF"
00257 
00258     # make a control that rotates around the view axis
00259     control = InteractiveMarkerControl()
00260     control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
00261     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00262     control.orientation.w = 1
00263     control.name = "rotate"
00264     int_marker.controls.append(control)
00265 
00266     # create a box in the center which should not be view facing,
00267     # but move in the camera plane.
00268     control = InteractiveMarkerControl()
00269     control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
00270     control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
00271     control.independent_marker_orientation = True
00272     control.name = "move"
00273     control.markers.append( makeBox(int_marker) )
00274     control.always_visible = True
00275     int_marker.controls.append(control)
00276 
00277     server.insert(int_marker, processFeedback)
00278 
00279 def makeQuadrocopterMarker():
00280     global marker_pos
00281     int_marker = InteractiveMarker()
00282     int_marker.header.frame_id = "/base_link"
00283     int_marker.pose.position.y = -3.0 * marker_pos
00284     marker_pos += 1
00285     int_marker.scale = 1
00286 
00287     int_marker.name = "quadrocopter"
00288     int_marker.description = "Quadrocopter"
00289 
00290     makeBoxControl(int_marker)
00291 
00292     control = InteractiveMarkerControl()
00293     control.orientation.w = 1
00294     control.orientation.x = 0
00295     control.orientation.y = 1
00296     control.orientation.z = 0
00297     control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
00298     int_marker.controls.append(copy.deepcopy(control))
00299     control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00300     int_marker.controls.append(control)
00301 
00302     server.insert(int_marker, processFeedback)
00303 
00304 def makeChessPieceMarker():
00305     global marker_pos
00306     int_marker = InteractiveMarker()
00307     int_marker.header.frame_id = "/base_link"
00308     int_marker.pose.position.y = -3.0 * marker_pos
00309     marker_pos += 1
00310     int_marker.scale = 1
00311 
00312     int_marker.name = "chess_piece"
00313     int_marker.description = "Chess Piece\n(2D Move + Alignment)"
00314 
00315     control = InteractiveMarkerControl()
00316     control.orientation.w = 1
00317     control.orientation.x = 0
00318     control.orientation.y = 1
00319     control.orientation.z = 0
00320     control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
00321     int_marker.controls.append(copy.deepcopy(control))
00322 
00323     # make a box which also moves in the plane
00324     control.markers.append( makeBox(int_marker) )
00325     control.always_visible = True
00326     int_marker.controls.append(control)
00327 
00328     # we want to use our special callback function
00329     server.insert(int_marker, processFeedback)
00330 
00331     # set different callback for POSE_UPDATE feedback
00332     server.setCallback(int_marker.name, alignMarker, InteractiveMarkerFeedback.POSE_UPDATE )
00333 
00334 def makePanTiltMarker():
00335     global marker_pos
00336     int_marker = InteractiveMarker()
00337     int_marker.header.frame_id = "/base_link"
00338     int_marker.pose.position.y = -3.0 * marker_pos
00339     marker_pos += 1
00340     int_marker.scale = 1
00341 
00342     int_marker.name = "pan_tilt"
00343     int_marker.description = "Pan / Tilt"
00344 
00345     makeBoxControl(int_marker)
00346 
00347     control = InteractiveMarkerControl()
00348     control.orientation.w = 1
00349     control.orientation.x = 0
00350     control.orientation.y = 1
00351     control.orientation.z = 0
00352     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00353     control.orientation_mode = InteractiveMarkerControl.FIXED
00354     int_marker.controls.append(control)
00355 
00356     control = InteractiveMarkerControl()
00357     control.orientation.w = 1
00358     control.orientation.x = 0
00359     control.orientation.y = 0
00360     control.orientation.z = 1
00361     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00362     control.orientation_mode = InteractiveMarkerControl.INHERIT
00363     int_marker.controls.append(control)
00364 
00365     server.insert(int_marker, processFeedback)
00366 
00367 def makeMenuMarker():
00368     global marker_pos
00369     int_marker = InteractiveMarker()
00370     int_marker.header.frame_id = "/base_link"
00371     int_marker.pose.position.y = -3.0 * marker_pos
00372     marker_pos += 1
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():
00395     global marker_pos
00396     int_marker = InteractiveMarker()
00397     int_marker.header.frame_id = "/moving_frame"
00398     int_marker.pose.position.y = -3.0 * marker_pos
00399     marker_pos += 1
00400     int_marker.scale = 1
00401 
00402     int_marker.name = "moving"
00403     int_marker.description = "Marker Attached to a\nMoving Frame"
00404 
00405     control = InteractiveMarkerControl()
00406     control.orientation.w = 1
00407     control.orientation.x = 1
00408     control.orientation.y = 0
00409     control.orientation.z = 0
00410     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00411     int_marker.controls.append(copy.deepcopy(control))
00412 
00413     control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
00414     control.always_visible = True
00415     control.markers.append( makeBox(int_marker) )
00416     int_marker.controls.append(control)
00417 
00418     server.insert(int_marker, processFeedback)
00419 
00420 
00421 if __name__=="__main__":
00422     rospy.init_node("basic_controls")
00423     br = TransformBroadcaster()
00424     
00425     # create a timer to update the published transforms
00426     rospy.Timer(rospy.Duration(0.01), frameCallback)
00427 
00428     server = InteractiveMarkerServer("basic_controls")
00429 
00430     menu_handler.insert( "First Entry", callback=processFeedback )
00431     menu_handler.insert( "Second Entry", callback=processFeedback )
00432     sub_menu_handle = menu_handler.insert( "Submenu" )
00433     menu_handler.insert( "First Entry", parent=sub_menu_handle, callback=processFeedback )
00434     menu_handler.insert( "Second Entry", parent=sub_menu_handle, callback=processFeedback )
00435 
00436     make6DofMarker( False )
00437     make6DofMarker( True )
00438     makeRandomDofMarker( )
00439     makeViewFacingMarker( )
00440     makeQuadrocopterMarker( )
00441     makeChessPieceMarker( )
00442     makePanTiltMarker( )
00443     makeMenuMarker( )
00444     makeMovingMarker( )
00445 
00446     server.applyChanges()
00447 
00448     rospy.spin()
00449
basic_controls.py
