Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('assistive_teleop')
00004 import rospy
00005 import actionlib
00006 from geometry_msgs.msg import PoseStamped
00007 from std_msgs.msg import String, Bool
00008 from trajectory_msgs.msg import JointTrajectoryPoint
00009 from pr2_controllers_msgs.msg import JointTrajectoryControllerState, JointTrajectoryActionGoal
00010 from pr2_common_action_msgs.msg import TuckArmsAction, TuckArmsGoal
00011
00012
00013 class TuckArmsIntermediary():
00014
00015 def __init__(self):
00016 rospy.init_node('tuck_arms_intermediary')
00017 self.tuck = actionlib.SimpleActionClient('tuck_arms', TuckArmsAction)
00018 rospy.Subscriber('r_arm_controller/state', JointTrajectoryControllerState , self.set_joint_state_r)
00019 rospy.Subscriber('l_arm_controller/state', JointTrajectoryControllerState , self.set_joint_state_l)
00020 rospy.Subscriber('wt_tuck_arms', String, self.tuck_arm)
00021 self.joints_out_r = rospy.Publisher('r_arm_controller/joint_trajectory_action/goal', JointTrajectoryActionGoal )
00022 self.joints_out_l = rospy.Publisher('l_arm_controller/joint_trajectory_action/goal', JointTrajectoryActionGoal )
00023 self.wt_log_out = rospy.Publisher('wt_log_out', String )
00024
00025 print "Waiting for tuck_arms server"
00026 self.tuck.wait_for_server()
00027 print "Tuck arms server found"
00028
00029 def set_joint_state_r(self,msg):
00030 self.joint_state_r = msg
00031
00032 def set_joint_state_l(self,msg):
00033 self.joint_state_l = msg
00034
00035 def tuck_arm(self, msg):
00036 tuck_goal = TuckArmsGoal()
00037
00038 if (msg.data == 'right' or msg.data == 'left'):
00039 recover = True
00040 recover_goal = JointTrajectoryActionGoal()
00041 else:
00042 recover = False
00043
00044 if msg.data == 'right':
00045 tuck_goal.tuck_left = False
00046 tuck_goal.tuck_right = True
00047 print "Tucking Right Arm Only"
00048 self.wt_log_out.publish(data="Tucking Right Arm Only")
00049
00050 recover_goal.goal.trajectory.joint_names = self.joint_state_l.joint_names
00051 recover_position = self.joint_state_l.actual
00052 recover_position.time_from_start = rospy.Duration(5)
00053 recover_publisher = self.joints_out_l
00054
00055 elif msg.data == 'left':
00056 tuck_goal.tuck_left = True
00057 tuck_goal.tuck_right = False
00058 print "Tucking Left Arm Only"
00059 self.wt_log_out.publish(data="Tucking Left Arm Only")
00060
00061 recover_goal.goal.trajectory.joint_names = self.joint_state_r.joint_names
00062 recover_position = self.joint_state_r.actual
00063 recover_position.time_from_start = rospy.Duration(5)
00064 recover_publisher = self.joints_out_r
00065 elif msg.data == 'both':
00066 tuck_goal.tuck_left = True
00067 tuck_goal.tuck_right = True
00068 print "Tucking Both Arms"
00069 self.wt_log_out.publish(data="Tucking Both Arms")
00070
00071 else:
00072 print "Bad input to Tuck Arm Intermediary"
00073
00074
00075 finished_within_time = False
00076 self.tuck.send_goal(tuck_goal)
00077 finished_within_time = self.tuck.wait_for_result(rospy.Duration(30))
00078 if not (finished_within_time):
00079 self.tuck.cancel_goal()
00080 self.wt_log_out.publish(data="Timed out tucking right arm")
00081 rospy.loginfo("Timed out tucking right arm")
00082 else:
00083 state = self.tuck.get_state()
00084 success = (state == 'SUCCEEDED')
00085 if (success):
00086 rospy.loginfo("Action Finished: %s" %state)
00087 self.wt_log_out.publish(data="Tuck Right Arm: Succeeded")
00088 else:
00089 rospy.loginfo("Action failed: %s" %state)
00090 self.wt_log_out.publish(data="Tuck Right Arm: Failed: %s" %state)
00091
00092 if (recover):
00093 recover_goal.goal.trajectory.points.append(recover_position)
00094 recover_publisher.publish(recover_goal);
00095 print "Sending Recovery Goal to restore initial position of non-tucked arm"
00096 self.wt_log_out.publish(data="Sending Recovery Goal to restore initial position of non-tucked arm")
00097
00098 if __name__ == '__main__':
00099 TAI = TuckArmsIntermediary()
00100
00101 while not rospy.is_shutdown():
00102 rospy.sleep(0.1)