hrl_lib: interactive_marker_helper.py Source File

Go to the documentation of this file.
00001 import interactive_markers.interactive_marker_server as ims
00002 import std_msgs.msg as stdm
00003 
00004 def feedback_to_string(ftype):
00005     names = ['keep_alive', 'pose_update', 
00006              'menu_select', 'button_click',
00007              'mouse_down', 'mouse_up']
00008     fb = ims.InteractiveMarkerFeedback
00009     consts = [fb.KEEP_ALIVE, fb.POSE_UPDATE,
00010                 fb.MENU_SELECT, fb.BUTTON_CLICK,
00011                 fb.MOUSE_DOWN, fb.MOUSE_UP]
00012 
00013     for n, value in zip(names, consts):
00014         if ftype == value:
00015             return n
00016 
00017     return 'invalid type'
00018 
00019 
00020 def interactive_marker(name, pose, scale):
00021     int_marker = ims.InteractiveMarker()
00022     int_marker.header.frame_id = "/map"
00023     int_marker.pose.position.x = pose[0][0]
00024     int_marker.pose.position.y = pose[0][1]
00025     int_marker.pose.position.z = pose[0][2]
00026     int_marker.pose.orientation.x = pose[1][0]
00027     int_marker.pose.orientation.y = pose[1][1]
00028     int_marker.pose.orientation.z = pose[1][2]
00029     int_marker.pose.orientation.w = pose[1][3]
00030     
00031     int_marker.scale = scale
00032     int_marker.name = name
00033     int_marker.description = name
00034     return int_marker
00035 
00036 def make_rviz_marker(scale):
00037     marker = ims.Marker()
00038     marker.type = ims.Marker.SPHERE
00039     marker.scale.x = scale * 0.45
00040     marker.scale.y = scale * 0.45
00041     marker.scale.z = scale * 0.45
00042     marker.color = stdm.ColorRGBA(.5,.5,.5,1)
00043     return marker
00044 
00045 def make_sphere_control(name, scale):
00046     control =  ims.InteractiveMarkerControl()
00047     control.name = name + '_sphere'
00048     control.always_visible = True
00049     control.markers.append(make_rviz_marker(scale))
00050     control.interaction_mode = ims.InteractiveMarkerControl.BUTTON
00051     return control
00052 
00053 def make_control_marker(orientation=[0,0,0,1.]):
00054     control = ims.InteractiveMarkerControl()
00055     control.orientation.x = orientation[0]
00056     control.orientation.y = orientation[1]
00057     control.orientation.z = orientation[2]
00058     control.orientation.w = orientation[3]
00059     control.interaction_mode = ims.InteractiveMarkerControl.MOVE_AXIS
00060     return control
00061 
00062 def make_directional_controls(name, x=True, y=True, z=True):
00063     l = []
00064     
00065     if x:
00066         x_control = make_control_marker()
00067         x_control.orientation.x = 1
00068         x_control.name = name + "_move_x"
00069         l.append(x_control)
00070     
00071     if y:
00072         y_control = make_control_marker()
00073         y_control.orientation.y = 1
00074         y_control.name = name + "_move_y"
00075         l.append(y_control)
00076 
00077     if z:
00078         z_control = make_control_marker()
00079         z_control.orientation.z = 1
00080         z_control.name = name + "_move_z"
00081         l.append(z_control)
00082 
00083     return l
00084 
00085 def make_orientation_controls(name, x=True, y=True, z=True):
00086     controls = make_directional_controls(name + '_rotate', x, y, z)
00087     for c in controls:
00088         c.interaction_mode = ims.InteractiveMarkerControl.ROTATE_AXIS
00089     return controls
00090