def test_L_line_get_closest_waypoint_delivers_expected_values(self):
sut = tl_impl.TLDetector(self.c_waypoints_2d['L_line'])
exp = [([0, -1], 0), ([1, -1], 1), ([2, -1], 1), ([4, 4], 2)]
act = []
for query, idx in exp:
act.append((query, sut.get_closest_waypoint(query)))
self.assertEqual(exp, act)
def test_ver_line_get_closest_waypoint_delivers_expected_values(self):
sut = tl_impl.TLDetector(self.c_waypoints_2d['ver_line'])
exp = [([0, 0], 0), ([-0.1, 42.0], 3), ([0.9, 0.9], 1), ([2.1,
1.9], 2),
([0, 2.5001], 3)]
act = []
for query, idx in exp:
act.append((query, sut.get_closest_waypoint(query)))
self.assertEqual(exp, act)
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
#print("TLDetector is ready: {0}".format(self.impl.is_ready()))
if not self.impl.is_ready():
wps = self.waypoints
wps_2d = [[wp.pose.pose.position.x, wp.pose.pose.position.y]
for wp in wps.waypoints]
self.impl = tl_impl.TLDetector(wps_2d)
if not self.impl.is_ready():
raise ValueError(
"tl_impl.TLDetector could not be initialized!")
def test_diag_line_closest_traffic_lights_deliver_expected_values(self):
sut = tl_impl.TLDetector(self.c_waypoints_2d['diag_line'])
# mocks the TrafficLight list
c_lights = [0, 1]
# position of stop lines for respective traffic light len(c_lights) == len(c_lights_stop_lines_2d)
c_lights_stop_lines_2d = [[0, 0], [1, 1]]
# position, closest wp, light
exp = [([0, 0], 0, 0), ([0.75, 0.75], 1, 1), ([2.5, 2.5], -1, None),
([3, 3], -1, None)]
act = []
for pos_2d, exp_wp_idx, exp_closest_light in exp:
act_wp_idx, act_closest_light = sut.get_closest_traffic_light(
pos_2d, c_lights, c_lights_stop_lines_2d)
act.append((pos_2d, act_wp_idx, act_closest_light))
self.assertEqual(exp, act)
def test_L_line_long_closest_traffic_lights_deliver_expected_values(self):
sut = tl_impl.TLDetector(self.c_waypoints_2d['L_line_long'])
# mocks the TrafficLight list
c_lights = [0, 1, 2, 3, 4]
# position of stop lines for respective traffic light len(c_lights) == len(c_lights_stop_lines_2d)
c_lights_stop_lines_2d = [[0.25, -0.25], [0.75, 0.25], [2.25, 0.75],
[2.25, 1.75], [3.25, 1.75]]
# position, closest wp, light
exp = [([0.25, -0.25], 0, 0), ([0.75, 0.25], 1, 1), ([2.25,
0.75], 3, 2),
([2.25, 1.75], 4, 3), ([3.25, 1.75], 5, 4)]
act = []
for pos_2d, exp_wp_idx, exp_closest_light in exp:
act_wp_idx, act_closest_light = sut.get_closest_traffic_light(
pos_2d, c_lights, c_lights_stop_lines_2d)
act.append((pos_2d, act_wp_idx, act_closest_light))
self.assertEqual(exp, act)
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
# Empty detector
self.impl = tl_impl.TLDetector()
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier(rospy.get_param("/path_to_graph"))
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.spin()
def test_all_methods_throw_ValueError_if_object_not_initialized(self):
tld = tl_impl.TLDetector()
self.assertRaises(ValueError, tld.get_closest_waypoint, [0, 0])