tuck_arms_intermediary.py
Go to the documentation of this file.
00001 #!/usr/bin/python
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)


assistive_teleop
Author(s): Phillip M. Grice, Advisor: Prof. Charlie Kemp, Lab: The Healthcare Robotoics Lab at Georgia Tech.
autogenerated on Wed Nov 27 2013 11:35:34