class AutoPilot(BaseAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.save_path = None if path_to_conf_file: now = datetime.datetime.now() string = pathlib.Path(os.environ['ROUTES']).stem + '_' string += '_'.join(map(lambda x: '%02d' % x, (now.month, now.day, now.hour, now.minute, now.second))) print(string) self.save_path = pathlib.Path(path_to_conf_file) / string self.save_path.mkdir(exist_ok=False) (self.save_path / 'rgb').mkdir() (self.save_path / 'mask').mkdir() (self.save_path / 'rgb_left').mkdir() (self.save_path / 'rgb_right').mkdir() (self.save_path / 'measurements').mkdir() def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def _get_angle_to(self, pos, theta, target): R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) aim = R.T.dot(target - pos) angle = -np.degrees(np.arctan2(-aim[1], aim[0])) angle = 0.0 if np.isnan(angle) else angle return angle def _get_control(self, target, far_target, tick_data): pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # Steering. angle_unnorm = self._get_angle_to(pos, theta, target) angle = angle_unnorm / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) steer = round(steer, 3) # Acceleration. angle_far_unnorm = self._get_angle_to(pos, theta, far_target) should_slow = abs(angle_far_unnorm) > 45.0 or abs(angle_unnorm) > 5.0 target_speed = 4 if should_slow else 7.0 brake = self._should_brake() target_speed = target_speed if not brake else 0.0 delta = np.clip(target_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) if brake: steer *= 0.5 throttle = 0.0 return steer, throttle, brake, target_speed def run_step(self, input_data, timestamp): if not self.initialized: self._init() if self.step % 100 == 0: index = (self.step // 100) % len(WEATHERS) self._world.set_weather(WEATHERS[index]) data = self.tick(input_data) gps = self._get_position(data) near_node, near_command = self._waypoint_planner.run_step(gps) far_node, far_command = self._command_planner.run_step(gps) steer, throttle, brake, target_speed = self._get_control(near_node, far_node, data) control = carla.VehicleControl() control.steer = steer + 1e-2 * np.random.randn() control.throttle = throttle control.brake = float(brake) if self.step % 10 == 0: self.save(far_node, near_command, steer, throttle, float(brake), target_speed, data) return control def save(self, far_node, near_command, steer, throttle, brake, target_speed, tick_data): frame = self.step // 10 pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] data = { "x": pos[0], "y": pos[1], "theta": theta, "speed": speed, "target_speed": target_speed, "x_command": far_node[0], "y_command": far_node[1], "command": near_command.value, "steer": steer, "throttle": throttle, "brake": brake, } with open(self.save_path / 'measurements' / ('%04d.json' % frame), 'w') as f: json.dump(data, f) Image.fromarray(tick_data['rgb']).save(self.save_path / 'rgb' / ('%04d.png' % frame)) Image.fromarray(tick_data['mask']).save(self.save_path / 'mask' / ('%04d.png' % frame)) Image.fromarray(tick_data['rgb_left']).save(self.save_path / 'rgb_left' / ('%04d.png' % frame)) Image.fromarray(tick_data['rgb_right']).save(self.save_path / 'rgb_right' / ('%04d.png' % frame)) def _should_brake(self): actors = self._world.get_actors() vehicle = self._is_vehicle_hazard(actors.filter('*vehicle*')) light = self._is_light_red(actors.filter('*traffic_light*')) walker = self._is_walker_hazard(actors.filter('*walker*')) return any(x is not None for x in [vehicle, light, walker]) def _draw_line(self, p, v, z, color=(255, 0, 0)): if not DEBUG: return p1 = _location(p[0], p[1], z) p2 = _location(p[0]+v[0], p[1]+v[1], z) color = carla.Color(*color) self._world.debug.draw_line(p1, p2, 0.25, color, 0.01) def _is_light_red(self, lights_list): if self._vehicle.get_traffic_light_state() != carla.libcarla.TrafficLightState.Green: affecting = self._vehicle.get_traffic_light() for light in self._traffic_lights: if light.id == affecting.id: return affecting return None def _is_walker_hazard(self, walkers_list): z = self._vehicle.get_location().z p1 = _numpy(self._vehicle.get_location()) v1 = 10.0 * _orientation(self._vehicle.get_transform().rotation.yaw) self._draw_line(p1, v1, z+2.5, (0, 0, 255)) for walker in walkers_list: v2_hat = _orientation(walker.get_transform().rotation.yaw) s2 = np.linalg.norm(_numpy(walker.get_velocity())) if s2 < 0.05: v2_hat *= s2 p2 = -3.0 * v2_hat + _numpy(walker.get_location()) v2 = 8.0 * v2_hat self._draw_line(p2, v2, z+2.5) collides, collision_point = get_collision(p1, v1, p2, v2) if collides: return walker return None def _is_vehicle_hazard(self, vehicle_list): z = self._vehicle.get_location().z o1 = _orientation(self._vehicle.get_transform().rotation.yaw) p1 = _numpy(self._vehicle.get_location()) s1 = max(7.5, 2.0 * np.linalg.norm(_numpy(self._vehicle.get_velocity()))) v1_hat = o1 v1 = s1 * v1_hat self._draw_line(p1, v1, z+2.5, (255, 0, 0)) for target_vehicle in vehicle_list: if target_vehicle.id == self._vehicle.id: continue o2 = _orientation(target_vehicle.get_transform().rotation.yaw) p2 = _numpy(target_vehicle.get_location()) s2 = max(5.0, 2.0 * np.linalg.norm(_numpy(target_vehicle.get_velocity()))) v2_hat = o2 v2 = s2 * v2_hat p2_p1 = p2 - p1 distance = np.linalg.norm(p2_p1) p2_p1_hat = p2_p1 / (distance + 1e-4) self._draw_line(p2, v2, z+2.5, (255, 0, 0)) angle_to_car = np.degrees(np.arccos(v1_hat.dot(p2_p1_hat))) angle_between_heading = np.degrees(np.arccos(o1.dot(o2))) if angle_between_heading > 60.0 and not (angle_to_car < 15 and distance < s1): continue elif angle_to_car > 30.0: continue elif distance > s1: continue return target_vehicle return None
class ImageAgent(BaseAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.converter = Converter() self.net = ImageModel.load_from_checkpoint(path_to_conf_file) self.net.cuda() self.net.eval() # addition: modified from leaderboard/team_code/auto_pilot.py self.save_path = None parent_folder = os.environ['SAVE_FOLDER'] string = pathlib.Path(os.environ['ROUTES']).stem self.save_path = pathlib.Path(parent_folder) / string # addition: modified from leaderboard/team_code/auto_pilot.py def save(self, steer, throttle, brake, tick_data): # frame = self.step // 10 frame = self.step pos = self._get_position(tick_data) far_command = tick_data['far_command'] speed = tick_data['speed'] string = os.environ['SAVE_FOLDER'] + '/' + pathlib.Path( os.environ['ROUTES']).stem center_str = string + '/' + 'rgb' + '/' + ('%04d.png' % frame) left_str = string + '/' + 'rgb_left' + '/' + ('%04d.png' % frame) right_str = string + '/' + 'rgb_right' + '/' + ('%04d.png' % frame) # topdown_str = string + '/' + 'topdown' + '/' + ('%04d.png' % frame) # rgb_with_car_str = string + '/' + 'rgb_with_car' + '/' + ('%04d.png' % frame) center = self.save_path / 'rgb' / ('%04d.png' % frame) left = self.save_path / 'rgb_left' / ('%04d.png' % frame) right = self.save_path / 'rgb_right' / ('%04d.png' % frame) topdown = self.save_path / 'topdown' / ('%04d.png' % frame) rgb_with_car = self.save_path / 'rgb_with_car' / ('%04d.png' % frame) data_row = ','.join([ str(i) for i in [ frame, far_command, speed, steer, throttle, brake, center_str, left_str, right_str ] ]) with (self.save_path / 'measurements.csv').open("a") as f_out: f_out.write(data_row + '\n') Image.fromarray(tick_data['rgb']).save(center) Image.fromarray(tick_data['rgb_left']).save(left) Image.fromarray(tick_data['rgb_right']).save(right) # addition Image.fromarray(COLOR[CONVERTER[tick_data['topdown']]]).save(topdown) Image.fromarray(tick_data['rgb_with_car']).save(rgb_with_car) def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) # addition: self._vehicle = CarlaDataProvider.get_hero_actor() self._world = self._vehicle.get_world() def tick(self, input_data): result = super().tick(input_data) result['image'] = np.concatenate( tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) rgb_with_car = cv2.cvtColor(input_data['rgb_with_car'][1][:, :, :3], cv2.COLOR_BGR2RGB) result['rgb_with_car'] = rgb_with_car theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) gps = self._get_position(result) # modification far_node, far_command = self._command_planner.run_step(gps) target = R.T.dot(far_node - gps) target *= 5.5 target += [128, 256] target = np.clip(target, 0, 256) result['target'] = target # addition: self._actors = self._world.get_actors() self._traffic_lights = get_nearby_lights( self._vehicle, self._actors.filter('*traffic_light*')) result['far_command'] = far_command topdown = input_data['map'][1][:, :, 2] topdown = draw_traffic_lights(topdown, self._vehicle, self._traffic_lights) result['topdown'] = topdown return result @torch.no_grad() def run_step_using_learned_controller(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() points, (target_cam, _) = self.net.forward(img, target) control = self.net.controller(points).cpu().squeeze() steer = control[0].item() desired_speed = control[1].item() speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() points, (target_cam, _) = self.net.forward(img, target) points_cam = points.clone().cpu() points_cam[..., 0] = (points_cam[..., 0] + 1) / 2 * img.shape[-1] points_cam[..., 1] = (points_cam[..., 1] + 1) / 2 * img.shape[-2] points_cam = points_cam.squeeze() points_world = self.converter.cam_to_world(points_cam).numpy() aim = (points_world[1] + points_world[0]) / 2.0 angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 # desired_speed *= (1 - abs(angle)) ** 2 speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) # addition: from leaderboard/team_code/auto_pilot.py if self.step == 0: title_row = ','.join([ 'frame_id', 'far_command', 'speed', 'steering', 'throttle', 'brake', 'center', 'left', 'right' ]) with (self.save_path / 'measurements.csv').open("a") as f_out: f_out.write(title_row + '\n') if self.step % 2 == 0: self.gather_info() record_every_n_steps = 3 if self.step % record_every_n_steps == 0: self.save(steer, throttle, brake, tick_data) return control
class AutoPilot(MapAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.save_path = None if path_to_conf_file: now = datetime.datetime.now() string = pathlib.Path(os.environ['ROUTES']).stem + '_' string += '_'.join( map(lambda x: '%02d' % x, (now.month, now.day, now.hour, now.minute, now.second))) print(string) self.save_path = pathlib.Path(path_to_conf_file) / string self.save_path.mkdir(exist_ok=False) (self.save_path / 'rgb').mkdir() (self.save_path / 'rgb_left').mkdir() (self.save_path / 'rgb_right').mkdir() (self.save_path / 'topdown').mkdir() (self.save_path / 'measurements').mkdir() def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def _get_angle_to(self, pos, theta, target): R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) aim = R.T.dot(target - pos) angle = -np.degrees(np.arctan2(-aim[1], aim[0])) angle = 0.0 if np.isnan(angle) else angle return angle def _get_control(self, target, far_target, tick_data, _draw): pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # Steering. angle_unnorm = self._get_angle_to(pos, theta, target) angle = angle_unnorm / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) steer = round(steer, 3) # Acceleration. angle_far_unnorm = self._get_angle_to(pos, theta, far_target) should_slow = abs(angle_far_unnorm) > 45.0 or abs(angle_unnorm) > 5.0 target_speed = 4 if should_slow else 7.0 brake = self._should_brake() target_speed = target_speed if not brake else 0.0 delta = np.clip(target_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) if brake: steer *= 0.5 throttle = 0.0 _draw.text((5, 90), 'Speed: %.3f' % speed) _draw.text((5, 110), 'Target: %.3f' % target_speed) _draw.text((5, 130), 'Angle: %.3f' % angle_unnorm) _draw.text((5, 150), 'Angle Far: %.3f' % angle_far_unnorm) return steer, throttle, brake, target_speed def run_step(self, input_data, timestamp): if not self.initialized: self._init() if self.step % 100 == 0: index = (self.step // 100) % len(WEATHERS) self._world.set_weather(WEATHERS[index]) data = self.tick(input_data) topdown = data['topdown'] rgb = np.hstack((data['rgb_left'], data['rgb'], data['rgb_right'])) gps = self._get_position(data) near_node, near_command = self._waypoint_planner.run_step(gps) far_node, far_command = self._command_planner.run_step(gps) _topdown = Image.fromarray(COLOR[CONVERTER[topdown]]) _rgb = Image.fromarray(rgb) _draw = ImageDraw.Draw(_topdown) _topdown.thumbnail((256, 256)) _rgb = _rgb.resize((int(256 / _rgb.size[1] * _rgb.size[0]), 256)) _combined = Image.fromarray(np.hstack((_rgb, _topdown))) _draw = ImageDraw.Draw(_combined) steer, throttle, brake, target_speed = self._get_control( near_node, far_node, data, _draw) _draw.text((5, 10), 'FPS: %.3f' % (self.step / (time.time() - self.wall_start))) _draw.text((5, 30), 'Steer: %.3f' % steer) _draw.text((5, 50), 'Throttle: %.3f' % throttle) _draw.text((5, 70), 'Brake: %s' % brake) if HAS_DISPLAY: cv2.imshow('map', cv2.cvtColor(np.array(_combined), cv2.COLOR_BGR2RGB)) cv2.waitKey(1) control = carla.VehicleControl() control.steer = steer + 1e-2 * np.random.randn() control.throttle = throttle control.brake = float(brake) if self.step % 10 == 0: self.save(far_node, near_command, steer, throttle, brake, target_speed, data) return control def save(self, far_node, near_command, steer, throttle, brake, target_speed, tick_data): frame = self.step // 10 pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] data = { 'x': pos[0], 'y': pos[1], 'theta': theta, 'speed': speed, 'target_speed': target_speed, 'x_command': far_node[0], 'y_command': far_node[1], 'command': near_command.value, 'steer': steer, 'throttle': throttle, 'brake': brake, } (self.save_path / 'measurements' / ('%04d.json' % frame)).write_text( str(data)) Image.fromarray(tick_data['rgb']).save(self.save_path / 'rgb' / ('%04d.png' % frame)) Image.fromarray(tick_data['rgb_left']).save( self.save_path / 'rgb_left' / ('%04d.png' % frame)) Image.fromarray(tick_data['rgb_right']).save( self.save_path / 'rgb_right' / ('%04d.png' % frame)) Image.fromarray(tick_data['topdown']).save(self.save_path / 'topdown' / ('%04d.png' % frame)) def _should_brake(self): actors = self._world.get_actors() vehicle = self._is_vehicle_hazard(actors.filter('*vehicle*')) light = self._is_light_red(actors.filter('*traffic_light*')) walker = self._is_walker_hazard(actors.filter('*walker*')) return any(x is not None for x in [vehicle, light, walker]) def _draw_line(self, p, v, z, color=(255, 0, 0)): if not DEBUG: return p1 = _location(p[0], p[1], z) p2 = _location(p[0] + v[0], p[1] + v[1], z) color = carla.Color(*color) self._world.debug.draw_line(p1, p2, 0.25, color, 0.01) def _is_light_red(self, lights_list): if self._vehicle.get_traffic_light_state( ) != carla.libcarla.TrafficLightState.Green: affecting = self._vehicle.get_traffic_light() for light in self._traffic_lights: if light.id == affecting.id: return affecting return None def _is_walker_hazard(self, walkers_list): z = self._vehicle.get_location().z p1 = _numpy(self._vehicle.get_location()) v1 = 10.0 * _orientation(self._vehicle.get_transform().rotation.yaw) self._draw_line(p1, v1, z + 2.5, (0, 0, 255)) for walker in walkers_list: v2_hat = _orientation(walker.get_transform().rotation.yaw) s2 = np.linalg.norm(_numpy(walker.get_velocity())) if s2 < 0.05: v2_hat *= s2 p2 = -3.0 * v2_hat + _numpy(walker.get_location()) v2 = 8.0 * v2_hat self._draw_line(p2, v2, z + 2.5) collides, collision_point = get_collision(p1, v1, p2, v2) if collides: return walker return None def _is_vehicle_hazard(self, vehicle_list): z = self._vehicle.get_location().z o1 = _orientation(self._vehicle.get_transform().rotation.yaw) p1 = _numpy(self._vehicle.get_location()) s1 = max(7.5, 2.0 * np.linalg.norm(_numpy(self._vehicle.get_velocity()))) v1_hat = o1 v1 = s1 * v1_hat self._draw_line(p1, v1, z + 2.5, (255, 0, 0)) for target_vehicle in vehicle_list: if target_vehicle.id == self._vehicle.id: continue o2 = _orientation(target_vehicle.get_transform().rotation.yaw) p2 = _numpy(target_vehicle.get_location()) s2 = max( 5.0, 2.0 * np.linalg.norm(_numpy(target_vehicle.get_velocity()))) v2_hat = o2 v2 = s2 * v2_hat p2_p1 = p2 - p1 distance = np.linalg.norm(p2_p1) p2_p1_hat = p2_p1 / (distance + 1e-4) self._draw_line(p2, v2, z + 2.5, (255, 0, 0)) angle_to_car = np.degrees(np.arccos(v1_hat.dot(p2_p1_hat))) angle_between_heading = np.degrees(np.arccos(o1.dot(o2))) if angle_between_heading > 60.0 and not (angle_to_car < 15 and distance < s1): continue elif angle_to_car > 30.0: continue elif distance > s1: continue return target_vehicle return None
class AutoPilot(MapAgent): # for stop signs PROXIMITY_THRESHOLD = 30.0 # meters SPEED_THRESHOLD = 0.1 WAYPOINT_STEP = 1.0 # meters def setup(self, path_to_conf_file): super().setup(path_to_conf_file) def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) # for stop signs self._target_stop_sign = None # the stop sign affecting the ego vehicle self._stop_completed = False # if the ego vehicle has completed the stop sign self._affected_by_stop = False # if the ego vehicle is influenced by a stop sign def _get_angle_to(self, pos, theta, target): R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) aim = R.T.dot(target - pos) angle = -np.degrees(np.arctan2(-aim[1], aim[0])) angle = 0.0 if np.isnan(angle) else angle return angle def _get_control(self, target, far_target, tick_data): pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # Steering. angle_unnorm = self._get_angle_to(pos, theta, target) angle = angle_unnorm / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) steer = round(steer, 3) # Acceleration. angle_far_unnorm = self._get_angle_to(pos, theta, far_target) should_slow = abs(angle_far_unnorm) > 45.0 or abs(angle_unnorm) > 5.0 target_speed = 4.0 if should_slow else 7.0 brake = self._should_brake() target_speed = target_speed if not brake else 0.0 self.should_slow = int(should_slow) self.should_brake = int(brake) self.angle = angle self.angle_unnorm = angle_unnorm self.angle_far_unnorm = angle_far_unnorm delta = np.clip(target_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) if brake: steer *= 0.5 throttle = 0.0 return steer, throttle, brake, target_speed def run_step(self, input_data, timestamp): if not self.initialized: self._init() # change weather for visual diversity if self.step % 10 == 0: index = random.choice(range(len(WEATHERS))) self.weather_id = WEATHERS_IDS[index] weather = WEATHERS[WEATHERS_IDS[index]] print (self.weather_id, weather) self._world.set_weather(weather) data = self.tick(input_data) gps = self._get_position(data) near_node, near_command = self._waypoint_planner.run_step(gps) far_node, far_command = self._command_planner.run_step(gps) steer, throttle, brake, target_speed = self._get_control(near_node, far_node, data) control = carla.VehicleControl() control.steer = steer + 1e-2 * np.random.randn() control.throttle = throttle control.brake = float(brake) if self.step % 10 == 0 and self.save_path is not None: self.save(near_node, far_node, near_command, steer, throttle, brake, target_speed, data) return control def _should_brake(self): actors = self._world.get_actors() vehicle = self._is_vehicle_hazard(actors.filter('*vehicle*')) light = self._is_light_red(actors.filter('*traffic_light*')) walker = self._is_walker_hazard(actors.filter('*walker*')) stop_sign = self._is_stop_sign_hazard(actors.filter('*stop*')) self.is_vehicle_present = 1 if vehicle is not None else 0 self.is_red_light_present = 1 if light is not None else 0 self.is_pedestrian_present = 1 if walker is not None else 0 self.is_stop_sign_present = 1 if stop_sign is not None else 0 return any(x is not None for x in [vehicle, light, walker, stop_sign]) def _point_inside_boundingbox(self, point, bb_center, bb_extent): A = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y - bb_extent.y) B = carla.Vector2D(bb_center.x + bb_extent.x, bb_center.y - bb_extent.y) D = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y + bb_extent.y) M = carla.Vector2D(point.x, point.y) AB = B - A AD = D - A AM = M - A am_ab = AM.x * AB.x + AM.y * AB.y ab_ab = AB.x * AB.x + AB.y * AB.y am_ad = AM.x * AD.x + AM.y * AD.y ad_ad = AD.x * AD.x + AD.y * AD.y return am_ab > 0 and am_ab < ab_ab and am_ad > 0 and am_ad < ad_ad def _get_forward_speed(self, transform=None, velocity=None): """ Convert the vehicle transform directly to forward speed """ if not velocity: velocity = self._vehicle.get_velocity() if not transform: transform = self._vehicle.get_transform() vel_np = np.array([velocity.x, velocity.y, velocity.z]) pitch = np.deg2rad(transform.rotation.pitch) yaw = np.deg2rad(transform.rotation.yaw) orientation = np.array([np.cos(pitch) * np.cos(yaw), np.cos(pitch) * np.sin(yaw), np.sin(pitch)]) speed = np.dot(vel_np, orientation) return speed def _is_actor_affected_by_stop(self, actor, stop, multi_step=20): """ Check if the given actor is affected by the stop """ affected = False # first we run a fast coarse test current_location = actor.get_location() stop_location = stop.get_transform().location if stop_location.distance(current_location) > self.PROXIMITY_THRESHOLD: return affected stop_t = stop.get_transform() transformed_tv = stop_t.transform(stop.trigger_volume.location) # slower and accurate test based on waypoint's horizon and geometric test list_locations = [current_location] waypoint = self._world.get_map().get_waypoint(current_location) for _ in range(multi_step): if waypoint: waypoint = waypoint.next(self.WAYPOINT_STEP)[0] if not waypoint: break list_locations.append(waypoint.transform.location) for actor_location in list_locations: if self._point_inside_boundingbox(actor_location, transformed_tv, stop.trigger_volume.extent): affected = True return affected def _is_stop_sign_hazard(self, stop_sign_list): if self._affected_by_stop: if not self._stop_completed: current_speed = self._get_forward_speed() if current_speed < self.SPEED_THRESHOLD: self._stop_completed = True return None else: return self._target_stop_sign else: # reset if the ego vehicle is outside the influence of the current stop sign if not self._is_actor_affected_by_stop(self._vehicle, self._target_stop_sign): self._affected_by_stop = False self._stop_completed = False self._target_stop_sign = None return None ve_tra = self._vehicle.get_transform() ve_dir = ve_tra.get_forward_vector() wp = self._world.get_map().get_waypoint(ve_tra.location) 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 if dot_ve_wp > 0: # Ignore all when going in a wrong lane for stop_sign in stop_sign_list: if self._is_actor_affected_by_stop(self._vehicle, stop_sign): # this stop sign is affecting the vehicle self._affected_by_stop = True self._target_stop_sign = stop_sign return self._target_stop_sign return None def _is_light_red(self, lights_list): if self._vehicle.get_traffic_light_state() != carla.libcarla.TrafficLightState.Green: affecting = self._vehicle.get_traffic_light() for light in self._traffic_lights: if light.id == affecting.id: return affecting return None def _is_walker_hazard(self, walkers_list): z = self._vehicle.get_location().z p1 = _numpy(self._vehicle.get_location()) v1 = 10.0 * _orientation(self._vehicle.get_transform().rotation.yaw) for walker in walkers_list: v2_hat = _orientation(walker.get_transform().rotation.yaw) s2 = np.linalg.norm(_numpy(walker.get_velocity())) if s2 < 0.05: v2_hat *= s2 p2 = -3.0 * v2_hat + _numpy(walker.get_location()) v2 = 8.0 * v2_hat collides, collision_point = get_collision(p1, v1, p2, v2) if collides: return walker return None def _is_vehicle_hazard(self, vehicle_list): z = self._vehicle.get_location().z o1 = _orientation(self._vehicle.get_transform().rotation.yaw) p1 = _numpy(self._vehicle.get_location()) s1 = max(10, 3.0 * np.linalg.norm(_numpy(self._vehicle.get_velocity()))) # increases the threshold distance v1_hat = o1 v1 = s1 * v1_hat for target_vehicle in vehicle_list: if target_vehicle.id == self._vehicle.id: continue o2 = _orientation(target_vehicle.get_transform().rotation.yaw) p2 = _numpy(target_vehicle.get_location()) s2 = max(5.0, 2.0 * np.linalg.norm(_numpy(target_vehicle.get_velocity()))) v2_hat = o2 v2 = s2 * v2_hat p2_p1 = p2 - p1 distance = np.linalg.norm(p2_p1) p2_p1_hat = p2_p1 / (distance + 1e-4) angle_to_car = np.degrees(np.arccos(v1_hat.dot(p2_p1_hat))) angle_between_heading = np.degrees(np.arccos(o1.dot(o2))) # to consider -ve angles too angle_to_car = min(angle_to_car, 360.0 - angle_to_car) angle_between_heading = min(angle_between_heading, 360.0 - angle_between_heading) if angle_between_heading > 60.0 and not (angle_to_car < 15 and distance < s1): continue elif angle_to_car > 30.0: continue elif distance > s1: continue return target_vehicle return None
class AutoPilot(MapAgent): def _init(self): super()._init() self._default_target_speed = 7.0 self._default_slow_target_speed = 4.0 self._angle = None self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def _get_angle_to(self, pos, theta, target): R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) aim = R.T.dot(target - pos) angle = -np.degrees(np.arctan2(-aim[1], aim[0])) angle = 0.0 if np.isnan(angle) else angle return angle def _get_control(self, target, far_target, tick_data, _draw): pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # Steering. angle_unnorm = self._get_angle_to(pos, theta, target) angle = angle_unnorm / 90 self._angle = np.radians(angle_unnorm) steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) steer = round(steer, 3) # Acceleration. angle_far_unnorm = self._get_angle_to(pos, theta, far_target) should_slow = abs(angle_far_unnorm) > 45.0 or abs(angle_unnorm) > 5.0 target_speed = self._default_slow_target_speed if should_slow else self._default_target_speed brake = self._should_brake() target_speed = target_speed if not brake else 0.0 self._target_speed = target_speed delta = np.clip(target_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) if brake: steer *= 0.5 throttle = 0.0 _draw.text((5, 90), 'Speed: %.3f' % speed) _draw.text((5, 110), 'Target: %.3f' % target_speed) _draw.text((5, 130), 'Angle: %.3f' % angle_unnorm) _draw.text((5, 150), 'Angle Far: %.3f' % angle_far_unnorm) return steer, throttle, brake, target_speed def run_step(self, input_data, timestamp): if not self.initialized: self._init() # self._world.set_weather(WEATHERS[int(os.environ['WEATHER_INDEX'])]) # 100 -> 50 since 20Hz -> 10Hz if self.step % 50 == 0 and self.args.changing_weather: index = np.random.randint(0, len(WEATHERS)) self._world.set_weather(WEATHERS[index]) data = self.tick(input_data) rgb_with_car = cv2.cvtColor(input_data['rgb_with_car'][1][:, :, :3], cv2.COLOR_BGR2RGB) data['rgb_with_car'] = rgb_with_car topdown = data['topdown'] rgb = np.hstack((data['rgb_left'], data['rgb'], data['rgb_right'])) gps = self._get_position(data) near_node, near_command = self._waypoint_planner.run_step(gps) far_node, far_command = self._command_planner.run_step(gps) _topdown = Image.fromarray(COLOR[CONVERTER[topdown]]) _rgb = Image.fromarray(rgb) _draw = ImageDraw.Draw(_topdown) _topdown.thumbnail((256, 256)) _rgb = _rgb.resize((int(256 / _rgb.size[1] * _rgb.size[0]), 256)) _combined = Image.fromarray(np.hstack((_rgb, _topdown))) _draw = ImageDraw.Draw(_combined) steer, throttle, brake, target_speed = self._get_control( near_node, far_node, data, _draw) _draw.text((5, 10), 'FPS: %.3f' % (self.step / (time.time() - self.wall_start))) _draw.text((5, 30), 'Steer: %.3f' % steer) _draw.text((5, 50), 'Throttle: %.3f' % throttle) _draw.text((5, 70), 'Brake: %s' % brake) if HAS_DISPLAY: cv2.imshow('map', cv2.cvtColor(np.array(_combined), cv2.COLOR_BGR2RGB)) cv2.waitKey(1) control = carla.VehicleControl() control.steer = steer + 1e-2 * np.random.randn() control.throttle = throttle control.brake = float(brake) # we only gether info every 2 frames for faster processing speed if self.step % 1 == 0: self.gather_info() # if this number is very small, we may not have the exact numbers and images for the event happening (e.g. the frame when a collision happen). However, this is usually ok if we only use these for retraining purpose record_every_n_steps = self.record_every_n_step if self.step % record_every_n_steps == 0: self.save(record_every_n_steps, far_command, steer, throttle, brake, target_speed, data) self.save_json(record_every_n_steps, far_node, near_command, steer, throttle, brake, target_speed, data) return control def save_json(self, record_every_n_steps, far_node, near_command, steer, throttle, brake, target_speed, tick_data): frame = int(self.step // record_every_n_steps) pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # pos, , far_node, near_command data = { 'x': pos[0], 'y': pos[1], 'theta': theta, 'speed': speed, 'target_speed': target_speed, 'x_command': far_node[0], 'y_command': far_node[1], 'command': near_command.value, 'steer': steer, 'throttle': throttle, 'brake': brake, } pth = self.save_path / 'measurements' pth.mkdir(parents=False, exist_ok=True) (pth / ('%04d.json' % frame)).write_text(str(data)) def save(self, record_every_n_steps, far_command, steer, throttle, brake, target_speed, tick_data): frame = int(self.step // record_every_n_steps) speed = tick_data['speed'] center = os.path.join('rgb', ('%06d.png' % frame)) left = os.path.join('rgb_left', ('%06d.png' % frame)) right = os.path.join('rgb_right', ('%06d.png' % frame)) topdown = os.path.join('topdown', ('%06d.png' % frame)) rgb_with_car = os.path.join('rgb_with_car', ('%06d.png' % frame)) data_row = ','.join([ str(i) for i in [ frame, far_command, speed, steer, throttle, brake, str(center), str(left), str(right) ] ]) with (self.save_path / 'measurements.csv').open("a") as f_out: f_out.write(data_row + '\n') Image.fromarray(tick_data['rgb']).save(self.save_path / center) Image.fromarray(tick_data['rgb_left']).save(self.save_path / left) Image.fromarray(tick_data['rgb_right']).save(self.save_path / right) # modification # Image.fromarray(COLOR[CONVERTER[tick_data['topdown']]]).save(topdown) Image.fromarray(tick_data['topdown']).save(self.save_path / topdown) Image.fromarray(tick_data['rgb_with_car']).save(self.save_path / rgb_with_car) ######################################################################## # log necessary info for action-based if self.args.save_action_based_measurements: from affordances import get_driving_affordances self._pedestrian_forbidden_distance = 10.0 self._pedestrian_max_detected_distance = 50.0 self._vehicle_forbidden_distance = 10.0 self._vehicle_max_detected_distance = 50.0 self._tl_forbidden_distance = 10.0 self._tl_max_detected_distance = 50.0 self._speed_detected_distance = 10.0 current_affordances = get_driving_affordances( self, self._pedestrian_forbidden_distance, self._pedestrian_max_detected_distance, self._vehicle_forbidden_distance, self._vehicle_max_detected_distance, self._tl_forbidden_distance, self._tl_max_detected_distance, self._angle, self._default_target_speed, self._target_speed, self._speed_detected_distance, angle=True) is_vehicle_hazard = current_affordances['is_vehicle_hazard'] is_red_tl_hazard = current_affordances['is_red_tl_hazard'] is_pedestrian_hazard = current_affordances['is_pedestrian_hazard'] forward_speed = current_affordances['forward_speed'] relative_angle = current_affordances['relative_angle'] target_speed = current_affordances['target_speed'] closest_pedestrian_distance = current_affordances[ 'closest_pedestrian_distance'] closest_vehicle_distance = current_affordances[ 'closest_vehicle_distance'] closest_red_tl_distance = current_affordances[ 'closest_red_tl_distance'] log_folder = str(self.save_path / 'affordance_measurements') if not os.path.exists(log_folder): os.mkdir(log_folder) log_path = os.path.join(log_folder, f'{self.step:06}.json') ego_transform = self._vehicle.get_transform() ego_location = ego_transform.location ego_rotation = ego_transform.rotation ego_velocity = self._vehicle.get_velocity() # relative_angle # lane_center_waypoint = self._map.get_waypoint(ego_location, lane_type=carla.LaneType.Any) # lane_center_transform = lane_center_waypoint.transform # # # relative_angle = np.abs(lane_center_transform.rotation.yaw - ego_rotation.yaw) # if lane_center_transform.rotation.yaw - ego_rotation.yaw > 180: # relative_angle = np.abs(lane_center_transform.rotation.yaw - ego_rotation.yaw - 360) # elif ego_rotation.yaw - lane_center_transform.rotation.yaw > 180: # relative_angle = np.abs(lane_center_transform.rotation.yaw - ego_rotation.yaw + 360) # # relative_angle = np.radians(relative_angle) # vehicle, pedestrian, traffic light # actors = self._world.get_actors() # vehicle_list = actors.filter('*vehicle*') # pedestrian_list = actors.filter('walker*') # tls = actors.filter('*traffic_light*') # # ego_bbox = get_bbox(self._vehicle) # # closest_vehicle_distance = 50 # for i, vehicle in enumerate(vehicle_list): # if vehicle.id == self._vehicle.id: # continue # other_bbox = get_bbox(vehicle) # for other_b in other_bbox: # for ego_b in ego_bbox: # d = norm_2d(other_b, ego_b) # # print('vehicle', i, 'd', d) # closest_vehicle_distance = np.min([closest_vehicle_distance, d]) # # closest_pedestrian_distance = 50 # for i, pedestrian in enumerate(pedestrian_list): # other_bbox = get_bbox(pedestrian) # for other_b in other_bbox: # for ego_b in ego_bbox: # d = norm_2d(other_b, ego_b) # # print('pedestrian', i, 'd', d) # closest_pedestrian_distance = np.min([closest_pedestrian_distance, d]) # # closest_red_tl_distance = 50 # is_red_tl_hazard = False # for tl in tls: # if tl.state == carla.TrafficLightState.Red: # tl_location = tl.get_transform().location # d = norm_2d(ego_location, tl_location) # closest_red_tl_distance = np.min([closest_red_tl_distance, d]) # # if d < 10: # affecting = self._vehicle.get_traffic_light() # if affecting and tl.id == affecting.id: # is_red_tl_hazard = True # # is_pedestrian_hazard = bool(closest_pedestrian_distance < 10) # is_vehicle_hazard = bool(closest_vehicle_distance < 10) brake_noise = 0.0 throttle_noise = 0.0 # 1.0 -> 0.0 steer_noise = 0.0 # NaN -> 0.0 # class RoadOption # VOID = -1 # LEFT = 1 # RIGHT = 2 # STRAIGHT = 3 # LANEFOLLOW = 4 # CHANGELANELEFT = 5 # CHANGELANERIGHT = 6 map_roadoption_to_action_based_roadoption = { -1: 2, 1: 3, 2: 4, 3: 5, 4: 2, 5: 2, 6: 2 } # print('\n far_command', far_command) # save info for action-based rep json_log_data = { "brake": float(brake), "closest_red_tl_distance": closest_red_tl_distance, "throttle": throttle, "directions": float(map_roadoption_to_action_based_roadoption[ far_command.value]), "brake_noise": brake_noise, "is_red_tl_hazard": is_red_tl_hazard, "opponents": {}, "closest_pedestrian_distance": closest_pedestrian_distance, "is_pedestrian_hazard": is_pedestrian_hazard, "lane": {}, "is_vehicle_hazard": is_vehicle_hazard, "throttle_noise": throttle_noise, "ego_actor": { "velocity": [ego_velocity.x, ego_velocity.y, ego_velocity.z], "position": [ego_location.x, ego_location.y, ego_location.z], "orientation": [ego_rotation.roll, ego_rotation.pitch, ego_rotation.yaw] }, "hand_brake": False, "steer_noise": steer_noise, "reverse": False, "relative_angle": relative_angle, "closest_vehicle_distance": closest_vehicle_distance, "walkers": {}, "forward_speed": forward_speed, "steer": steer, "target_speed": target_speed } with open(log_path, 'w') as f_out: json.dump(json_log_data, f_out, indent=4) def _should_brake(self): actors = self._world.get_actors() vehicle = self._is_vehicle_hazard(actors.filter('*vehicle*')) light = self._is_light_red(actors.filter('*traffic_light*')) walker = self._is_walker_hazard(actors.filter('*walker*')) return any(x is not None for x in [vehicle, light, walker]) def _draw_line(self, p, v, z, color=(255, 0, 0)): if not DEBUG: return p1 = _location(p[0], p[1], z) p2 = _location(p[0] + v[0], p[1] + v[1], z) color = carla.Color(*color) self._world.debug.draw_line(p1, p2, 0.25, color, 0.01) def _is_light_red(self, lights_list): if self._vehicle.get_traffic_light_state( ) != carla.libcarla.TrafficLightState.Green: affecting = self._vehicle.get_traffic_light() for light in self._traffic_lights: if light.id == affecting.id: return affecting return None def _is_walker_hazard(self, walkers_list): z = self._vehicle.get_location().z p1 = _numpy(self._vehicle.get_location()) v1 = 10.0 * _orientation(self._vehicle.get_transform().rotation.yaw) # self._draw_line(p1, v1, z+2.5, (0, 0, 255)) for walker in walkers_list: v2_hat = _orientation(walker.get_transform().rotation.yaw) s2 = np.linalg.norm(_numpy(walker.get_velocity())) if s2 < 0.05: v2_hat *= s2 p2 = -3.0 * v2_hat + _numpy(walker.get_location()) v2 = 8.0 * v2_hat # self._draw_line(p2, v2, z+2.5) collides, collision_point = get_collision(p1, v1, p2, v2) if collides: return walker return None def _is_vehicle_hazard(self, vehicle_list): z = self._vehicle.get_location().z o1 = _orientation(self._vehicle.get_transform().rotation.yaw) p1 = _numpy(self._vehicle.get_location()) s1 = max(7.5, 2.0 * np.linalg.norm(_numpy(self._vehicle.get_velocity()))) v1_hat = o1 v1 = s1 * v1_hat # self._draw_line(p1, v1, z+2.5, (255, 0, 0)) for target_vehicle in vehicle_list: if target_vehicle.id == self._vehicle.id: continue o2 = _orientation(target_vehicle.get_transform().rotation.yaw) p2 = _numpy(target_vehicle.get_location()) s2 = max( 5.0, 2.0 * np.linalg.norm(_numpy(target_vehicle.get_velocity()))) v2_hat = o2 v2 = s2 * v2_hat p2_p1 = p2 - p1 distance = np.linalg.norm(p2_p1) p2_p1_hat = p2_p1 / (distance + 1e-4) # self._draw_line(p2, v2, z+2.5, (255, 0, 0)) angle_to_car = np.degrees(np.arccos(v1_hat.dot(p2_p1_hat))) angle_between_heading = np.degrees(np.arccos(o1.dot(o2))) if angle_between_heading > 60.0 and not (angle_to_car < 15 and distance < s1): continue elif angle_to_car > 30.0: continue elif distance > s1: continue return target_vehicle return None
class ImageAgent(BaseAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.converter = Converter() self.net = ImageModel.load_from_checkpoint(path_to_conf_file) self.net.cuda() self.net.eval() def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def tick(self, input_data): result = super().tick(input_data) result['image'] = np.concatenate( tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) gps = self._get_position(result) far_node, _ = self._command_planner.run_step(gps) target = R.T.dot(far_node - gps) target *= 5.5 target += [128, 256] target = np.clip(target, 0, 256) result['target'] = target return result @torch.no_grad() def run_step_using_learned_controller(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() points, (target_cam, _) = self.net.forward(img, target) control = self.net.controller(points).cpu().squeeze() steer = control[0].item() desired_speed = control[1].item() speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() points, (target_cam, _) = self.net.forward(img, target) points_cam = points.clone().cpu() points_cam[..., 0] = (points_cam[..., 0] + 1) / 2 * img.shape[-1] points_cam[..., 1] = (points_cam[..., 1] + 1) / 2 * img.shape[-2] points_cam = points_cam.squeeze() points_world = self.converter.cam_to_world(points_cam).numpy() aim = (points_world[1] + points_world[0]) / 2.0 angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 # desired_speed *= (1 - abs(angle)) ** 2 speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control
class ImageAgent(BaseAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.converter = Converter() self.net = ImageModel.load_from_checkpoint(path_to_conf_file) self.net.cuda() self.net.eval() def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) self.save_images_path = Path(f'{SAVE_PATH_BASE}/images/{ROUTE_NAME}') #self.save_path.mkdir() def tick(self, input_data): result = super().tick(input_data) result['image'] = np.concatenate(tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 result['theta'] = theta #print((theta * 180 / np.pi)%360) R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) result['R'] = R gps = self._get_position(result) # method returns position in meters # transform route waypoints to overhead map view route = self._command_planner.run_step(gps) # oriented in world frame nodes = np.array([node for node, _ in route]) # (N,2) nodes = nodes - gps # center at agent position and rotate nodes = R.T.dot(nodes.T) # (2,2) x (2,N) = (2,N) nodes = nodes.T * 5.5 # (N,2) # to map frame (5.5 pixels per meter) nodes += [128,256] nodes = np.clip(nodes, 0, 256) commands = [command for _, command in route] # populate results result['num_waypoints'] = len(route) result['route_map'] = nodes result['commands'] = commands result['target'] = nodes[1] return result @torch.no_grad() def run_step_using_learned_controller(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() points, (target_cam, _) = self.net.forward(img, target) control = self.net.controller(points).cpu().squeeze() steer = control[0].item() desired_speed = control[1].item() speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display( tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) # prepare image model inputs img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() # forward through NN and process output points, (target_cam, _) = self.net.forward(img, target) #heatmap = _.cpu().numpy() #heatmap = cv2.flip(heatmap[0][0], 0) #cv2.imshow('heatmap', heatmap) points_cam = points.clone().cpu() points_cam[..., 0] = (points_cam[..., 0] + 1) / 2 * img.shape[-1] points_cam[..., 1] = (points_cam[..., 1] + 1) / 2 * img.shape[-2] points_cam = points_cam.squeeze() points_world = self.converter.cam_to_world(points_cam).numpy() tick_data['points_world'] = points_world # decide on control waypoint aim = (points_world[1] + points_world[0]) / 2.0 tick_data['aim_world'] = aim # compute steer angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) # compute throttle speed = tick_data['speed'] desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 # create control object control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG or SAVE_IMAGES: # make display image tick_data['points_cam'] = points.cpu().squeeze() tick_data['points_map'] = self.converter.cam_to_map(points_cam).numpy() self.debug_display( tick_data, target_cam.squeeze(), steer, throttle, brake, desired_speed) return control def debug_display(self, tick_data, target_cam, steer, throttle, brake, desired_speed): # make BEV image # transform aim from world to map aim_world = np.array(tick_data['aim_world']) aim_map = self.converter.world_to_map(torch.Tensor(aim_world)).numpy() # append to image model points and plot points_plot = np.vstack([tick_data['points_map'], aim_map]) points_plot = points_plot - [128,256] # center at origin points_plot = tick_data['R'].dot(points_plot.T).T points_plot = points_plot * -1 # why is this required? points_plot = points_plot + 256/2 # recenter origin in middle of plot _waypoint_img = self._command_planner.debug.img for x, y in points_plot: ImageDraw.Draw(_waypoint_img).ellipse((x-2, y-2, x+2, y+2), (0, 191, 255)) x, y = points_plot[-1] ImageDraw.Draw(_waypoint_img).ellipse((x-2, y-2, x+2, y+2), (255, 105, 147)) # make RGB images # draw center RGB image _rgb = Image.fromarray(tick_data['rgb']) _draw_rgb = ImageDraw.Draw(_rgb) for x, y in tick_data['points_cam']: # image model waypoints x = (x + 1)/2 * 256 y = (y + 1)/2 * 144 _draw_rgb.ellipse((x-2, y-2, x+2, y+2), (0, 191, 255)) # transform aim from world to cam aim_world = np.array(tick_data['aim_world']) aim_cam = self.converter.world_to_cam(torch.Tensor(aim_world)).numpy() x, y = aim_cam _draw_rgb.ellipse((x-2, y-2, x+2, y+2), (255, 105, 147)) # draw route waypoints in RGB image route_map = np.array(tick_data['route_map']) route_map = route_map[:3].squeeze() route_cam = self.converter.map_to_cam(torch.Tensor(route_map)).numpy() for i, (x, y) in enumerate(route_cam): if i == 0: # waypoint we just passed if y >= 139 or x <= 2 or x >= 254: # bottom of frame (behind us) continue color = (0, 255, 0) # green elif i == 1: # target color = (255, 0, 0) # red elif i == 2: # beyond target color = (139, 0, 139) # darkmagenta else: continue _draw_rgb.ellipse((x-2, y-2, x+2, y+2), color) _combined = Image.fromarray(np.hstack([tick_data['rgb_left'], _rgb, tick_data['rgb_right']])) _draw = ImageDraw.Draw(_combined) # draw debug text text_color = (139, 0, 139) #darkmagenta _draw.text((5, 10), 'Steer: %.3f' % steer, text_color) _draw.text((5, 30), 'Throttle: %.3f' % throttle, text_color) _draw.text((5, 50), 'Brake: %s' % brake, text_color) _draw.text((5, 70), 'Speed: %.3f' % tick_data['speed'], text_color) _draw.text((5, 90), 'Desired: %.3f' % desired_speed, text_color) cur_command, next_command = tick_data['commands'][:2] _draw.text((5, 110), f'Current: {cur_command}', text_color) _draw.text((5, 130), f'Next: {next_command}', text_color) # compose image to display/save _rgb_img = cv2.resize(np.array(_combined), DIM, interpolation=cv2.INTER_AREA) _save_img = Image.fromarray(np.hstack([_rgb_img, _waypoint_img])) _save_img = cv2.cvtColor(np.array(_save_img), cv2.COLOR_BGR2RGB) if self.step % 10 == 0 and SAVE_IMAGES: frame_number = self.step // 10 rep_number = int(os.environ.get('REP',0)) save_path = self.save_images_path / f'repetition_{rep_number:02d}' / f'{frame_number:06d}.png' cv2.imwrite(str(save_path), _save_img) if DEBUG: cv2.imshow('debug', _save_img) cv2.waitKey(1)
class AutoPilot(MapAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.save_path = None self.converter = Converter() self.save_images_path = pathlib.Path(f'{SAVE_PATH_BASE}/images/{ROUTE_NAME}') if path_to_conf_file and path_to_conf_file.split('/')[-1] != 'none': now = datetime.datetime.now() string = pathlib.Path(os.environ['ROUTES']).stem + '_' string += '_'.join(map(lambda x: '%02d' % x, (now.month, now.day, now.hour, now.minute, now.second))) print(string) self.save_path = pathlib.Path(path_to_conf_file) / string self.save_path.mkdir(exist_ok=False) (self.save_path / 'rgb').mkdir() (self.save_path / 'rgb_left').mkdir() (self.save_path / 'rgb_right').mkdir() (self.save_path / 'topdown').mkdir() (self.save_path / 'measurements').mkdir() def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def _get_angle_to(self, pos, theta, target): R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) aim = R.T.dot(target - pos) angle = -np.degrees(np.arctan2(-aim[1], aim[0])) angle = 0.0 if np.isnan(angle) else angle return angle def _get_control(self, target, far_target, tick_data): pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # Steering. angle_unnorm = self._get_angle_to(pos, theta, target) angle = angle_unnorm / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) steer = round(steer, 3) # Acceleration. angle_far_unnorm = self._get_angle_to(pos, theta, far_target) should_slow = abs(angle_far_unnorm) > 45.0 or abs(angle_unnorm) > 5.0 target_speed = 4 if should_slow else 7.0 brake = self._should_brake() target_speed = target_speed if not brake else 0.0 delta = np.clip(target_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) if brake: steer *= 0.5 throttle = 0.0 return steer, throttle, brake, target_speed def run_step(self, input_data, timestamp): if not self.initialized: self._init() if self.step % 100 == 0: index = (self.step // 100) % len(WEATHERS) self._world.set_weather(WEATHERS[index]) data = self.tick(input_data) gps = self._get_position(data) wpt_route = self._waypoint_planner.run_step(gps) cmd_route = self._command_planner.run_step(gps) cmd_nodes = np.array([node for node, _ in cmd_route]) cmd_cmds = [cmd for _, cmd in cmd_route] near_node, near_command = wpt_route[0] far_node, far_command = cmd_route[0] steer, throttle, brake, target_speed = self._get_control(near_node, far_node, data) topdown = data['topdown'] _topdown = Image.fromarray(COLOR[CONVERTER[topdown]]) _topdown_draw = ImageDraw.Draw(_topdown) _rgb = Image.fromarray(data['rgb']) _rgb_draw = ImageDraw.Draw(_rgb) if (SAVE_IMAGES or HAS_DISPLAY) and not self.save_path: r = 2 theta = data['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) nodes = cmd_nodes - gps nodes = R.T.dot(nodes.T).T nodes = nodes * 5.5 nodes_bev = nodes.copy() + [256, 256] nodes_bev = nodes_bev[:3] for i, (x,y) in enumerate(nodes_bev): if i == 0: color = (0, 255, 0) elif i == 1: # target color = (255, 0, 0) elif i == 2: color = (139, 0, 139) _topdown_draw.ellipse((x-2*r, y-2*r, x+2*r, y+2*r), color) nodes_cam = nodes.copy()[:3].squeeze() nodes_cam = nodes_cam + [128, 256] nodes_cam = np.clip(nodes_cam, 0, 256) nodes_cam = self.converter.map_to_cam(torch.Tensor(nodes_cam)).numpy() for i, (x,y) in enumerate(nodes_cam[:3]): if i == 0: if not (0 < y < 143 and 0 < x < 255): continue color = (0, 255, 0) elif i == 1: # target color = (255, 0, 0) elif i == 2: color = (139, 0, 139) _rgb_draw.ellipse((x-r, y-r, x+r, y+r), color) #rgb = np.hstack((data['rgb_left'], data['rgb'], data['rgb_right'])) _rgb = Image.fromarray(np.hstack((data['rgb_left'], _rgb, data['rgb_right']))) _draw = ImageDraw.Draw(_rgb) text_color = (139, 0, 139) #darkmagenta _draw.text((5, 10), 'Steer: %.3f' % steer, text_color) _draw.text((5, 30), 'Throttle: %.3f' % throttle, text_color) _draw.text((5, 50), 'Brake: %s' % brake, text_color) _draw.text((5, 70), 'Speed: %.3f' % data['speed'], text_color) _draw.text((5, 90), 'Target: %.3f' % target_speed, text_color) cur_command, next_command = cmd_cmds[:2] _draw.text((5, 110), f'Current: {cur_command}', text_color) _draw.text((5, 130), f'Next: {next_command}', text_color) # (256, 144) -> (256/144*256, 256) _rgb = _rgb.resize((int(256 / _rgb.size[1] * _rgb.size[0]), 256)) _topdown = _topdown.resize((256,256)) _combined = Image.fromarray(np.hstack((_rgb, _topdown))) if self.step % 10 == 0 and SAVE_IMAGES: _save_img = cv2.cvtColor(np.array(_combined), cv2.COLOR_BGR2RGB) frame_number = self.step//10 rep_number = int(os.environ.get('REP', 0)) save_path = self.save_images_path / f'repetition_{rep_number:02d}' / f'{frame_number:06d}.png' cv2.imwrite(str(save_path), _save_img) control = carla.VehicleControl() control.steer = steer + 1e-2 * np.random.randn() control.throttle = throttle control.brake = float(brake) if self.step % 10 == 0 and self.save_path: self.save(far_node, near_command, steer, throttle, brake, target_speed, data) if HAS_DISPLAY: cv2.imshow('map', cv2.cvtColor(np.array(_combined), cv2.COLOR_BGR2RGB)) cv2.waitKey(1) return control def save(self, far_node, near_command, steer, throttle, brake, target_speed, tick_data): frame = self.step // 10 pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] data = { 'x': pos[0], 'y': pos[1], 'theta': theta, 'speed': speed, 'target_speed': target_speed, 'x_command': far_node[0], 'y_command': far_node[1], 'command': near_command.value, 'steer': steer, 'throttle': throttle, 'brake': brake, } (self.save_path / 'measurements' / ('%04d.json' % frame)).write_text(str(data)) Image.fromarray(tick_data['rgb']).save(self.save_path / 'rgb' / ('%04d.png' % frame)) Image.fromarray(tick_data['rgb_left']).save(self.save_path / 'rgb_left' / ('%04d.png' % frame)) Image.fromarray(tick_data['rgb_right']).save(self.save_path / 'rgb_right' / ('%04d.png' % frame)) Image.fromarray(tick_data['topdown']).save(self.save_path / 'topdown' / ('%04d.png' % frame)) def _should_brake(self): actors = self._world.get_actors() vehicle = self._is_vehicle_hazard(actors.filter('*vehicle*')) light = self._is_light_red(actors.filter('*traffic_light*')) walker = self._is_walker_hazard(actors.filter('*walker*')) return any(x is not None for x in [vehicle, light, walker]) def _draw_line(self, p, v, z, color=(255, 0, 0)): if not DEBUG: return p1 = _location(p[0], p[1], z) p2 = _location(p[0]+v[0], p[1]+v[1], z) color = carla.Color(*color) self._world.debug.draw_line(p1, p2, 0.25, color, 0.01) def _is_light_red(self, lights_list): if self._vehicle.get_traffic_light_state() != carla.libcarla.TrafficLightState.Green: affecting = self._vehicle.get_traffic_light() for light in self._traffic_lights: if light.id == affecting.id: return affecting return None def _is_walker_hazard(self, walkers_list): z = self._vehicle.get_location().z p1 = _numpy(self._vehicle.get_location()) v1 = 10.0 * _orientation(self._vehicle.get_transform().rotation.yaw) self._draw_line(p1, v1, z+2.5, (0, 0, 255)) for walker in walkers_list: v2_hat = _orientation(walker.get_transform().rotation.yaw) s2 = np.linalg.norm(_numpy(walker.get_velocity())) if s2 < 0.05: v2_hat *= s2 p2 = -3.0 * v2_hat + _numpy(walker.get_location()) v2 = 8.0 * v2_hat self._draw_line(p2, v2, z+2.5) collides, collision_point = get_collision(p1, v1, p2, v2) if collides: return walker return None def _is_vehicle_hazard(self, vehicle_list): z = self._vehicle.get_location().z o1 = _orientation(self._vehicle.get_transform().rotation.yaw) p1 = _numpy(self._vehicle.get_location()) s1 = max(7.5, 2.0 * np.linalg.norm(_numpy(self._vehicle.get_velocity()))) v1_hat = o1 v1 = s1 * v1_hat self._draw_line(p1, v1, z+2.5, (255, 0, 0)) for target_vehicle in vehicle_list: if target_vehicle.id == self._vehicle.id: continue o2 = _orientation(target_vehicle.get_transform().rotation.yaw) p2 = _numpy(target_vehicle.get_location()) s2 = max(5.0, 2.0 * np.linalg.norm(_numpy(target_vehicle.get_velocity()))) v2_hat = o2 v2 = s2 * v2_hat p2_p1 = p2 - p1 distance = np.linalg.norm(p2_p1) p2_p1_hat = p2_p1 / (distance + 1e-4) self._draw_line(p2, v2, z+2.5, (255, 0, 0)) angle_to_car = np.degrees(np.arccos(v1_hat.dot(p2_p1_hat))) angle_between_heading = np.degrees(np.arccos(o1.dot(o2))) if angle_between_heading > 60.0 and not (angle_to_car < 15 and distance < s1): continue elif angle_to_car > 30.0: continue elif distance > s1: continue return target_vehicle return None
class ImageAgent(BaseAgent): def setup(self, path_to_conf_file, data_folder, route_folder, k, model_path): super().setup(path_to_conf_file, data_folder, route_folder, k, model_path) self.converter = Converter() self.net = ImageModel.load_from_checkpoint(path_to_conf_file) self.data_folder = data_folder self.route_folder = route_folder self.scene_number = k self.weather_file = self.route_folder + "/weather_data.csv" self.model_path = model_path self.model_vae = None self.net.cuda() self.net.eval() self.run = 0 self.risk = 0 self.state = [] self.monitors = [] self.blur_queue = Queue(maxsize=1) self.occlusion_queue = Queue(maxsize=1) self.am_queue = Queue(maxsize=1) self.pval_queue = Queue(maxsize=1) self.sval_queue = Queue(maxsize=1) self.mval_queue = Queue(maxsize=1) self.avg_risk_queue = Queue(maxsize=1) self.calib_set = [] self.result = [] self.blur = [] self.occlusion = [] self.detector_file = None self.detector_file = None K.clear_session() config = tf.ConfigProto() sess = tf.Session(config=config) set_session(sess) with open(self.model_path + 'auto_model.json', 'r') as jfile: self.model_vae = model_from_json(jfile.read()) self.model_vae.load_weights(self.model_path + 'auto_model.h5') self.model_vae._make_predict_function() self.fields = [ 'step', 'monitor_result', 'risk_score', 'rgb_blur', 'rgb_left_blur', 'rgb_right_blur', 'rgb_occluded', 'rgb_left_occluded', 'rgb_right_occluded', ] self.weather, self.run_number = process_weather_data( self.weather_file, self.scene_number) print(self.weather) with open(self.model_path + 'calibration.csv', 'r') as file: reader = csv.reader(file) for row in reader: self.calib_set.append(row) def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def blur_detection(self, result): self.blur = [] fm1, rgb_blur = blur_detector(result['rgb'], threshold=20) fm2, rgb_left_blur = blur_detector(result['rgb_left'], threshold=20) fm3, rgb_right_blur = blur_detector(result['rgb_right'], threshold=20) self.blur.append(rgb_blur) self.blur.append(rgb_left_blur) self.blur.append(rgb_right_blur) self.blur_queue.put(self.blur) def occlusion_detection(self, result): self.occlusion = [] percent1, rgb_occluded = occlusion_detector(result['rgb'], threshold=25) percent2, rgb_left_occluded = occlusion_detector(result['rgb_left'], threshold=25) percent3, rgb_right_occluded = occlusion_detector(result['rgb_right'], threshold=25) self.occlusion.append(rgb_occluded) self.occlusion.append(rgb_left_occluded) self.occlusion.append(rgb_right_occluded) self.occlusion_queue.put(self.occlusion) def integrand1(self, p_anomaly): result = 0.0 for i in range(len(p_anomaly)): result += p_anomaly[i] result = result / len(p_anomaly) return result def integrand(self, k, p_anomaly): result = 1.0 for i in range(len(p_anomaly)): result *= k * (p_anomaly[i]**(k - 1.0)) return result def mse(self, imageA, imageB): err = np.mean(np.power(imageA - imageB, 2), axis=1) return err def assurance_monitor(self, dist): if (self.step == 0): p_anomaly = [] prev_value = [] else: p_anomaly = self.pval_queue.get() prev_value = self.sval_queue.get() anomaly = 0 m = 0 delta = 10 threshold = 20 sliding_window = 15 threshold = 10.0 for i in range(len(self.calib_set)): if (float(dist) <= float(self.calib_set[i][0])): anomaly += 1 p_value = anomaly / len(self.calib_set) if (p_value < 0.005): p_anomaly.append(0.005) else: p_anomaly.append(p_value) if (len(p_anomaly)) >= sliding_window: p_anomaly = p_anomaly[-1 * sliding_window:] m = integrate.quad(self.integrand, 0.0, 1.0, args=(p_anomaly)) m_val = round(math.log(m[0]), 2) if (self.step == 0): S = 0 S_prev = 0 else: S = max(0, prev_value[0] + prev_value[1] - delta) prev_value = [] S_prev = S m_prev = m[0] prev_value.append(S_prev) prev_value.append(m_prev) self.pval_queue.put(p_anomaly) self.sval_queue.put(prev_value) self.mval_queue.put(m_val) def risk_computation(self, weather, blur_queue, am_queue, occlusion_queue, fault_scenario, fault_type, fault_time, fault_step): monitors = [] faults = [] faults.append(fault_type) blur = self.blur_queue.get() occlusion = self.occlusion_queue.get() mval = self.mval_queue.get() if (self.step == 0): avg_risk = [] else: avg_risk = self.avg_risk_queue.get() monitors = blur + occlusion state = {"enviornment": {}, "fault_modes": None, "monitor_values": {}} for i in range(len(weather)): label = ENV_LABELS[i] state["enviornment"][label] = weather[i] state["fault_modes"] = fault_type for j in range(len(monitors)): label = MONITOR_LABELS[j] state["monitor_values"][label] = monitors[j] state["monitor_values"]["lec_martingale"] = mval fault_modes = state["fault_modes"][0] environment = state["enviornment"] monitor_values = state["monitor_values"] if (fault_scenario == 0): fault_modes = state["fault_modes"][0] if (fault_scenario == 1): if (self.step >= fault_step[0] and self.step < fault_step[0] + fault_time[0]): fault_modes = state["fault_modes"][0] if (fault_scenario > 1): if (self.step >= fault_step[0] and self.step < fault_step[0] + fault_time[0]): fault_modes = state["fault_modes"][0] elif (self.step >= fault_step[1] and self.step < fault_step[1] + fault_time[1]): fault_modes = state["fault_modes"][1] bowtie = BowTie() r_t1_top = bowtie.rate_t1(state) * (1 - bowtie.prob_b1(state, fault_modes)) r_t2_top = bowtie.rate_t2(state) * (1 - bowtie.prob_b2(state, fault_modes)) r_top = r_t1_top + r_t2_top r_c1 = r_top * (1 - bowtie.prob_b3(state)) print("Dynamic Risk Score:%f" % r_c1) avg_risk.append(r_c1) if (len(avg_risk)) >= 20: avg_risk = avg_risk[-1 * 20:] #m = integrate.cumtrapz(avg_risk) m = self.integrand1(avg_risk) self.avg_risk_queue.put(avg_risk) dict = [{ 'step': self.step, 'monitor_result': mval, 'risk_score': r_c1, 'rgb_blur': blur[0], 'rgb_left_blur': blur[1], 'rgb_right_blur': blur[2], 'rgb_occluded': occlusion[0], 'rgb_left_occluded': occlusion[1], 'rgb_right_occluded': occlusion[2] }] if (self.step == 0): self.detector_file = self.data_folder + "/run%d.csv" % ( self.scene_number) file_exists = os.path.isfile(self.detector_file) with open(self.detector_file, 'a') as csvfile: # creating a csv dict writer object writer = csv.DictWriter(csvfile, fieldnames=self.fields) if not file_exists: writer.writeheader() writer.writerows(dict) return m, mval, blur[0], blur[1], blur[2], occlusion[0], occlusion[ 1], occlusion[2] def tick(self, input_data): self.time = time.time() result = super().tick(input_data) result['rgb_detector'] = cv2.resize(result['rgb'], (224, 224)) result['rgb_detector_left'] = cv2.resize(result['rgb_left'], (224, 224)) result['rgb_detector_right'] = cv2.resize(result['rgb_right'], (224, 224)) result['rgb'] = cv2.resize(result['rgb'], (256, 144)) result['rgb_left'] = cv2.resize(result['rgb_left'], (256, 144)) result['rgb_right'] = cv2.resize(result['rgb_right'], (256, 144)) detection_image = cv2.cvtColor(result['rgb_detector_right'], cv2.COLOR_BGR2RGB) detection_image = detection_image / 255. detection_image = np.reshape(detection_image, [ -1, detection_image.shape[0], detection_image.shape[1], detection_image.shape[2] ]) #B-VAE reconstruction based assurance monitor # TODO: Move this prediction into a thread. I had problems of using keras model in threads. predicted_reps = self.model_vae.predict_on_batch(detection_image) dist = np.square(np.subtract(np.array(predicted_reps), detection_image)).mean() #start other detectors BlurDetectorThread = Thread(target=self.blur_detection, args=(result, )) BlurDetectorThread.daemon = True OccusionDetectorThread = Thread(target=self.occlusion_detection, args=(result, )) OccusionDetectorThread.daemon = True AssuranceMonitorThread = Thread( target=self.assurance_monitor, args=(dist, )) #image,model,calibration_set,pval_queue,sval_queue AssuranceMonitorThread.daemon = True AssuranceMonitorThread.start() BlurDetectorThread.start() OccusionDetectorThread.start() result['image'] = np.concatenate( tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) gps = self._get_position(result) result['gps_y'] = gps[0] result['gps_x'] = gps[1] far_node, _ = self._command_planner.run_step(gps) result['actual_y'] = far_node[0] result['actual_x'] = far_node[1] result['far_node'] = far_node target = R.T.dot(far_node - gps) target *= 5.5 target += [128, 256] target = np.clip(target, 0, 256) #waypoints_left = self._command_planner.get_waypoints_remaining(gps) #print("step:%d waypoints:%d"%(self.step,(9-waypoints_left))) #Synthetically add noise to the radar data based on the weather if (self.weather[0] <= "20.0" and self.weather[1] <= "20.0" and self.weather[2] <= "20.0"): result['cloud'][0] = result['cloud'][0] result['cloud_right'][0] = result['cloud_right'][0] elif ((self.weather[0] > "20.0" and self.weather[0] < "50.0") and (self.weather[1] > "20.0" and self.weather[1] < "50.0") and (self.weather[2] > "20.0" and self.weather[2] < "50.0")): noise = np.random.normal(0, 0.5, result['cloud'][0].shape) result['cloud'][0] += noise result['cloud_right'][0] += noise elif ((self.weather[0] > "50.0" and self.weather[0] < "70.0") and (self.weather[1] > "50.0" and self.weather[1] < "70.0") and (self.weather[2] > "50.0" and self.weather[2] < "70.0")): noise = np.random.normal(0, 1.5, result['cloud'][0].shape) result['cloud'][0] += noise result['cloud_right'][0] += noise elif ((self.weather[0] > "70.0" and self.weather[0] < "100.0") and (self.weather[1] > "70.0" and self.weather[1] < "100.0") and (self.weather[2] > "70.0" and self.weather[2] < "100.0")): noise = np.random.normal(0, 2, result['cloud'][0].shape) result['cloud'][0] += noise result['cloud_right'][0] += noise else: noise = np.random.normal(0, 0.5, result['cloud'][0].shape) result['cloud'][0] += noise result['cloud_right'][0] += noise result['target'] = target #Extract minimum distance from the 2 RADAR's. Center and right RADAR's object_depth = [] object_vel = [] for i in range(len(result['cloud'])): object_depth.append(result['cloud'][i][0]) object_vel.append(result['cloud'][i][3]) index = np.argmin(object_depth) result['object_velocity'] = object_vel[index] result['object_distance'] = abs(min(object_depth)) object_depth_right = [] object_vel_right = [] for i in range(len(result['cloud_right'])): object_depth_right.append(result['cloud_right'][i][0]) object_vel_right.append(result['cloud_right'][i][3]) index = np.argmin(object_depth_right) result['object_velocity_right'] = object_vel_right[index] result['object_distance_right'] = abs(min(object_depth_right)) #Join the threads BlurDetectorThread.join() OccusionDetectorThread.join() AssuranceMonitorThread.join() #Compute risk # TODO: We could do it in a thread in the future when risk is computed only once every few cycles. Now risk is computed every cycle t = time.time() risk, mval, blur0, blur1, blur2, Occlusion0, Occlusion1, Occlusion2 = self.risk_computation( self.weather, self.blur_queue, self.am_queue, self.occlusion_queue, result["fault_scenario"], result["fault_type"], result["fault_duration"], result["fault_step"]) like = 1 - math.exp(-risk) result['time'] = time.time() - t result['risk'] = risk del predicted_reps gc.collect() return result #Step function that runs the Primary LEC and the secondary AEBS controller @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() time_val = 0.0 tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() #Primary LEC controller with torch.no_grad(): points, (target_cam, _) = self.net.forward(img, target) points_cam = points.clone().cpu() points_cam[..., 0] = (points_cam[..., 0] + 1) / 2 * img.shape[-1] points_cam[..., 1] = (points_cam[..., 1] + 1) / 2 * img.shape[-2] points_cam = points_cam.squeeze() points_world = self.converter.cam_to_world(points_cam).numpy() aim = (points_world[1] + points_world[0]) / 2.0 angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 speed = tick_data['speed'] stopping_distance = ((speed * speed) / 13.72) #Supervisory AEBS controller if (speed >= 0.5): if (tick_data['object_distance'] < (1.5 * speed + (speed * speed) / 13.72) or tick_data['object_distance_right'] < (1.5 * speed + (speed * speed) / 13.72)): brake = True val = 1 print("emergency") else: brake = False elif (speed < 0.5): if ((tick_data['object_distance'] < 3) or (tick_data['object_distance_right'] < 3)): brake = True val = 1 print("emergency") else: brake = False else: brake = desired_speed < 0.2 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) #Log AV stats fields = [ 'step', 'stopping_distance', 'Radar_distance', 'speed', 'steer', 'gps_x', 'gps_y', 'actual_x', 'actual_y', 'time', 'risk_time' ] dict = [{ 'step': self.step, 'stopping_distance': abs(stopping_distance), 'Radar_distance': tick_data['object_distance'], 'speed': speed, 'steer': steer, 'gps_x': tick_data['gps_x'], 'gps_y': tick_data['gps_y'], 'actual_x': tick_data['actual_x'], 'actual_y': tick_data['actual_y'], 'time': float(time.time() - self.time), 'risk_time': tick_data['time'] }] if (self.step == 0): self.braking_file = self.data_folder + "/emergency_braking%d.csv" % ( self.scene_number) file_exists = os.path.isfile(self.braking_file) with open(self.braking_file, 'a') as csvfile: # creating a csv dict writer object writer = csv.DictWriter(csvfile, fieldnames=fields) if not file_exists: writer.writeheader() writer.writerows(dict) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control
class PrivilegedAgent(MapAgent): def setup(self, path_to_conf_file): # make conf file a json file? # store hparams for map model # can extend to store all of the save paths so we don't rely on os environ stuff # can copy this to the corresponding save path super().setup(path_to_conf_file) self.converter = Converter() self.net = MapModel.load_from_checkpoint(path_to_conf_file) self.net.cuda() self.net.eval() def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) self.save_images_path = Path(f'{SAVE_PATH_BASE}/images/{ROUTE_NAME}') #self.save_path.mkdir() def tick(self, input_data): result = super().tick(input_data) result['image'] = np.concatenate( tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 result['theta'] = theta #print((theta * 180 / np.pi)%360) R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) result['R'] = R gps = self._get_position(result) # method returns position in meters # transform route waypoints to overhead map view route = self._command_planner.run_step(gps) # oriented in world frame nodes = np.array([node for node, _ in route]) # (N,2) nodes = nodes - gps # center at agent position and rotate nodes = R.T.dot(nodes.T) # (2,2) x (2,N) = (2,N) nodes = nodes.T * 5.5 # (N,2) # to map frame (5.5 pixels per meter) nodes += [128, 256] #nodes = np.clip(nodes, 0, 256) commands = [command for _, command in route] target = np.clip(nodes[1], 0, 256) # populate results result['num_waypoints'] = len(route) result['route_map'] = nodes result['commands'] = commands result['target'] = target return result @torch.no_grad() def run_step_using_learned_controller(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() #points, (target_cam, _) = self.net.forward(img, target) points, (target_cam, _) = self.net.forward(img, target) control = self.net.controller(points).cpu().squeeze() steer = control[0].item() desired_speed = control[1].item() speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) topdown = Image.fromarray(tick_data['topdown']) topdown = topdown.crop((128, 0, 128 + 256, 256)) topdown = np.array(topdown) topdown = preprocess_semantic(topdown) topdown = topdown[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() #points, (target_cam, _) = self.net.forward(topdown, target) points = self.net.forward(topdown, target) # world frame points_map = points.clone().cpu().squeeze() # what's this conversion for? # was this originally normalized for training stability or something? points_map = points_map + 1 points_map = points_map / 2 * 256 points_map = np.clip(points_map, 0, 256) points_cam = self.converter.map_to_cam(points_map).numpy() points_world = self.converter.map_to_world(points_map).numpy() points_map = points_map.numpy() tick_data['points_map'] = points_map tick_data['points_cam'] = points_cam tick_data['points_world'] = points_world #img = tick_data['image'] aim = (points_world[1] + points_world[0]) / 2.0 angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 tick_data['aim_world'] = aim speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) #print(timestamp) # GAMETIME if DEBUG or SAVE_IMAGES: # transform image model cam points to overhead BEV image (spectator frame?) self.debug_display(tick_data, steer, throttle, brake, desired_speed) return control def debug_display(self, tick_data, steer, throttle, brake, desired_speed, r=2): topdown = tick_data['topdown'] _topdown = Image.fromarray(COLOR[CONVERTER[topdown]]) _topdown_draw = ImageDraw.Draw(_topdown) # model points points_map = tick_data['points_map'] points_td = points_map + [128, 0] for i, (x, y) in enumerate(points_td): _topdown_draw.ellipse((x - 2 * r, y - 2 * r, x + 2 * r, y + 2 * r), (0, 191, 255)) route_map = tick_data['route_map'] route_td = route_map + [128, 0] # control point aim_world = np.array(tick_data['aim_world']) aim_map = self.converter.world_to_map(torch.Tensor(aim_world)).numpy() aim_map = aim_map + [128, 0] x, y = aim_map _topdown_draw.ellipse((x - 2, y - 2, x + 2, y + 2), (255, 105, 147)) # route waypoints for i, (x, y) in enumerate(route_td[:3]): if i == 0: color = (0, 255, 0) elif i == 1: color = (255, 0, 0) elif i == 2: color = (139, 0, 139) _topdown_draw.ellipse((x - 2 * r, y - 2 * r, x + 2 * r, y + 2 * r), color) # make RGB images # draw center RGB image _rgb = Image.fromarray(tick_data['rgb']) _draw_rgb = ImageDraw.Draw(_rgb) for x, y in tick_data['points_cam']: # image model waypoints #x = (x + 1)/2 * 256 #y = (y + 1)/2 * 144 _draw_rgb.ellipse((x - 2, y - 2, x + 2, y + 2), (0, 191, 255)) # transform aim from world to cam aim_cam = self.converter.world_to_cam(torch.Tensor(aim_world)).numpy() x, y = aim_cam _draw_rgb.ellipse((x - 2, y - 2, x + 2, y + 2), (255, 105, 147)) # draw route waypoints in RGB image route_map = np.array(tick_data['route_map']) route_map = np.clip(route_map, 0, 256) route_map = route_map[:3].squeeze() # just the next couple route_cam = self.converter.map_to_cam(torch.Tensor(route_map)).numpy() for i, (x, y) in enumerate(route_cam): if i == 0: # waypoint we just passed if not (0 < y < 143 and 0 < x < 255): continue color = (0, 255, 0) # green elif i == 1: # target color = (255, 0, 0) # red elif i == 2: # beyond target color = (139, 0, 139) # darkmagenta else: continue _draw_rgb.ellipse((x - 2, y - 2, x + 2, y + 2), color) _combined = Image.fromarray( np.hstack([tick_data['rgb_left'], _rgb, tick_data['rgb_right']])) _draw = ImageDraw.Draw(_combined) # draw debug text text_color = (139, 0, 139) #darkmagenta _draw.text((5, 10), 'Steer: %.3f' % steer, text_color) _draw.text((5, 30), 'Throttle: %.3f' % throttle, text_color) _draw.text((5, 50), 'Brake: %s' % brake, text_color) _draw.text((5, 70), 'Speed: %.3f' % tick_data['speed'], text_color) _draw.text((5, 90), 'Desired: %.3f' % desired_speed, text_color) cur_command, next_command = tick_data['commands'][:2] _draw.text((5, 110), f'Current: {cur_command}', text_color) _draw.text((5, 130), f'Next: {next_command}', text_color) _rgb_img = _combined.resize( (int(256 / _combined.size[1] * _combined.size[0]), 256)) _topdown = _topdown.resize((256, 256)) _save_img = Image.fromarray(np.hstack([_rgb_img, _topdown])) _save_img = cv2.cvtColor(np.array(_save_img), cv2.COLOR_BGR2RGB) if self.step % 10 == 0 and SAVE_IMAGES: frame_number = self.step // 10 rep_number = int(os.environ.get('REP', 0)) save_path = self.save_images_path / f'repetition_{rep_number:02d}' / f'{frame_number:06d}.png' cv2.imwrite(str(save_path), _save_img) if DEBUG: cv2.imshow('debug', _save_img) cv2.waitKey(1)
class ImageAgent(BaseAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.converter = Converter() self.net = ImageModel.load_from_checkpoint(path_to_conf_file) self.net.cuda() self.net.eval() # addition: modified from leaderboard/team_code/auto_pilot.py def save(self, steer, throttle, brake, tick_data): # frame = self.step // 10 frame = self.step pos = self._get_position(tick_data) far_command = tick_data['far_command'] speed = tick_data['speed'] center = os.path.join('rgb', ('%04d.png' % frame)) left = os.path.join('rgb_left', ('%04d.png' % frame)) right = os.path.join('rgb_right', ('%04d.png' % frame)) topdown = os.path.join('topdown', ('%04d.png' % frame)) rgb_with_car = os.path.join('rgb_with_car', ('%04d.png' % frame)) data_row = ','.join([str(i) for i in [frame, far_command, speed, steer, throttle, brake, str(center), str(left), str(right)]]) with (self.save_path / 'measurements.csv' ).open("a") as f_out: f_out.write(data_row+'\n') Image.fromarray(tick_data['rgb']).save(self.save_path / center) Image.fromarray(tick_data['rgb_left']).save(self.save_path / left) Image.fromarray(tick_data['rgb_right']).save(self.save_path / right) # addition Image.fromarray(COLOR[CONVERTER[tick_data['topdown']]]).save(self.save_path / topdown) Image.fromarray(tick_data['rgb_with_car']).save(self.save_path / rgb_with_car) ######################################################################## # log necessary info for action-based if self.args.save_action_based_measurements: from affordances import get_driving_affordances self._pedestrian_forbidden_distance = 10.0 self._pedestrian_max_detected_distance = 50.0 self._vehicle_forbidden_distance = 10.0 self._vehicle_max_detected_distance = 50.0 self._tl_forbidden_distance = 10.0 self._tl_max_detected_distance = 50.0 self._speed_detected_distance = 10.0 self._default_target_speed = 10 self._angle = None current_affordances = get_driving_affordances(self, self._pedestrian_forbidden_distance, self._pedestrian_max_detected_distance, self._vehicle_forbidden_distance, self._vehicle_max_detected_distance, self._tl_forbidden_distance, self._tl_max_detected_distance, self._angle_rad, self._default_target_speed, self._target_speed, self._speed_detected_distance, angle=True) is_vehicle_hazard = current_affordances['is_vehicle_hazard'] is_red_tl_hazard = current_affordances['is_red_tl_hazard'] is_pedestrian_hazard = current_affordances['is_pedestrian_hazard'] forward_speed = current_affordances['forward_speed'] relative_angle = current_affordances['relative_angle'] target_speed = current_affordances['target_speed'] closest_pedestrian_distance = current_affordances['closest_pedestrian_distance'] closest_vehicle_distance = current_affordances['closest_vehicle_distance'] closest_red_tl_distance = current_affordances['closest_red_tl_distance'] log_folder = str(self.save_path / 'affordance_measurements') if not os.path.exists(log_folder): os.mkdir(log_folder) log_path = os.path.join(log_folder, f'{self.step:06}.json') ego_transform = self._vehicle.get_transform() ego_location = ego_transform.location ego_rotation = ego_transform.rotation ego_velocity = self._vehicle.get_velocity() brake_noise = 0.0 throttle_noise = 0.0 # 1.0 -> 0.0 steer_noise = 0.0 # NaN -> 0.0 # class RoadOption map_roadoption_to_action_based_roadoption = {-1:2, 1:3, 2:4, 3:5, 4:2, 5:2, 6:2} # save info for action-based rep json_log_data = { "brake": float(brake), "closest_red_tl_distance": closest_red_tl_distance, "throttle": throttle, "directions": float(map_roadoption_to_action_based_roadoption[far_command.value]), "brake_noise": brake_noise, "is_red_tl_hazard": is_red_tl_hazard, "opponents": {}, "closest_pedestrian_distance": closest_pedestrian_distance, "is_pedestrian_hazard": is_pedestrian_hazard, "lane": {}, "is_vehicle_hazard": is_vehicle_hazard, "throttle_noise": throttle_noise, "ego_actor": { "velocity": [ ego_velocity.x, ego_velocity.y, ego_velocity.z ], "position": [ ego_location.x, ego_location.y, ego_location.z ], "orientation": [ ego_rotation.roll, ego_rotation.pitch, ego_rotation.yaw ] }, "hand_brake": False, "steer_noise": steer_noise, "reverse": False, "relative_angle": relative_angle, "closest_vehicle_distance": closest_vehicle_distance, "walkers": {}, "forward_speed": forward_speed, "steer": steer, "target_speed": target_speed } with open(log_path, 'w') as f_out: json.dump(json_log_data, f_out, indent=4) def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) # addition: self._vehicle = CarlaDataProvider.get_hero_actor() self._world = self._vehicle.get_world() # ------------------------------------------------------- # add a local_planner in order to estimate relative angle # self.target_speed = 10 # args_lateral_dict = { # 'K_P': 1, # 'K_D': 0.4, # 'K_I': 0, # 'dt': 1.0/10.0} # self._local_planner = LocalPlanner( # self._vehicle, opt_dict={'target_speed' : self.target_speed, # 'lateral_control_dict':args_lateral_dict}) # self._hop_resolution = 2.0 # self._path_seperation_hop = 2 # self._path_seperation_threshold = 0.5 # self._grp = None # # self._map = CarlaDataProvider.get_map() # route = [(self._map.get_waypoint(x[0].location), x[1]) for x in self._global_plan_world_coord] # # self._local_planner.set_global_plan(route) def tick(self, input_data): result = super().tick(input_data) result['image'] = np.concatenate(tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) rgb_with_car = cv2.cvtColor(input_data['rgb_with_car'][1][:, :, :3], cv2.COLOR_BGR2RGB) result['rgb_with_car'] = rgb_with_car result['radar_central'] = input_data['radar_central'] theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) gps = self._get_position(result) # modification far_node, far_command = self._command_planner.run_step(gps) target = R.T.dot(far_node - gps) target *= 5.5 target += [128, 256] target = np.clip(target, 0, 256) result['target'] = target # addition: self._actors = self._world.get_actors() self._traffic_lights = get_nearby_lights(self._vehicle, self._actors.filter('*traffic_light*')) result['far_command'] = far_command topdown = input_data['map'][1][:, :, 2] topdown = draw_traffic_lights(topdown, self._vehicle, self._traffic_lights) result['topdown'] = topdown return result @torch.no_grad() def run_step_using_learned_controller(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() import random torch.manual_seed(2) torch.cuda.manual_seed_all(2) torch.backends.cudnn.deterministic = True np.random.seed(1) random.seed(1) device = torch.device('cuda') torch.backends.cudnn.benchmark = False points, (target_cam, _) = self.net.forward(img, target) control = self.net.controller(points).cpu().squeeze() steer = control[0].item() desired_speed = control[1].item() speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display( tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) radar_data = tick_data['radar_central'][1] img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() points, (target_cam, _) = self.net.forward(img, target) points_cam = points.clone().cpu() if self.step == 0: print('\n'*5) print('step :', self.step) # print('radar') # print(radar_data.shape) # print(radar_data) # print(np.max(radar_data, axis=0)) print('image', np.sum(tick_data['image'])) # print('img', torch.sum(img)) # print('target', target) # print('points', points) print('\n'*5) points_cam[..., 0] = (points_cam[..., 0] + 1) / 2 * img.shape[-1] points_cam[..., 1] = (points_cam[..., 1] + 1) / 2 * img.shape[-2] points_cam = points_cam.squeeze() points_world = self.converter.cam_to_world(points_cam).numpy() aim = (points_world[1] + points_world[0]) / 2.0 angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) self._angle_rad = np.radians(angle) angle = angle / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 # desired_speed *= (1 - abs(angle)) ** 2 self._target_speed = desired_speed speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) # addition: from leaderboard/team_code/auto_pilot.py if self.step == 0: title_row = ','.join(['frame_id', 'far_command', 'speed', 'steering', 'throttle', 'brake', 'center', 'left', 'right']) with (self.save_path / 'measurements.csv' ).open("a") as f_out: f_out.write(title_row+'\n') if self.step % 1 == 0: self.gather_info((steer, throttle, float(brake), speed)) record_every_n_steps = self.record_every_n_step if self.step % record_every_n_steps == 0: self.save(steer, throttle, brake, tick_data) return control
class AutoPilot(MapAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.save_path = None parent_folder = os.environ['SAVE_FOLDER'] string = pathlib.Path(os.environ['ROUTES']).stem self.save_path = pathlib.Path(parent_folder) / string def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def _get_angle_to(self, pos, theta, target): R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) aim = R.T.dot(target - pos) angle = -np.degrees(np.arctan2(-aim[1], aim[0])) angle = 0.0 if np.isnan(angle) else angle return angle def _get_control(self, target, far_target, tick_data, _draw): pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # Steering. angle_unnorm = self._get_angle_to(pos, theta, target) angle = angle_unnorm / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) steer = round(steer, 3) # Acceleration. angle_far_unnorm = self._get_angle_to(pos, theta, far_target) should_slow = abs(angle_far_unnorm) > 45.0 or abs(angle_unnorm) > 5.0 target_speed = 4 if should_slow else 7.0 brake = self._should_brake() target_speed = target_speed if not brake else 0.0 delta = np.clip(target_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) if brake: steer *= 0.5 throttle = 0.0 _draw.text((5, 90), 'Speed: %.3f' % speed) _draw.text((5, 110), 'Target: %.3f' % target_speed) _draw.text((5, 130), 'Angle: %.3f' % angle_unnorm) _draw.text((5, 150), 'Angle Far: %.3f' % angle_far_unnorm) return steer, throttle, brake, target_speed def run_step(self, input_data, timestamp): if not self.initialized: self._init() # self._world.set_weather(WEATHERS[int(os.environ['WEATHER_INDEX'])]) # if self.step % 100 == 0: # index = (self.step // 100) % len(WEATHERS) # self._world.set_weather(WEATHERS[index]) data = self.tick(input_data) rgb_with_car = cv2.cvtColor(input_data['rgb_with_car'][1][:, :, :3], cv2.COLOR_BGR2RGB) data['rgb_with_car'] = rgb_with_car topdown = data['topdown'] rgb = np.hstack((data['rgb_left'], data['rgb'], data['rgb_right'])) gps = self._get_position(data) near_node, near_command = self._waypoint_planner.run_step(gps) far_node, far_command = self._command_planner.run_step(gps) _topdown = Image.fromarray(COLOR[CONVERTER[topdown]]) _rgb = Image.fromarray(rgb) _draw = ImageDraw.Draw(_topdown) _topdown.thumbnail((256, 256)) _rgb = _rgb.resize((int(256 / _rgb.size[1] * _rgb.size[0]), 256)) _combined = Image.fromarray(np.hstack((_rgb, _topdown))) _draw = ImageDraw.Draw(_combined) steer, throttle, brake, target_speed = self._get_control( near_node, far_node, data, _draw) _draw.text((5, 10), 'FPS: %.3f' % (self.step / (time.time() - self.wall_start))) _draw.text((5, 30), 'Steer: %.3f' % steer) _draw.text((5, 50), 'Throttle: %.3f' % throttle) _draw.text((5, 70), 'Brake: %s' % brake) if HAS_DISPLAY: cv2.imshow('map', cv2.cvtColor(np.array(_combined), cv2.COLOR_BGR2RGB)) cv2.waitKey(1) control = carla.VehicleControl() control.steer = steer + 1e-2 * np.random.randn() control.throttle = throttle control.brake = float(brake) # we only gether info every 2 frames for faster processing speed if self.step % 2 == 0: self.gather_info() # if this number is very small, we may not have the exact numbers and images for the event happening (e.g. the frame when a collision happen). However, this is usually ok if we only use these for retraining purpose record_every_n_steps = 3 if self.step % record_every_n_steps == 0: self.save(record_every_n_steps, far_command, steer, throttle, brake, target_speed, data) self.save_json(record_every_n_steps, far_node, near_command, steer, throttle, brake, target_speed, data) return control def save_json(self, record_every_n_steps, far_node, near_command, steer, throttle, brake, target_speed, tick_data): frame = int(self.step // record_every_n_steps) pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # pos, , far_node, near_command data = { 'x': pos[0], 'y': pos[1], 'theta': theta, 'speed': speed, 'target_speed': target_speed, 'x_command': far_node[0], 'y_command': far_node[1], 'command': near_command.value, 'steer': steer, 'throttle': throttle, 'brake': brake, } pth = self.save_path / 'measurements' pth.mkdir(parents=False, exist_ok=True) (pth / ('%04d.json' % frame)).write_text(str(data)) def save(self, record_every_n_steps, far_command, steer, throttle, brake, target_speed, tick_data): frame = int(self.step // record_every_n_steps) speed = tick_data['speed'] string = os.environ['SAVE_FOLDER'] + '/' + pathlib.Path( os.environ['ROUTES']).stem center_str = string + '/' + 'rgb' + '/' + ('%04d.png' % frame) left_str = string + '/' + 'rgb_left' + '/' + ('%04d.png' % frame) right_str = string + '/' + 'rgb_right' + '/' + ('%04d.png' % frame) # topdown_str = string + '/' + 'topdown' + '/' + ('%04d.png' % frame) center = self.save_path / 'rgb' / ('%04d.png' % frame) left = self.save_path / 'rgb_left' / ('%04d.png' % frame) right = self.save_path / 'rgb_right' / ('%04d.png' % frame) topdown = self.save_path / 'topdown' / ('%04d.png' % frame) rgb_with_car = self.save_path / 'rgb_with_car' / ('%04d.png' % frame) data_row = ','.join([ str(i) for i in [ frame, far_command, speed, steer, throttle, brake, center_str, left_str, right_str ] ]) with (self.save_path / 'measurements.csv').open("a") as f_out: f_out.write(data_row + '\n') Image.fromarray(tick_data['rgb']).save(center) Image.fromarray(tick_data['rgb_left']).save(left) Image.fromarray(tick_data['rgb_right']).save(right) # modification # Image.fromarray(COLOR[CONVERTER[tick_data['topdown']]]).save(topdown) Image.fromarray(tick_data['topdown']).save(topdown) Image.fromarray(tick_data['rgb_with_car']).save(rgb_with_car) def _should_brake(self): actors = self._world.get_actors() vehicle = self._is_vehicle_hazard(actors.filter('*vehicle*')) light = self._is_light_red(actors.filter('*traffic_light*')) walker = self._is_walker_hazard(actors.filter('*walker*')) return any(x is not None for x in [vehicle, light, walker]) def _draw_line(self, p, v, z, color=(255, 0, 0)): if not DEBUG: return p1 = _location(p[0], p[1], z) p2 = _location(p[0] + v[0], p[1] + v[1], z) color = carla.Color(*color) self._world.debug.draw_line(p1, p2, 0.25, color, 0.01) def _is_light_red(self, lights_list): if self._vehicle.get_traffic_light_state( ) != carla.libcarla.TrafficLightState.Green: affecting = self._vehicle.get_traffic_light() for light in self._traffic_lights: if light.id == affecting.id: return affecting return None def _is_walker_hazard(self, walkers_list): z = self._vehicle.get_location().z p1 = _numpy(self._vehicle.get_location()) v1 = 10.0 * _orientation(self._vehicle.get_transform().rotation.yaw) self._draw_line(p1, v1, z + 2.5, (0, 0, 255)) for walker in walkers_list: v2_hat = _orientation(walker.get_transform().rotation.yaw) s2 = np.linalg.norm(_numpy(walker.get_velocity())) if s2 < 0.05: v2_hat *= s2 p2 = -3.0 * v2_hat + _numpy(walker.get_location()) v2 = 8.0 * v2_hat self._draw_line(p2, v2, z + 2.5) collides, collision_point = get_collision(p1, v1, p2, v2) if collides: return walker return None def _is_vehicle_hazard(self, vehicle_list): z = self._vehicle.get_location().z o1 = _orientation(self._vehicle.get_transform().rotation.yaw) p1 = _numpy(self._vehicle.get_location()) s1 = max(7.5, 2.0 * np.linalg.norm(_numpy(self._vehicle.get_velocity()))) v1_hat = o1 v1 = s1 * v1_hat self._draw_line(p1, v1, z + 2.5, (255, 0, 0)) for target_vehicle in vehicle_list: if target_vehicle.id == self._vehicle.id: continue o2 = _orientation(target_vehicle.get_transform().rotation.yaw) p2 = _numpy(target_vehicle.get_location()) s2 = max( 5.0, 2.0 * np.linalg.norm(_numpy(target_vehicle.get_velocity()))) v2_hat = o2 v2 = s2 * v2_hat p2_p1 = p2 - p1 distance = np.linalg.norm(p2_p1) p2_p1_hat = p2_p1 / (distance + 1e-4) self._draw_line(p2, v2, z + 2.5, (255, 0, 0)) angle_to_car = np.degrees(np.arccos(v1_hat.dot(p2_p1_hat))) angle_between_heading = np.degrees(np.arccos(o1.dot(o2))) if angle_between_heading > 60.0 and not (angle_to_car < 15 and distance < s1): continue elif angle_to_car > 30.0: continue elif distance > s1: continue return target_vehicle return None