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