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 PKG = 'door_handle_detector'
00039
00040 import roslib; roslib.load_manifest(PKG)
00041
00042 import sys
00043 import os
00044 import string
00045 from rosrecord import *
00046 import rospy
00047
00048 import rostest
00049 import unittest
00050 import math
00051
00052
00053 from door_handle_detector.srv import *
00054 from door_msgs.msg import *
00055
00056 class TestDoorHandleDetector(unittest.TestCase):
00057 def setUp(self):
00058 rospy.init_node("handle_tester")
00059
00060
00061 self.epsilon = 0.1
00062
00063
00064 def handle_dist(self, handle_laser, handle_camera):
00065 dx = handle_laser.doors[0].handle.x - handle_camera.doors[0].handle.x
00066 dy = handle_laser.doors[0].handle.y - handle_camera.doors[0].handle.y
00067 dz = handle_laser.doors[0].handle.z - handle_camera.doors[0].handle.z
00068 return math.sqrt(dx*dx + dy*dy + dz*dz)
00069
00070
00071 def test_door_handle_detector(self):
00072 rospy.sleep(2.0)
00073 d = Door()
00074 d.frame_p1.x = 1.0
00075 d.frame_p1.y = -0.5
00076 d.frame_p2.x = 1.0
00077 d.frame_p2.y = 0.5
00078 d.hinge = d.HINGE_P2
00079 d.rot_dir = d.ROT_DIR_COUNTERCLOCKWISE
00080 d.header.frame_id = "base_footprint"
00081 d.header.stamp = rospy.get_rostime()
00082 print "time ",d.header.stamp
00083
00084 door = self.detect_door_laser(d)
00085 handle_camera = self.detect_handle_camera(door.doors[0])
00086
00087 cnt = 0
00088
00089 dist = self.epsilon + 1
00090
00091 while cnt<5 and dist>self.epsilon:
00092 handle_laser = self.detect_handle_laser(door.doors[0])
00093 dist = self.handle_dist(handle_laser,handle_camera)
00094 cnt += 1
00095
00096
00097 print 'Difference between camera and laser: %9.6f'%(dist)
00098
00099 self.assertTrue(dist < self.epsilon)
00100
00101 def detect_door_laser(self, door_request):
00102
00103 print "Waiting for service...", rospy.resolve_name('doors_detector')
00104 rospy.wait_for_service('doors_detector')
00105 try:
00106 print "Getting service proxy"
00107 find_door_laser = rospy.ServiceProxy('doors_detector', DoorsDetector)
00108 print "Calling service"
00109 door_reply = find_door_laser(door_request)
00110 print "Request finished"
00111 print "Door detected by laser at (%f, %f) (%f, %f)"%(door_reply.doors[0].door_p1.x, door_reply.doors[0].door_p1.y, door_reply.doors[0].door_p2.x, door_reply.doors[0].door_p2.y)
00112 return door_reply
00113 except:
00114 self.fail("doors_detector service call failed")
00115
00116 def detect_handle_laser(self, door_request):
00117
00118 print "Waiting for service...", rospy.resolve_name('handle_detector')
00119 rospy.wait_for_service('doors_detector')
00120 print "Service is available"
00121 detected = True
00122 for tries in xrange(2):
00123 print "Laser detection try: %d"%(tries+1)
00124 try:
00125 print "Getting service proxy"
00126 find_handle_laser = rospy.ServiceProxy('handle_detector', DoorsDetector)
00127 print "Calling service"
00128 door_reply = find_handle_laser(door_request)
00129 print "Request finished"
00130 print "Handle detected by laser at (%f, %f, %f)"%(door_reply.doors[0].handle.x, door_reply.doors[0].handle.y, door_reply.doors[0].handle.z)
00131 print "Laser frame", door_reply.doors[0].header.frame_id
00132 return door_reply
00133 except:
00134 detected = False
00135 if not detected:
00136 self.fail("handle_detector service call failed")
00137
00138 def detect_handle_camera(self, door_request):
00139
00140 print "Waiting for service...", rospy.resolve_name('door_handle_vision_detector')
00141 rospy.wait_for_service('door_handle_vision_detector')
00142 print "Service is available"
00143 try:
00144 print "Getting service proxy"
00145 find_handle = rospy.ServiceProxy('door_handle_vision_detector', DoorsDetector)
00146 print "Calling service"
00147 door_reply = find_handle(door_request)
00148 print "Request finished"
00149 print "Handle detected by camera at (%f, %f, %f)"%(door_reply.doors[0].handle.x, door_reply.doors[0].handle.y, door_reply.doors[0].handle.z)
00150 print "Camera frame", door_reply.doors[0].header.frame_id
00151 return door_reply
00152 except:
00153 self.fail("door_handle_vision_detector service call failed")
00154
00155
00156 if __name__ == "__main__":
00157 rostest.run('door_handle_detector', 'door_handle_detection',
00158 TestDoorHandleDetector, sys.argv)