vehicle: carla.Vehicle
(vehicle, offset=0.0, dist_threshold=15.0)
| 112 | |
| 113 | @staticmethod |
| 114 | def get_light_state(vehicle, offset=0.0, dist_threshold=15.0): |
| 115 | ''' |
| 116 | vehicle: carla.Vehicle |
| 117 | ''' |
| 118 | vec_tra = vehicle.get_transform() |
| 119 | veh_dir = vec_tra.get_forward_vector() |
| 120 | |
| 121 | hit_loc = vec_tra.transform(carla.Location(x=offset)) |
| 122 | hit_wp = TrafficLightHandler.carla_map.get_waypoint(hit_loc) |
| 123 | |
| 124 | light_loc = None |
| 125 | light_state = None |
| 126 | light_id = None |
| 127 | for i in range(TrafficLightHandler.num_tl): |
| 128 | traffic_light = TrafficLightHandler.list_tl_actor[i] |
| 129 | tv_loc = 0.5*TrafficLightHandler.list_stopline_wps[i][0].transform.location \ |
| 130 | + 0.5*TrafficLightHandler.list_stopline_wps[i][-1].transform.location |
| 131 | |
| 132 | distance = np.sqrt((tv_loc.x-hit_loc.x)**2 + (tv_loc.y-hit_loc.y)**2) |
| 133 | if distance > dist_threshold: |
| 134 | continue |
| 135 | |
| 136 | for wp in TrafficLightHandler.list_stopline_wps[i]: |
| 137 | |
| 138 | wp_dir = wp.transform.get_forward_vector() |
| 139 | dot_ve_wp = veh_dir.x * wp_dir.x + veh_dir.y * wp_dir.y + veh_dir.z * wp_dir.z |
| 140 | |
| 141 | wp_1 = wp.previous(4.0)[0] |
| 142 | same_road = (hit_wp.road_id == wp.road_id) and (hit_wp.lane_id == wp.lane_id) |
| 143 | same_road_1 = (hit_wp.road_id == wp_1.road_id) and (hit_wp.lane_id == wp_1.lane_id) |
| 144 | |
| 145 | # if (wp.road_id != wp_1.road_id) or (wp.lane_id != wp_1.lane_id): |
| 146 | # print(f'Traffic Light Problem: {wp.road_id}={wp_1.road_id}, {wp.lane_id}={wp_1.lane_id}') |
| 147 | |
| 148 | if (same_road or same_road_1) and dot_ve_wp > 0: |
| 149 | # This light is red and is affecting our lane |
| 150 | loc_in_ev = trans_utils.loc_global_to_ref(wp.transform.location, vec_tra) |
| 151 | light_loc = np.array([loc_in_ev.x, loc_in_ev.y, loc_in_ev.z], dtype=np.float32) |
| 152 | light_state = traffic_light.state |
| 153 | light_id = traffic_light.id |
| 154 | break |
| 155 | |
| 156 | return light_state, light_loc, light_id |
| 157 | |
| 158 | @staticmethod |
| 159 | def get_junctoin_paths(veh_loc, color=0, dist_threshold=50.0): |
no test coverage detected