def __contains__(self, loc: carla.Location) -> bool:
return loc.distance(self.center) <= self.radius
def _distance(self, wp: msg.WaypointWithSpeedLimit,
loc: carla.Location) -> float:
wp_loc = carla.Location(wp.x, wp.y)
return loc.distance(wp_loc)
def on_pose_update(self, msg):
self._logger.debug("@{}: pose update.".format(msg.timestamp))
transform = msg.data.transform
location = Location(transform.location.x, transform.location.y,
transform.location.z)
veh_extent = self._vehicle.bounding_box.extent.x
tail_close_pt = Vector3D(-0.8 * veh_extent, 0.0, location.z).rotate(
transform.rotation.yaw).as_simulator_vector()
tail_close_pt = location + Location(tail_close_pt)
tail_far_pt = Vector3D(-veh_extent - 1, 0.0, location.z).rotate(
transform.rotation.yaw).as_simulator_vector()
tail_far_pt = location + Location(tail_far_pt)
for traffic_light, center, waypoints in self._traffic_lights:
center_loc = Location(center)
if self._last_red_light_id and \
self._last_red_light_id == traffic_light.id:
continue
if center_loc.distance(location) > self.DISTANCE_LIGHT:
continue
if traffic_light.state != TrafficLightState.Red:
continue
for wp in waypoints:
tail_wp = self._map.get_waypoint(tail_far_pt)
# Calculate the dot product
ve_dir = transform.forward_vector
wp_dir = wp.transform.get_forward_vector()
dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + \
ve_dir.z * wp_dir.z
# Check the lane until all the "tail" has passed
if tail_wp.road_id == wp.road_id and \
tail_wp.lane_id == wp.lane_id and dot_ve_wp > 0:
# This light is red and is affecting our lane
yaw_wp = wp.transform.rotation.yaw
lane_width = wp.lane_width
location_wp = wp.transform.location
lft_lane_wp = Vector3D(
0.4 * lane_width, 0.0,
location_wp.z).rotate(yaw_wp +
90).as_simulator_vector()
lft_lane_wp = location_wp + Location(lft_lane_wp)
rgt_lane_wp = Vector3D(
0.4 * lane_width, 0.0,
location_wp.z).rotate(yaw_wp -
90).as_simulator_vector()
rgt_lane_wp = location_wp + Location(rgt_lane_wp)
# Is the vehicle traversing the stop line?
seg1 = (tail_close_pt, tail_far_pt)
seg2 = (lft_lane_wp, rgt_lane_wp)
if self.is_vehicle_crossing_line(seg1, seg2):
location = traffic_light.get_transform().location
message = TrafficInfractionMessage(
TrafficInfractionType.RED_LIGHT_INVASION,
pylot.utils.Location.from_simulator_location(
location), msg.timestamp)
self._traffic_light_invasion_stream.send(message)
self._last_red_light_id = traffic_light.id
break