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_doors_gazebo_demo'
00037 NAME = 'test_door_no_executive'
00038
00039 import roslib
00040 roslib.load_manifest(PKG)
00041
00042 import unittest, sys, os, math
00043 import time
00044 import rospy, rostest
00045 from nav_msgs.msg import *
00046 from std_msgs.msg import String
00047 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00048 from door_msgs.msg import Door, DoorAction, DoorGoal
00049 from actionlib import *
00050
00051 TEST_DURATION = 60000.0
00052
00053
00054 class TestDoorNoExecutive(unittest.TestCase):
00055 def __init__(self, *args):
00056 super(TestDoorNoExecutive, self).__init__(*args)
00057
00058
00059 prior_door = Door()
00060 prior_door.frame_p1.x = 1.0
00061 prior_door.frame_p1.y = -0.5
00062 prior_door.frame_p2.x = 1.0
00063 prior_door.frame_p2.y = 0.5
00064 prior_door.door_p1.x = 1.0
00065 prior_door.door_p1.y = -0.5
00066 prior_door.door_p2.x = 1.0
00067 prior_door.door_p2.y = 0.5
00068 prior_door.travel_dir.x = 1.0
00069 prior_door.travel_dir.y = 0.0
00070 prior_door.travel_dir.z = 0.0
00071 prior_door.rot_dir = Door.ROT_DIR_COUNTERCLOCKWISE
00072 prior_door.hinge = Door.HINGE_P2
00073 prior_door.header.frame_id = "base_footprint"
00074 self.door = DoorGoal()
00075 self.door.door = prior_door
00076
00077 self.move = MoveBaseGoal()
00078 self.move.target_pose.header.frame_id = 'odom_combined'
00079 self.move.target_pose.pose.position.x = 10
00080 self.move.target_pose.pose.position.y = 10
00081 self.move.target_pose.pose.orientation.w = 1
00082
00083 self.ac_door = SimpleActionClient('move_through_door', DoorAction)
00084 print 'Waiting for open door action server'
00085 self.ac_door.wait_for_server()
00086
00087 self.ac_move = SimpleActionClient('move_base_local', MoveBaseAction)
00088 print 'Waiting for move base action server'
00089 self.ac_move.wait_for_server()
00090
00091 rospy.Subscriber("/test_output", String, self.stringOutput)
00092
00093
00094 def stringOutput(self, str):
00095 print str.data
00096
00097
00098 def test_door_no_executive(self):
00099 self.assert_(self.ac_door.send_goal_and_wait(self.door, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))
00100 self.assert_(self.ac_move.send_goal_and_wait(self.move, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))
00101
00102
00103
00104 if __name__ == '__main__':
00105 rospy.init_node('open_door_test', anonymous=True)
00106 rostest.run(PKG, sys.argv[0], TestDoorNoExecutive, sys.argv)
00107