Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 import roslib
00040 roslib.load_manifest('pr2_pick_and_place_demos')
00041 import rospy
00042 import actionlib
00043 from pr2_pick_and_place_demos.pick_and_place_demo import *
00044 from pr2_controllers_msgs.msg import SingleJointPositionAction, SingleJointPositionGoal
00045
00046 if __name__ == '__main__':
00047
00048 rospy.init_node('pick_and_place_demo', anonymous=False)
00049
00050 use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_controller', 0)
00051 use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_detection', 0)
00052 just_start = rospy.get_param("~just_start", True)
00053
00054 pick_and_place_demo = PickAndPlaceDemo(use_slip_controller = use_slip_controller, \
00055 use_slip_detection = use_slip_detection)
00056 if just_start:
00057
00058 torso_action_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction);
00059 rospy.loginfo("run_pick_and_place_demo: waiting for torso action server")
00060 torso_action_client.wait_for_server()
00061 rospy.loginfo("run_pick_and_place_demo: torso action server found")
00062 goal = SingleJointPositionGoal()
00063 goal.position = 0.295
00064 rospy.loginfo("sending command to lift torso")
00065 torso_action_client.send_goal(goal)
00066 torso_action_client.wait_for_result(rospy.Duration(30))
00067
00068
00069 rospy.loginfo("starting pick and place demo")
00070 pick_and_place_demo.start_autonomous_thread(False)
00071 rospy.spin()
00072 else:
00073 rospy.loginfo("starting pick and place keyboard interface")
00074 pick_and_place_demo.keyboard_interface()