moveit.py
Go to the documentation of this file.
00001 import rospy
00002 from joy_pose_6d import JoyPose6D
00003 
00004 import imp
00005 try:
00006   imp.find_module("actionlib")
00007 except:
00008   import roslib; roslib.load_manifest('jsk_teleop_joy')
00009 
00010 import actionlib
00011 from std_msgs.msg import String, Empty
00012 import xml.etree.ElementTree as ET
00013 
00014 class JoyMoveIt(JoyPose6D):
00015   def __init__(self, name, args):
00016     JoyPose6D.__init__(self, name, args)
00017     self.supportFollowView(True)
00018     self.frame_id = self.getArg('frame_id', '/map')
00019     self.planning_group = self.getArg('planning_group')
00020     self.plan_group_pub = rospy.Publisher('/rviz/moveit/select_planning_group', String)
00021     self.plan_pub = rospy.Publisher("/rviz/moveit/plan", Empty)
00022     self.execute_pub = rospy.Publisher("/rviz/moveit/execute", Empty)
00023     self.update_start_state_pub = rospy.Publisher("/rviz/moveit/update_start_state", Empty)
00024     self.update_goal_state_pub = rospy.Publisher("/rviz/moveit/update_goal_state", Empty)
00025     self.counter = 0
00026   def enable(self):
00027     self.plan_group_pub.publish(self.planning_group)
00028   def joyCB(self, status, history):
00029     JoyPose6D.joyCB(self, status, history)
00030     latest = history.latest()
00031     if not latest:
00032       return
00033     if status.triangle and not latest.triangle:
00034       # self.current_planning_group_index = self.current_planning_group_index + 1
00035       # if self.current_planning_group_index >= len(self.planning_groups):
00036       #   self.current_planning_group_index = 0
00037       # self.plan_group_pub.publish(self.planning_groups[self.current_planning_group_index])
00038       self.update_goal_state_pub.publish(Empty())
00039     elif status.cross and not latest.cross:
00040       # self.current_planning_group_index = self.current_planning_group_index - 1
00041       # if self.current_planning_group_index < 0:
00042       #   self.current_planning_group_index = len(self.planning_groups) - 1
00043       # self.plan_group_pub.publish(self.planning_groups[self.current_planning_group_index])
00044       pass
00045     elif status.square and not latest.square:
00046       self.plan_pub.publish(Empty())
00047     elif status.circle and not latest.circle:
00048       self.execute_pub.publish(Empty())
00049     self.counter = self.counter + 1
00050     if self.counter % 10:
00051       self.update_start_state_pub.publish(Empty())


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Wed Sep 16 2015 04:37:30