$search
00001 #!/usr/bin/env python 00002 # 00003 # Test for the rosie_pr2_gripper_action 00004 # 00005 # Copyright (c) 2010 Alexis Maldonado <maldonado@tum.de> 00006 # Technische Universitaet Muenchen 00007 # 00008 # This program is free software: you can redistribute it and/or modify 00009 # it under the terms of the GNU General Public License as published by 00010 # the Free Software Foundation, either version 3 of the License, or 00011 # (at your option) any later version. 00012 # 00013 # This program is distributed in the hope that it will be useful, 00014 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 # GNU General Public License for more details. 00017 # 00018 # You should have received a copy of the GNU General Public License 00019 # along with this program. If not, see <http://www.gnu.org/licenses/>. 00020 00021 import roslib; roslib.load_manifest('rosie_pr2_gripper_action') 00022 import rospy 00023 00024 from actionlib_msgs.msg import * 00025 from pr2_controllers_msgs.msg import * 00026 import actionlib 00027 00028 00029 def gripper_command(client, position): 00030 goal = Pr2GripperCommandGoal() 00031 goal.command.position = position 00032 goal.command.max_effort = 0.5 00033 client.send_goal(goal) 00034 client.wait_for_result(rospy.Duration(20.0)) #Wait up to 20s for the server 00035 00036 def main(): 00037 rospy.loginfo("test_pr2_gripper_action: Starting up!") 00038 rospy.init_node('test_pr2_gripper_action') 00039 00040 00041 l_client = actionlib.SimpleActionClient('/l_gripper_controller/gripper_action', Pr2GripperCommandAction) 00042 r_client = actionlib.SimpleActionClient('/r_gripper_controller/gripper_action', Pr2GripperCommandAction) 00043 00044 l_client.wait_for_server(rospy.Duration(20.0)) 00045 r_client.wait_for_server(rospy.Duration(20.0)) 00046 00047 print("Left hand:") 00048 print("Closing the fingers.") 00049 gripper_command(l_client, 0.0) 00050 print("Opening the fingers.") 00051 gripper_command(l_client, 0.08) 00052 00053 print("Right hand:") 00054 print("Closing the fingers.") 00055 gripper_command(r_client, 0.0) 00056 print("Opening the fingers.") 00057 gripper_command(r_client, 0.08) 00058 00059 00060 if __name__ == '__main__': 00061 main()