rosie_pr2_gripper_action.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # PR2-Gripper Action Server for Rosie (using the armhand action server)
00004 # Mimics the PR2 gripper interface
00005 #
00006 # Copyright (c) 2010 Alexis Maldonado <maldonado@tum.de>
00007 # Technische Universitaet Muenchen
00008 #
00009 # This program is free software: you can redistribute it and/or modify
00010 # it under the terms of the GNU General Public License as published by
00011 # the Free Software Foundation, either version 3 of the License, or
00012 # (at your option) any later version.
00013 #
00014 # This program is distributed in the hope that it will be useful,
00015 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017 # GNU General Public License for more details.
00018 #
00019 # You should have received a copy of the GNU General Public License
00020 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
00021 
00022 import roslib; roslib.load_manifest('rosie_pr2_gripper_action')
00023 import rospy
00024 
00025 from actionlib_msgs.msg import *
00026 from pr2_controllers_msgs.msg import *
00027 import actionlib
00028 import cogman_msgs.msg
00029 
00030 
00031 
00032 class RosiePR2GripperActionServer:
00033     def __init__(self):
00034 
00035         self.threshold = 0.05
00036         rospy.loginfo('open/close threshold = %5.3f' % (self.threshold))
00037         
00038         self.l_action_server = actionlib.simple_action_server.SimpleActionServer('/l_gripper_controller/gripper_action',Pr2GripperCommandAction, self.l_executeCB)
00039         self.r_action_server = actionlib.simple_action_server.SimpleActionServer('/r_gripper_controller/gripper_action',Pr2GripperCommandAction, self.r_executeCB)
00040 
00041         self.l_action_client = actionlib.SimpleActionClient('left_arm', cogman_msgs.msg.ArmHandAction)
00042         self.r_action_client = actionlib.SimpleActionClient('right_arm', cogman_msgs.msg.ArmHandAction)
00043 
00044         self.l_action_client.wait_for_server(rospy.Duration(20.0))
00045         self.r_action_client.wait_for_server(rospy.Duration(20.0))
00046         
00047         #Put the hands in a known state
00048         self.hand_primitive(self.l_action_client,'open_relax')
00049         self.hand_primitive(self.r_action_client,'open_relax')
00050         
00051         
00052     def r_executeCB(self, goal):
00053         self.executeCB(goal, 'right')
00054         
00055     def l_executeCB(self, goal):
00056         self.executeCB(goal, 'left')
00057         
00058     def executeCB(self, goal, side='right'):
00059         if side == 'left':
00060             client = self.l_action_client
00061             action_server = self.l_action_server
00062         elif side =='right':
00063             client = self.r_action_client
00064             action_server = self.r_action_server
00065         else:
00066             rospy.logerr('Wrong side specified, expected left/right')
00067             return(None)
00068 
00069         # position == 0.08 -> Full open  0.0 -> Closed
00070         if goal.command.position < self.threshold:
00071             self.hand_primitive(client, '3pinch')
00072         else:
00073             self.hand_primitive(client, 'open_relax')
00074         
00075         client.wait_for_result(rospy.Duration(20.0))  #Wait up to 20s for the server
00076             
00077         result = Pr2GripperCommandResult()
00078         # float64 position
00079         # float64 effort
00080         # bool stalled
00081         # bool reached_goal
00082         
00083         self.success = True  #FIXME: Check for success from the armhand server
00084         if self.success:
00085             result.position = goal.command.position  # Give the same position as desired
00086             result.effort = 0.5  # Some non-zero value
00087             result.stalled = False
00088             result.reached_goal = True
00089             action_server.set_succeeded(result)
00090         else:
00091             result.reached_goal = False
00092             rospy.logerr("PR2 gripper action FAILED")
00093             self.action_server.set_aborted(result)
00094     
00095     def hand_primitive(self, client, primitive_name):
00096         goal = cogman_msgs.msg.ArmHandGoal()
00097         goal.command = 'hand_primitive'
00098         goal.hand_primitive = primitive_name
00099         client.send_goal(goal)
00100         
00101         
00102 def main():
00103     rospy.loginfo("rosie_pr2_gripper_action: Starting up!")
00104     rospy.init_node('rosie_pr2_gripper_action')
00105     tuck_arms_action_server = RosiePR2GripperActionServer()
00106     rospy.spin()
00107 
00108 if __name__ == '__main__':
00109     main()


rosie_pr2_gripper_action
Author(s): Alexis Maldonado
autogenerated on Mon Oct 6 2014 09:02:25