interactive_marker_tutorials: interactive_marker

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 
00035 from interactive_markers.interactive_marker_server import *
00036 from interactive_markers.menu_handler import *
00037 
00038 server = None
00039 marker_pos = 0
00040 
00041 menu_handler = MenuHandler()
00042 
00043 h_first_entry = 0
00044 h_mode_last = 0
00045 
00046 def enableCb( feedback ):
00047     handle = feedback.menu_entry_id
00048     state = menu_handler.getCheckState( handle )
00049 
00050     if state == MenuHandler.CHECKED:
00051         menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00052         rospy.loginfo("Hiding first menu entry")
00053         menu_handler.setVisible( h_first_entry, False )
00054     else:
00055         menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00056         rospy.loginfo("Showing first menu entry")
00057         menu_handler.setVisible( h_first_entry, True )
00058 
00059     menu_handler.reApply( server )
00060     rospy.loginfo("update")
00061     server.applyChanges()
00062 
00063 def modeCb(feedback):
00064     global h_mode_last
00065     menu_handler.setCheckState( h_mode_last, MenuHandler.UNCHECKED )
00066     h_mode_last = feedback.menu_entry_id
00067     menu_handler.setCheckState( h_mode_last, MenuHandler.CHECKED )
00068 
00069     rospy.loginfo("Switching to menu entry #" + str(h_mode_last))
00070     menu_handler.reApply( server )
00071     print "DONE"
00072     server.applyChanges()
00073 
00074 def makeBox( msg ):
00075     marker = Marker()
00076 
00077     marker.type = Marker.CUBE
00078     marker.scale.x = msg.scale * 0.45
00079     marker.scale.y = msg.scale * 0.45
00080     marker.scale.z = msg.scale * 0.45
00081     marker.color.r = 0.5
00082     marker.color.g = 0.5
00083     marker.color.b = 0.5
00084     marker.color.a = 1.0
00085 
00086     return marker
00087 
00088 def makeBoxControl( msg ):
00089     control = InteractiveMarkerControl()
00090     control.always_visible = True
00091     control.markers.append( makeBox(msg) )
00092     msg.controls.append( control )
00093     return control
00094 
00095 def makeEmptyMarker( dummyBox=True ):
00096     global marker_pos
00097     int_marker = InteractiveMarker()
00098     int_marker.header.frame_id = "/base_link"
00099     int_marker.pose.position.y = -3.0 * marker_pos
00100     marker_pos += 1
00101     int_marker.scale = 1
00102     return int_marker
00103 
00104 def makeMenuMarker( name ):
00105     int_marker = makeEmptyMarker()
00106     int_marker.name = name
00107 
00108     control = InteractiveMarkerControl()
00109 
00110     control.interaction_mode = InteractiveMarkerControl.BUTTON
00111     control.always_visible = True
00112 
00113     control.markers.append( makeBox( int_marker ) )
00114     int_marker.controls.append(control)
00115 
00116     server.insert( int_marker )
00117 
00118 def deepCb( feedback ):
00119     rospy.loginfo("The deep sub-menu has been found.")
00120 
00121 def initMenu():
00122     global h_first_entry, h_mode_last
00123     h_first_entry = menu_handler.insert( "First Entry" )
00124     entry = menu_handler.insert( "deep", parent=h_first_entry)
00125     entry = menu_handler.insert( "sub", parent=entry );
00126     entry = menu_handler.insert( "menu", parent=entry, callback=deepCb );
00127 
00128     menu_handler.setCheckState( menu_handler.insert( "Show First Entry", callback=enableCb ), MenuHandler.CHECKED )
00129 
00130     sub_menu_handle = menu_handler.insert( "Switch" )
00131     for i in range(5):
00132         s = "Mode " + str(i)
00133         h_mode_last = menu_handler.insert( s, parent=sub_menu_handle, callback=modeCb )
00134         menu_handler.setCheckState( h_mode_last, MenuHandler.UNCHECKED)
00135     # check the very last entry
00136     menu_handler.setCheckState( h_mode_last, MenuHandler.CHECKED )
00137 
00138 if __name__=="__main__":
00139     rospy.init_node("menu")
00140     
00141     server = InteractiveMarkerServer("menu")
00142 
00143     initMenu()
00144     
00145     makeMenuMarker( "marker1" )
00146     makeMenuMarker( "marker2" )
00147 
00148     menu_handler.apply( server, "marker1" )
00149     menu_handler.apply( server, "marker2" )
00150     server.applyChanges()
00151 
00152     rospy.spin()
00153
menu.py
