simulation_object_tracker: test

Go to the documentation of this file.
00001 #! /usr/bin/env python
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 simulation_object_tracker.object_tracker import SimulatedDoor
00008 from geometry_msgs import msg as geometry
00009 from tidyup_msgs import srv
00010 from tidyup_msgs import msg
00011 from tidyup_arm_services.simple_arm_tasks import SimpleArmTasks
00012 
00013 if __name__ == '__main__':
00014   try:
00015     rospy.init_node('simulation_object_tracker')
00016     arm_tasks = SimpleArmTasks()
00017     objectTracker = ObjectTracker()
00018     objectTracker.create_objects(dataFileName = '../configs/simulated_objects.dat')
00019     
00020     # TEST
00021     arm_tasks._tasks.move_arms_to_side()
00022     rospy.loginfo('starting test.')
00023     side_request = msg.ArmToSideGoal()
00024     side_request.right_arm = True
00025     objectTracker._to_side_execute(side_request)
00026     side_request.right_arm = False
00027     side_request.left_arm = True
00028     objectTracker._to_side_execute(side_request)
00029     door_state_reesponse = objectTracker._detect_door_state_callback(srv.DetectDoorStateRequest())
00030     print door_state_reesponse
00031     open_door_request = msg.OpenDoorGoal()
00032     open_door_request.right_arm = True
00033     objectTracker._open_door_execute(open_door_request)
00034     door_state_reesponse = objectTracker._detect_door_state_callback(srv.DetectDoorStateRequest())
00035     print door_state_reesponse
00036     rospy.loginfo('test done.')
00037   except:
00038     traceback.print_exc(file=sys.stdout)
00039   rospy.spin()