jsk_interactive_marker_control.py
Go to the documentation of this file.
1 import rospy
2 import actionlib
3 from jsk_teleop_joy.joy_pose_6d import JoyPose6D
4 from std_msgs.msg import String, Empty
5 from geometry_msgs.msg import PoseStamped
6 import xml.etree.ElementTree as ET
7 from jsk_interactive_marker.msg import MarkerMenu
8 from jsk_rviz_plugins.msg import OverlayMenu
9 
10 class EndEffector(JoyPose6D):
11  mode = 0
12  MENU_MODE = 1
13  JOY_MODE = 2
14  def __init__(self, name, args):
15  JoyPose6D.__init__(self, name, args)
16  self.support_follow_view = True
17  self.frame_id = self.getArg('frame_id', 'base_link')
18  self.marker_menu_pub = rospy.Publisher(self.getArg('marker_menu',
19  'marker_menu'),
20  MarkerMenu)
21  self.menu_pub = rospy.Publisher("/overlay_menu", OverlayMenu)
22  self.menus = ['LARM', 'RARM',
23  'close hand', 'open hand', 'toggle ik rotation']
24  self.mode = self.JOY_MODE
25  self.current_index = 0
26  def publishMenu(self, index, close=False):
27  menu = OverlayMenu()
28  menu.menus = self.menus
29  menu.current_index = index
30  menu.title = "JSK teleop menu"
31  if close:
32  menu.action = OverlayMenu.ACTION_CLOSE
33  self.menu_pub.publish(menu)
34  def publishMarkerMenu(self, menu):
35  menu_msg = MarkerMenu()
36  menu_msg.menu = menu
37  self.marker_menu_pub.publish(menu_msg)
38  def joyCB(self, status, history):
39  if history.length() > 0:
40  latest = history.latest()
41  else:
42  return
43 
44  if self.mode == self.MENU_MODE:
45  if history.new(status, "triangle") or history.new(status, "cross"):
46  self.mode = self.JOY_MODE
47  self.publishMenu(self.current_index, True)
48  elif history.new(status, "up") or history.new(status, "left_analog_up"):
49  self.current_index = self.current_index - 1
50  if self.current_index < 0:
51  self.current_index = len(self.menus) - 1
52  self.publishMenu(self.current_index)
53  elif history.new(status, "down") or history.new(status, "left_analog_down"):
54  self.current_index = self.current_index + 1
55  if self.current_index >= len(self.menus):
56  self.current_index = 0
57  self.publishMenu(self.current_index)
58  elif history.new(status, "circle"):
59  if self.menus[self.current_index] == "ARMS":
60  self.publishMarkerMenu(MarkerMenu.SET_MOVE_ARMS)
61  elif self.menus[self.current_index] == "RARM":
62  self.publishMarkerMenu(MarkerMenu.SET_MOVE_RARM)
63  elif self.menus[self.current_index] == "LARM":
64  self.publishMarkerMenu(MarkerMenu.SET_MOVE_LARM)
65  elif self.menus[self.current_index] == "close hand":
66  self.publishMarkerMenu(MarkerMenu.START_GRASP)
67  elif self.menus[self.current_index] == "open hand":
68  self.publishMarkerMenu(MarkerMenu.STOP_GRASP)
69  elif self.menus[self.current_index] == "toggle ik rotation":
70  self.publishMarkerMenu(MarkerMenu.IK_ROTATION_AXIS_T)
71  self.publishMenu(self.current_index, True)
72  self.mode = self.JOY_MODE
73  else:
74  self.publishMenu(self.current_index)
75  elif self.mode == self.JOY_MODE:
76  if history.new(status, "triangle"):
77  self.mode = self.MENU_MODE
78  elif history.new(status, "circle"):
79  self.publishMarkerMenu(MarkerMenu.MOVE)
80  elif history.new(status, "start"):
81  self.publishMarkerMenu(MarkerMenu.PLAN)
82  elif history.new(status, "select"):
83  self.publishMarkerMenu(MarkerMenu.RESET_JOINT)
84  else:
85  JoyPose6D.joyCB(self, status, history)
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.MENU_MODE
int MENU_MODE
Definition: jsk_interactive_marker_control.py:12
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.JOY_MODE
int JOY_MODE
Definition: jsk_interactive_marker_control.py:13
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.support_follow_view
support_follow_view
Definition: jsk_interactive_marker_control.py:16
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.joyCB
def joyCB(self, status, history)
Definition: jsk_interactive_marker_control.py:38
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.mode
int mode
Definition: jsk_interactive_marker_control.py:11
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.marker_menu_pub
marker_menu_pub
Definition: jsk_interactive_marker_control.py:18
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.frame_id
frame_id
Definition: jsk_interactive_marker_control.py:17
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.current_index
current_index
Definition: jsk_interactive_marker_control.py:25
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.publishMarkerMenu
def publishMarkerMenu(self, menu)
Definition: jsk_interactive_marker_control.py:34
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.menus
menus
Definition: jsk_interactive_marker_control.py:22
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector
Definition: jsk_interactive_marker_control.py:10
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.__init__
def __init__(self, name, args)
Definition: jsk_interactive_marker_control.py:14
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.publishMenu
def publishMenu(self, index, close=False)
Definition: jsk_interactive_marker_control.py:26
jsk_teleop_joy.plugin.jsk_interactive_marker_control.EndEffector.menu_pub
menu_pub
Definition: jsk_interactive_marker_control.py:21


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:49