Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('simulation_object_tracker')
00004 import rospy
00005 import sys, traceback
00006 from simulation_object_tracker.object_tracker import ObjectTracker
00007 from geometry_msgs import msg as geometry
00008 from tidyup_msgs import srv
00009 from tidyup_msgs import msg
00010 from pr2_python import hand_description
00011 from pr2_python import arm_mover
00012 from tidyup_arm_services.simple_arm_tasks import SimpleArmTasks
00013
00014 if __name__ == '__main__':
00015 try:
00016 rospy.init_node('simulation_object_tracker')
00017 arm_tasks = SimpleArmTasks()
00018 objectTracker = ObjectTracker()
00019 objectTracker.create_objects(dataFileName = '../configs/simulated_objects.dat')
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 rospy.loginfo('starting test.')
00033 side_request = msg.ArmToSideGoal()
00034 side_request.right_arm = True
00035 arm_tasks._to_side_execute(side_request)
00036 side_request.right_arm = False
00037 side_request.left_arm = True
00038 arm_tasks._to_side_execute(side_request)
00039 response = objectTracker._detect_graspables_callback(srv.DetectGraspableObjects())
00040 print response['objects']
00041 graspability_request = srv.RequestObjectsGraspabilityRequest()
00042 graspability_request.objects = response['objects']
00043 graspability_response = objectTracker._request_graspability_callback(graspability_request)
00044 print graspability_response['objects']
00045 grasp_request = msg.GraspObjectGoal()
00046 for object in response['objects']:
00047 if object.reachable_right_arm:
00048 grasp_request.target = object
00049 grasp_request.right_arm = True
00050 break
00051 elif object.reachable_left_arm:
00052 grasp_request.target = object
00053 grasp_request.left_arm = True
00054 break
00055 if not grasp_request.right_arm and not grasp_request.left_arm:
00056 rospy.logwarn('no objects in range, aborting test.')
00057 sys.exit(1)
00058 objectTracker._pick_up_execute(grasp_request)
00059 carry_request = msg.PostGraspPositionGoal()
00060 carry_request.right_arm = True
00061 arm_tasks._to_carry_execute(carry_request)
00062 putdown_request = msg.PlaceObjectGoal()
00063 putdown_request.right_arm = True
00064 putdown_request.position = grasp_request.target.pose
00065 putdown_request.position.pose.position.z += 0.05
00066 objectTracker._put_down_execute(putdown_request)
00067 side_request = msg.ArmToSideGoal()
00068 side_request.right_arm = True
00069 arm_tasks._to_side_execute(side_request)
00070 except:
00071 traceback.print_exc(file=sys.stdout)