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()