Go to the documentation of this file.00001 import rospy
00002 import actionlib
00003 from joy_pose_6d import JoyPose6D
00004 from std_msgs.msg import String, Empty
00005 from geometry_msgs.msg import PoseStamped
00006 import xml.etree.ElementTree as ET
00007 from jsk_interactive_marker.msg import MarkerMenu
00008 from jsk_rviz_plugins.msg import OverlayMenu
00009
00010 class EndEffector(JoyPose6D):
00011 mode = 0
00012 MENU_MODE = 1
00013 JOY_MODE = 2
00014 def __init__(self, name, args):
00015 JoyPose6D.__init__(self, name, args)
00016 self.support_follow_view = True
00017 self.frame_id = self.getArg('frame_id', 'base_link')
00018 self.marker_menu_pub = rospy.Publisher(self.getArg('marker_menu',
00019 'marker_menu'),
00020 MarkerMenu)
00021 self.menu_pub = rospy.Publisher("/overlay_menu", OverlayMenu)
00022 self.menus = ['LARM', 'RARM',
00023 'close hand', 'open hand', 'toggle ik rotation']
00024 self.mode = self.JOY_MODE
00025 self.current_index = 0
00026 def publishMenu(self, index, close=False):
00027 menu = OverlayMenu()
00028 menu.menus = self.menus
00029 menu.current_index = index
00030 menu.title = "JSK teleop menu"
00031 if close:
00032 menu.action = OverlayMenu.ACTION_CLOSE
00033 self.menu_pub.publish(menu)
00034 def publishMarkerMenu(self, menu):
00035 menu_msg = MarkerMenu()
00036 menu_msg.menu = menu
00037 self.marker_menu_pub.publish(menu_msg)
00038 def joyCB(self, status, history):
00039 if history.length() > 0:
00040 latest = history.latest()
00041 else:
00042 return
00043
00044 if self.mode == self.MENU_MODE:
00045 if history.new(status, "triangle") or history.new(status, "cross"):
00046 self.mode = self.JOY_MODE
00047 self.publishMenu(self.current_index, True)
00048 elif history.new(status, "up") or history.new(status, "left_analog_up"):
00049 self.current_index = self.current_index - 1
00050 if self.current_index < 0:
00051 self.current_index = len(self.menus) - 1
00052 self.publishMenu(self.current_index)
00053 elif history.new(status, "down") or history.new(status, "left_analog_down"):
00054 self.current_index = self.current_index + 1
00055 if self.current_index >= len(self.menus):
00056 self.current_index = 0
00057 self.publishMenu(self.current_index)
00058 elif history.new(status, "circle"):
00059 if self.menus[self.current_index] == "ARMS":
00060 self.publishMarkerMenu(MarkerMenu.SET_MOVE_ARMS)
00061 elif self.menus[self.current_index] == "RARM":
00062 self.publishMarkerMenu(MarkerMenu.SET_MOVE_RARM)
00063 elif self.menus[self.current_index] == "LARM":
00064 self.publishMarkerMenu(MarkerMenu.SET_MOVE_LARM)
00065 elif self.menus[self.current_index] == "close hand":
00066 self.publishMarkerMenu(MarkerMenu.START_GRASP)
00067 elif self.menus[self.current_index] == "open hand":
00068 self.publishMarkerMenu(MarkerMenu.STOP_GRASP)
00069 elif self.menus[self.current_index] == "toggle ik rotation":
00070 self.publishMarkerMenu(MarkerMenu.IK_ROTATION_AXIS_T)
00071 self.publishMenu(self.current_index, True)
00072 self.mode = self.JOY_MODE
00073 else:
00074 self.publishMenu(self.current_index)
00075 elif self.mode == self.JOY_MODE:
00076 if history.new(status, "triangle"):
00077 self.mode = self.MENU_MODE
00078 elif history.new(status, "circle"):
00079 self.publishMarkerMenu(MarkerMenu.MOVE)
00080 elif history.new(status, "start"):
00081 self.publishMarkerMenu(MarkerMenu.PLAN)
00082 elif history.new(status, "select"):
00083 self.publishMarkerMenu(MarkerMenu.RESET_JOINT)
00084 else:
00085 JoyPose6D.joyCB(self, status, history)