moveit.py
Go to the documentation of this file.
1 import rospy
2 from jsk_teleop_joy.joy_pose_6d import JoyPose6D
3 
4 import imp
5 try:
6  imp.find_module("actionlib")
7 except:
8  import roslib; roslib.load_manifest('jsk_teleop_joy')
9 
10 import actionlib
11 from std_msgs.msg import String, Empty
12 import xml.etree.ElementTree as ET
13 
14 class JoyMoveIt(JoyPose6D):
15  def __init__(self, name, args):
16  JoyPose6D.__init__(self, name, args)
17  self.supportFollowView(True)
18  self.frame_id = self.getArg('frame_id', '/map')
19  self.planning_group = self.getArg('planning_group')
20  self.plan_group_pub = rospy.Publisher('/rviz/moveit/select_planning_group', String)
21  self.plan_pub = rospy.Publisher("/rviz/moveit/plan", Empty)
22  self.execute_pub = rospy.Publisher("/rviz/moveit/execute", Empty)
23  self.update_start_state_pub = rospy.Publisher("/rviz/moveit/update_start_state", Empty)
24  self.update_goal_state_pub = rospy.Publisher("/rviz/moveit/update_goal_state", Empty)
25  self.counter = 0
26  def enable(self):
27  self.plan_group_pub.publish(self.planning_group)
28  def joyCB(self, status, history):
29  JoyPose6D.joyCB(self, status, history)
30  latest = history.latest()
31  if not latest:
32  return
33  if status.triangle and not latest.triangle:
34  # self.current_planning_group_index = self.current_planning_group_index + 1
35  # if self.current_planning_group_index >= len(self.planning_groups):
36  # self.current_planning_group_index = 0
37  # self.plan_group_pub.publish(self.planning_groups[self.current_planning_group_index])
38  self.update_goal_state_pub.publish(Empty())
39  elif status.cross and not latest.cross:
40  # self.current_planning_group_index = self.current_planning_group_index - 1
41  # if self.current_planning_group_index < 0:
42  # self.current_planning_group_index = len(self.planning_groups) - 1
43  # self.plan_group_pub.publish(self.planning_groups[self.current_planning_group_index])
44  pass
45  elif status.square and not latest.square:
46  self.plan_pub.publish(Empty())
47  elif status.circle and not latest.circle:
48  self.execute_pub.publish(Empty())
49  self.counter = self.counter + 1
50  if self.counter % 10:
51  self.update_start_state_pub.publish(Empty())
def joyCB(self, status, history)
Definition: moveit.py:28
def __init__(self, name, args)
Definition: moveit.py:15


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Sun May 28 2023 03:03:37