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 PKG = 'pr2_tabletop_manipulation_gazebo_demo'
00037
00038 import roslib
00039 roslib.load_manifest(PKG)
00040
00041 import unittest, sys, os, math
00042 import time
00043 import rospy, rostest
00044 from nav_msgs.msg import *
00045 from std_msgs.msg import String
00046 from pr2_controllers_msgs.msg import PointHeadGoal, PointHeadAction, SingleJointPositionAction, SingleJointPositionActionGoal
00047 from pr2_common_action_msgs.msg import TuckArmsGoal, TuckArmsAction
00048 from actionlib import *
00049
00050
00051 from pr2_pick_and_place_demos.pick_and_place_manager import *
00052 from object_manipulator.convert_functions import *
00053 from pr2_tabletop_manipulation_gazebo_demo import simple_pick_and_place_example
00054
00055 from gazebo.msg import *
00056 from gazebo.srv import *
00057
00058 TEST_DURATION = 60000.0
00059 TARGET_POSE_X = 0.6
00060 TARGET_POSE_Y = -0.15
00061
00062 class TestTabletopManipulationNoExecutive(unittest.TestCase):
00063 def __init__(self, *args):
00064 super(TestTabletopManipulationNoExecutive, self).__init__(*args)
00065
00066
00067 def stringOutput(self, str):
00068 print str.data
00069
00070
00071 def test_tabletop_manipulation_no_executive(self):
00072
00073
00074
00075
00076
00077
00078 print 'Waiting for point head action server'
00079 point_head_ac = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
00080 point_head_ac.wait_for_server()
00081 head_goal = PointHeadGoal()
00082 head_goal.target.header.frame_id = 'base_link'
00083 head_goal.target.point.x = 1.0
00084 head_goal.target.point.y = 0.0
00085 head_goal.target.point.z = 0.25
00086 head_goal.max_velocity = 0.1
00087 self.assert_(point_head_ac.send_goal_and_wait(head_goal, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098 sppe = simple_pick_and_place_example.SimplePickAndPlaceExample()
00099
00100
00101 table_height = .505
00102
00103 target_point_xyz = [.7, 0, table_height]
00104 target_point = create_point_stamped(target_point_xyz, '/base_link')
00105 success = sppe.pick_up_object_near_point(target_point, 0)
00106 self.assert_(success)
00107 if success:
00108
00109 place_rect_dims = [.06, .06]
00110
00111 center_xyz = [TARGET_POSE_X, TARGET_POSE_Y, table_height]
00112
00113 center_quat = [0,0,0,1]
00114 place_rect_center = create_pose_stamped(center_xyz+center_quat, '/base_link')
00115 self.assert_(sppe.place_object(0, place_rect_dims, place_rect_center))
00116
00117
00118 print 'Checking object pose to see if it is near goal'
00119 rospy.wait_for_service('/gazebo/get_model_state')
00120
00121 get_model_state = rospy.ServiceProxy('/gazebo/get_model_state',GetModelState)
00122 resp = get_model_state('object_1','pr2::base_link')
00123 print 'check object_1 pose status: ',resp
00124 self.assert_(abs(resp.pose.position.x - TARGET_POSE_X) < 0.06 and \
00125 abs(resp.pose.position.y - TARGET_POSE_Y) < 0.06 and \
00126 abs(resp.pose.position.z - table_height) < 0.06)
00127
00128
00129 if __name__ == '__main__':
00130 rospy.init_node('tabletop_manipulation_test', anonymous=True)
00131
00132
00133 time.sleep(50.0)
00134
00135 rostest.run(PKG, sys.argv[0], TestTabletopManipulationNoExecutive, sys.argv)
00136