def shift_spawn_point(carla_map, k, spawn_shifts, spawn_point): try: spawn_shift = spawn_shifts[k] spawn_shift < 0 except (TypeError, IndexError) as e: return spawn_point return carlautil.move_along_road(carla_map, spawn_point, spawn_shift)
def __perturb_spawn_point(self, spawn_point): spawn_shift = random.normalvariate(0.0, 5.0) perturb_spawn_point = carlautil.move_along_road( self.carla_map, spawn_point, spawn_shift) yaw = math.degrees(random.uniform(0, 2 * math.pi)) perturb_spawn_point.rotation = carla.Rotation(yaw=yaw) perturb_spawn_point.location += carla.Location(0, 0, 0.1) return perturb_spawn_point
def shift_spawn_point(carla_map, k, spawn_shifts, spawn_point): try: spawn_shift = spawn_shifts[k] except IndexError: return spawn_point try: spawn_shift = random.uniform(spawn_shift[0], spawn_shift[1]) except TypeError: pass try: spawn_shift < 0 except TypeError: return spawn_point return carlautil.move_along_road(carla_map, spawn_point, spawn_shift)
def run_scenes(self, n_scenes_per_map): logging.info(f"Getting {self.n_frames} frames, {n_scenes_per_map} scenes from map {self.carla_map.name}.") for idx in range(n_scenes_per_map): # set spawn point n_spawn_points = len(self.spawn_points) if idx % n_spawn_points == 0 and idx > 0: self.spawn_points = self.get_random_shuffled_spawn_points(self.carla_map) spawn_point = self.spawn_points[idx % n_spawn_points] if self.perturb_spawn_point: spawn_shift = random.normalvariate(0.0, 3.0) spawn_point = carlautil.move_along_road( self.carla_map, spawn_point, spawn_shift ) yaw = random.uniform(0, 2*math.pi) spawn_point.rotation = carla.Rotation(yaw=yaw) spawn_point.location += carla.Location(0, 0, 0.5) self.run_scene(idx, spawn_point) time.sleep(0.1) # wait to avoid crashing
def run(self): n_scenes_per_map = max(1, self.n_scenes // len(self.MAP_NAMES)) for map_name in self.MAP_NAMES: if self.carla_map.name != map_name: self.world, self.carla_map = self.load_worldmap(map_name) for idx in range(n_scenes_per_map): # set spawn point spawn_points = self.get_random_shuffled_spawn_points() n_spawn_points = len(spawn_points) if idx % n_spawn_points == 0 and idx > 0: spawn_points = self.get_random_shuffled_spawn_points() spawn_point = spawn_points[idx % n_spawn_points] if self.perturb_spawn_point: spawn_shift = random.normalvariate(0.0, 10.0) spawn_point = carlautil.move_along_road( self.carla_map, spawn_point, spawn_shift) yaw = random.uniform(0, 2 * math.pi) spawn_point.rotation = carla.Rotation(yaw=yaw) spawn_point.location += carla.Location(0, 0, 0.5) self.run_scene(spawn_point)
def scenario(scenario_params, carla_synchronous, request): client, world, carla_map, traffic_manager = carla_synchronous ego_vehicle = None try: # Set up the vehicle spawn_points = carla_map.get_spawn_points() spawn_point = spawn_points[scenario_params.ego_spawn_idx] if scenario_params.spawn_shift is not None: spawn_point = carlautil.move_along_road( carla_map, spawn_point, scenario_params.spawn_shift) spawn_point.location += carla.Location(0, 0, 0.1) blueprint = world.get_blueprint_library().find('vehicle.audi.a2') ego_vehicle = world.spawn_actor(blueprint, spawn_point) world.tick() # Set up the camera. shift = 15 shift = shift * ego_vehicle.get_transform().get_forward_vector() shift = shift + carla.Location(z=30) location = ego_vehicle.get_transform().location + shift world.get_spectator().set_transform( carla.Transform(location, carla.Rotation(pitch=-90))) ############### # Apply control # NOTE: 1 m/s == 3.6 km/h speeds = [] headings = [] control_steers = [] control_throttles = [] control_brakes = [] def add_stats(): # save the speed, heading and control values speed = carlautil.actor_to_speed(ego_vehicle) _, heading, _ = carlautil.to_rotation_ndarray(ego_vehicle) speeds.append(speed) headings.append(heading) control = ego_vehicle.get_control() control_steers.append(control.steer) control_throttles.append(control.throttle) control_brakes.append(control.brake) controller = VehiclePIDController(ego_vehicle) for idx in range(scenario_params.n_burn_steps): for ctrl in scenario_params.init_controls: if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]: ego_vehicle.apply_control(ctrl.control) break world.tick() add_stats() logging.info( f"starting at yaw {ego_vehicle.get_transform().rotation.yaw}") controller.set_plan(scenario_params.controls.target_speeds, scenario_params.controls.target_angles, scenario_params.controls.step_period) for idx in range(scenario_params.run_steps): control = controller.step() ego_vehicle.apply_control(control) world.tick() add_stats() ########## # Plotting fig, axes = plt.subplots(2, 2, figsize=(12, 12)) axes = axes.T.ravel() timesteps = np.arange(len(speeds)) * 0.05 plan_timesteps = timesteps[-len(controller.step_to_speed):] axes[0].plot(plan_timesteps, controller.step_to_speed, label="target") axes[0].plot(timesteps, speeds, label="measurement") axes[0].set_title("Speed response") axes[1].plot(timesteps, control_throttles, "b", label="throttle") axes[1].plot(timesteps, control_brakes, "r", label="brake") axes[1].set_title("Speed input") axes[2].plot(plan_timesteps, controller.step_to_angle, label="target") plan_headings = headings[-len(controller.step_to_angle):] _headings = util.npu.warp_radians_about_center( np.array(plan_headings), np.array(controller.step_to_angle)) axes[2].plot(plan_timesteps, _headings, label="measurement") axes[2].set_title("Heading response") axes[3].plot(timesteps, control_steers, "b", label="steer") axes[3].set_title("Steer input") for ax in axes: # loc = ticker.MultipleLocator(0.5) # ax.xaxis.set_major_locator(loc) ax.legend() ax.set_xlabel("time s") ax.grid(True) fig.tight_layout() fig.savefig(os.path.join("out", f"{request.node.callspec.id}.png")) fig.clf() finally: if ego_vehicle: ego_vehicle.destroy()
def scenario(scenario_params, carla_synchronous, request): client, world, carla_map, traffic_manager = carla_synchronous ego_vehicle = None try: # Set up the vehicle spawn_points = carla_map.get_spawn_points() spawn_point = spawn_points[scenario_params.ego_spawn_idx] if scenario_params.spawn_shift is not None: spawn_point = carlautil.move_along_road( carla_map, spawn_point, scenario_params.spawn_shift) spawn_point.location += carla.Location(0, 0, 0.1) blueprint = world.get_blueprint_library().find('vehicle.audi.a2') ego_vehicle = world.spawn_actor(blueprint, spawn_point) world.tick() # Set up the camera. shift = 45 shift = shift * ego_vehicle.get_transform().get_forward_vector() shift = shift + carla.Location(z=65) location = ego_vehicle.get_transform().location + shift world.get_spectator().set_transform( carla.Transform(location, carla.Rotation(pitch=-90))) # NOTE: ego_vehicle.get_speed_limit() documentation says m/s. This is incorrect. logging.info( f"Vehicle speed limit is {ego_vehicle.get_speed_limit()} km/h") ############### # Apply control # NOTE: 1 m/s == 3.6 km/h speeds = [] accelerations = [] vangulars = [] control_throttles = [] control_brakes = [] control_steerings = [] for idx in range(scenario_params.run_steps): for ctrl in scenario_params.controls: if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]: ego_vehicle.apply_control(ctrl.control) break world.tick() speed = carlautil.actor_to_speed(ego_vehicle) speeds.append(speed) accel = carlautil.actor_to_acceleration(ego_vehicle) accelerations.append(accel) vangular = ego_vehicle.get_angular_velocity().z vangulars.append(vangular) control_throttles.append(ctrl.control.throttle) control_brakes.append(ctrl.control.brake) control_steerings.append(ctrl.control.steer) ########## # Plotting fig, axes = plt.subplots(1, 3, figsize=(12, 12)) axes = axes.ravel() timesteps = np.arange(len(speeds)) * 0.05 axes[0].plot(timesteps, speeds, label="measurement") axes[0].set_title("Speed input/response") axes[0].set_ylabel("$m/s$") axes[1].plot(timesteps, accelerations, label="measurement") axes[1].set_title("Acceleration response") axes[1].set_ylabel("$m/s^2$") axes[2].plot(timesteps, vangulars, label="measurement") axes[2].plot(timesteps, control_steerings, label="steer input") axes[2].set_title("Angular velocity response") axes[2].set_ylabel("deg/$s$") for ax in axes: ax.grid() ax.legend() ax.set_xlabel("time s") for ax in axes[:2]: ax.plot(timesteps, control_throttles, "b", label="throttle input") ax.plot(timesteps, control_brakes, "r", label="brake input") fig.tight_layout() fig.savefig(os.path.join("out", f"{request.node.callspec.id}.png")) fig.clf() finally: if ego_vehicle: ego_vehicle.destroy()