interactive_marker.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2011 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 #
00018 
00019 import roslib; roslib.load_manifest('sr_unplug_connector')
00020 import rospy
00021 
00022 from interactive_markers.interactive_marker_server import *
00023 
00024 class InteractiveConnectorSelector(object):
00025     def __init__(self, object_names, callback_fct, interactive_server):
00026         # create an interactive marker server on the topic namespace simple_marker
00027         self.server = interactive_server
00028 
00029         self.callback_function = callback_fct
00030         self.object_names = object_names
00031 
00032         self.int_markers = {}
00033         self.object_markers = {}
00034         self.object_controls = {}
00035         self.select_controls = {}
00036 
00037         for object_name in self.object_names:
00038             self.create_marker( object_name )
00039 
00040     def create_marker(self, object_name):
00041         # create an interactive marker for our server
00042         int_marker = InteractiveMarker()
00043         int_marker.header.frame_id = "/"+object_name
00044         int_marker.name = object_name
00045         int_marker.description = "Select this object"
00046         int_marker.pose.position.z = -0.3
00047 
00048         self.int_markers[ object_name ] = int_marker
00049 
00050         # create a marker over the object
00051         object_marker = Marker()
00052         object_marker.type = Marker.SPHERE
00053         object_marker.pose.position.z = 0.6
00054 
00055         object_marker.scale.x = 0.1
00056         object_marker.scale.y = 0.1
00057         object_marker.scale.z = 0.1
00058         object_marker.color.r = 0.6
00059         object_marker.color.g = 0.02
00060         object_marker.color.b = 1.0
00061         object_marker.color.a = 1.0
00062         self.object_markers[object_name] = object_marker
00063 
00064         # create a non-interactive control which contains the box
00065         object_control = InteractiveMarkerControl()
00066         object_control.interaction_mode = InteractiveMarkerControl.BUTTON
00067         object_control.always_visible = True
00068         object_control.markers.append( object_marker )
00069 
00070         self.object_controls[object_name] = object_control
00071 
00072         # add the control to the interactive marker
00073         self.int_markers[object_name].controls.append( object_control )
00074 
00075         # add the interactive marker to our collection &
00076         # tell the server to call processFeedback() when feedback arrives for it
00077         self.server.insert(self.int_markers[object_name], self.processFeedback)
00078 
00079         # 'commit' changes and send to all clients
00080         self.server.applyChanges()
00081 
00082     def processFeedback(self, feedback):
00083         #this was not a click on the sphere
00084         if feedback.event_type != InteractiveMarkerFeedback.BUTTON_CLICK:
00085             return
00086         #One of the object has been selected.
00087         # We'll update all the dynamic markers
00088         # to make the selected one prominent.
00089 
00090         #this is the name of the selected object.
00091         selected_name = feedback.marker_name
00092 
00093         #we loop through all our interactive markers.
00094         for index, name in enumerate(self.object_controls):
00095             self.object_controls[name].markers.remove( self.object_markers[name] )
00096 
00097             if name == selected_name:
00098                 # the selected object marker is set
00099                 # higher, bigger and in green
00100                 self.object_markers[name].pose.position.z = 0.65
00101 
00102                 self.object_markers[name].scale.x = 0.15
00103                 self.object_markers[name].scale.y = 0.15
00104                 self.object_markers[name].scale.z = 0.15
00105                 self.object_markers[name].color.r = 0.32
00106                 self.object_markers[name].color.g = 0.83
00107                 self.object_markers[name].color.b = 0.15
00108             else:
00109                 #the other objects are slightly lower,
00110                 # smaller and in purple
00111                 self.object_markers[name].pose.position.z = 0.6
00112 
00113                 self.object_markers[name].scale.x = 0.1
00114                 self.object_markers[name].scale.y = 0.1
00115                 self.object_markers[name].scale.z = 0.1
00116                 self.object_markers[name].color.r = 0.6
00117                 self.object_markers[name].color.g = 0.02
00118                 self.object_markers[name].color.b = 1.0
00119 
00120             self.object_controls[name].markers.append( self.object_markers[name] )
00121 
00122             self.server.insert(self.int_markers[name])
00123 
00124         #we update the interactive marker server
00125         self.server.applyChanges()
00126         self.callback_function( selected_name )
00127 
00128 
00129 #this is just used for debug
00130 if __name__=="__main__":
00131     rospy.init_node("simple_marker")
00132 
00133     int_mark = InteractiveConnectorSelector(["srh/position/palm"], None)
00134     rospy.spin()


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Mon Oct 6 2014 07:42:15