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