full_controller.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('pr2_precise_trajectory')
00002 import rospy
00003 from pr2_precise_trajectory import *
00004 from pr2_precise_trajectory.arm_controller import *
00005 from pr2_precise_trajectory.gripper_controller import *
00006 from pr2_precise_trajectory.base_controller import *
00007 from pr2_precise_trajectory.head_controller import *
00008 from pr2_precise_trajectory.impact_watcher import *
00009 from pr2_precise_trajectory.joint_watcher import *
00010 from pr2_precise_trajectory.converter import *
00011 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00012 
00013 def transition_split(movements):
00014     first = movements[0]
00015     chunks = [[first]]
00016     last_transition = first.get('transition', 'wait')
00017     
00018     for move in movements[1:]:
00019         transition = move.get('transition', 'wait')
00020         if transition==last_transition and transition=='wait':
00021             chunks[-1].append(move)
00022         else:
00023             last_transition = transition
00024             chunks.append( [move] )
00025     return chunks
00026     
00027 
00028 class FullPr2Controller:
00029     def __init__(self, keys=[LEFT, RIGHT, HEAD, BASE, LEFT_HAND, RIGHT_HAND], impact=True):
00030         self.keys = keys
00031         self.arms = {}
00032 
00033         joint_map = {}
00034         for arm in [LEFT, RIGHT]:
00035             if arm not in keys:
00036                 continue
00037             self.arms[arm] = ArmController(arm)
00038             joint_map[arm] = get_arm_joint_names(arm)
00039 
00040         self.hands = {}
00041         for hand in [LEFT_HAND, RIGHT_HAND]:
00042             if hand not in keys:
00043                 continue
00044             self.hands[hand] = GripperController(hand)
00045             joint_map[hand] = ['%s_gripper_joint'%hand[0]]
00046 
00047         if HEAD in keys:
00048             self.head = HeadController() 
00049             joint_map[HEAD] = ['head_pan_joint', 'head_tilt_joint']
00050         else:
00051             self.head = None
00052 
00053         self.impacts = ImpactWatcher(['%s_gripper_sensor_controller'%arm for arm in self.arms.keys()]) if impact else None
00054         self.joint_watcher = JointWatcher(joint_map)
00055 
00056         if BASE in keys:
00057             self.base = BaseController()
00058             self.joint_watcher.add_tf(self.base.tf)
00059         else:
00060             self.base = None
00061 
00062     def do_action(self, movements):
00063         if len(movements)==0:
00064             return
00065 
00066         chunks = transition_split(movements)
00067 
00068         for ms in chunks:
00069             clients = []
00070             for key in self.keys:
00071                 sub = precise_subset(ms, key)
00072                 if len(sub)==0:
00073                     continue
00074                 if key==BASE:
00075                     seq = simple_to_move_sequence(sub)
00076                     self.base.send_goal(seq)
00077                     clients.append(self.base.client)
00078                 elif key==LEFT_HAND or key==RIGHT_HAND:
00079                     seq = simple_to_gripper_sequence(sub, key)
00080                     self.hands[key].send_goal(seq)
00081                     clients.append(self.hands[key].client)
00082                 else:
00083                     traj = simple_to_message(sub, key)
00084                     if key in self.arms:
00085                         self.arms[key].start_trajectory(traj, wait=False)
00086                         clients.append(self.arms[key].client)
00087                     elif key==HEAD:
00088                         self.head.start_trajectory(traj, wait=False)
00089                         clients.append(self.head.client)
00090 
00091             transition = ms[0].get('transition', 'wait')
00092             if transition=='wait':
00093                 for client in clients:
00094                     client.wait_for_result()
00095             elif transition=='impact':
00096                 rospy.sleep(.1)
00097                 self.impacts.wait_for_impact()
00098                 self.stop_arm()
00099 
00100     def stop_arm(self, time=0.1):
00101         for arm in self.arms:
00102             trajectory = simple_to_message_single(self.joint_watcher.get_positions(arm), time, arm)
00103             self.arms[arm].start_trajectory(trajectory, wait=False)
00104         rospy.sleep(time)
00105 
00106 


pr2_precise_trajectory
Author(s): David V. Lu!!
autogenerated on Sat Jun 8 2019 20:59:18