00001 import roslib; roslib.load_manifest('easy_markers')
00002
00003 from easy_markers.generator import MarkerGenerator
00004 from interactive_markers.interactive_marker_server import *
00005 from interactive_markers.menu_handler import *
00006
00007 TYPEDATA = {
00008 'rotate_x': [1,1,0,0, InteractiveMarkerControl.ROTATE_AXIS],
00009 'move_x' : [1,1,0,0, InteractiveMarkerControl.MOVE_AXIS],
00010 'rotate_z': [1,0,1,0, InteractiveMarkerControl.ROTATE_AXIS],
00011 'move_z' : [1,0,1,0, InteractiveMarkerControl.MOVE_AXIS],
00012 'rotate_y': [1,0,0,1, InteractiveMarkerControl.ROTATE_AXIS],
00013 'move_y' : [1,0,0,1, InteractiveMarkerControl.MOVE_AXIS]
00014 }
00015
00016 def default_callback(feedback):
00017 print feedback
00018
00019 class InteractiveGenerator:
00020 def __init__(self, name="interactive_markers"):
00021 self.server = InteractiveMarkerServer(name)
00022 self.mg = MarkerGenerator()
00023 self.mg.type = 1
00024 self.mg.scale = [.25]*3
00025 self.c = 0
00026 self.markers = {}
00027
00028
00029
00030 def makeMarker( self, callback=None, marker=None, pose=[0,0,0], controls=[],
00031 fixed=False, name=None, frame="/map", description="", imode=0):
00032
00033 if marker is None:
00034 marker = self.mg.marker()
00035
00036 if callback is None:
00037 callback = default_callback
00038
00039 if name is None:
00040 name = "control%d"%self.c
00041 self.c += 1
00042
00043 int_marker = InteractiveMarker()
00044 int_marker.header.frame_id = frame
00045 int_marker.pose.position.x = pose[0]
00046 int_marker.pose.position.y = pose[1]
00047 int_marker.pose.position.z = pose[2]
00048 int_marker.scale = 1
00049 int_marker.name = name
00050 int_marker.description = description
00051
00052 control = InteractiveMarkerControl()
00053 control.always_visible = True
00054 control.interaction_mode = imode
00055 control.markers.append( marker )
00056 int_marker.controls.append(control)
00057
00058 for control_name in controls:
00059 data = TYPEDATA[control_name]
00060 control = InteractiveMarkerControl()
00061 control.orientation.w = data[0]
00062 control.orientation.x = data[1]
00063 control.orientation.y = data[2]
00064 control.orientation.z = data[3]
00065 control.name = control_name
00066 control.interaction_mode = data[4]
00067 if fixed:
00068 control.orientation_mode = InteractiveMarkerControl.FIXED
00069 int_marker.controls.append(control)
00070
00071 self.server.insert(int_marker, callback)
00072 self.markers[name] = int_marker
00073 self.server.applyChanges()
