def show_square(client, x, y, z, r,
rotation=carla.Rotation(), t=1.0,
c=carla.Color(r=255, g=0, b=0, a=100)):
"""Create a square at a position with radius.
Parameters
----------
client : carla.Client or carla.World
"""
l = carla.Location(x, y, z)
carlautil.debug_point(client, l, t=t, c=c)
carlautil.debug_square(client, l, r, t=t, c=c)
def show_sloped_wps(client, dest=4.0,
pitch=6.0, t=1.0,
c=carla.Color(r=255, g=0, b=0, a=100)):
world = client.get_world()
carla_map = world.get_map()
wps = carla_map.generate_waypoints(dest)
ppitch = (360. - pitch)
def f(wp):
wp_pitch = wp.transform.rotation.pitch % 360.
return wp_pitch > pitch and wp_pitch < ppitch
sloped_wps = util.filter_to_list(f, wps)
for wp in sloped_wps:
loc = wp.transform.location
carlautil.debug_point(client, loc, t=t, c=c)
def at_slope_to_label(self, actor):
"""Check wheter actor (i.e. vehicle) is near a slope,
returning a ScenarioSlopeLabel.
TODO: make wp_locations array size smaller after debugging.
Don't need to check non-sloped waypoints.
"""
actor_location = carlautil.actor_to_location_ndarray(actor)
actor_xy = actor_location[:2]
actor_z = actor_location[-1]
"""Want to ignore waypoints above and below (i.e. in the case actor is on a bridge)."""
upperbound_z = actor.bounding_box.extent.z * 2
lowerbound_z = -1
xy_distances_to_wps = np.linalg.norm(self.wp_locations[:, :2] -
actor_xy,
axis=1)
z_displacement_to_wps = self.wp_locations[:, -1] - actor_z
"""get waypoints close to vehicle filter"""
wps_filter = np.logical_and(
xy_distances_to_wps < self.SLOPE_FIND_RADIUS,
np.logical_and(z_displacement_to_wps < upperbound_z,
z_displacement_to_wps > lowerbound_z))
#####
"""obtain extra slope information"""
wp_pitches = self.wp_pitches[wps_filter]
if wp_pitches.size == 0:
max_wp_pitch = 0.0
else:
wp_pitches = np.min(np.vstack((
wp_pitches,
np.abs(wp_pitches - 360.),
)),
axis=0)
max_wp_pitch = np.max(wp_pitches)
#####
if self._debug:
nearby_slopes = np.logical_and(self.wp_is_sloped == True,
wps_filter)
for wp_location in self.wp_locations[nearby_slopes]:
loc = carlautil.ndarray_to_location(wp_location)
carlautil.debug_point(self.carla_world, loc)
if np.any(self.wp_is_sloped[wps_filter]):
return ScenarioSlopeLabel.SLOPES, max_wp_pitch
else:
return ScenarioSlopeLabel.NONE, max_wp_pitch
def episode(self, episode_idx):
logging.info(f"doing episode {episode_idx}")
ego_vehicle = None
agent = None
other_vehicles = []
stats = util.AttrDict(success=False,
infeasibility=False,
steps=0,
plan_steps=0)
try:
spawn_indices = [self.scenario_params.ego_spawn_idx
] + self.scenario_params.other_spawn_ids
other_vehicle_ids = []
for k, spawn_idx in enumerate(spawn_indices):
if k == 0:
blueprint = self.blueprint_audi_a2
else:
blueprint = np.random.choice(self.blueprints)
spawn_point = self.spawn_points[spawn_idx]
spawn_point = shift_spawn_point(
self.carla_map, k, self.scenario_params.spawn_shifts,
spawn_point)
# Prevent collision with road.
spawn_point.location += carla.Location(0, 0, 0.5)
vehicle = self.world.spawn_actor(blueprint, spawn_point)
if k == 0:
ego_vehicle = vehicle
else:
vehicle.set_autopilot(True,
self.traffic_manager.get_port())
if self.scenario_params.ignore_signs:
self.traffic_manager.ignore_signs_percentage(
vehicle, 100.)
if self.scenario_params.ignore_lights:
self.traffic_manager.ignore_lights_percentage(
vehicle, 100.)
if self.scenario_params.ignore_vehicles:
self.traffic_manager.ignore_vehicles_percentage(
vehicle, 100.)
if not self.scenario_params.auto_lane_change:
self.traffic_manager.auto_lane_change(vehicle, False)
other_vehicles.append(vehicle)
other_vehicle_ids.append(vehicle.id)
frame = self.world.tick()
agent = self.motion_planner_cls(
ego_vehicle,
self.map_reader,
other_vehicle_ids,
self.eval_stg,
scene_builder_cls=self.scene_builder_cls,
scene_config=self.online_config,
**self.scenario_params,
**self.ctrl_params,
**self.DEBUG_SETTINGS,
)
agent.start_sensor()
assert agent.sensor_is_listening
if self.scenario_params.goal:
agent.set_goal(**self.scenario_params.goal)
"""Setup vehicle routes"""
if "CARLANAME" in os.environ and os.environ[
"CARLANAME"] == "carla-0.9.13":
for k, vehicle in enumerate(other_vehicles):
route = None
try:
route = self.scenario_params.other_routes[k]
len(route)
except (TypeError, IndexError) as e:
continue
self.traffic_manager.set_route(vehicle, route)
else:
logging.info("Skipping setting up OV routes.")
if episode_idx == 0:
"""Move the spectator to the ego vehicle.
The positioning is a little off"""
goal = agent.get_goal()
goal_x, goal_y = goal.x, -goal.y
state = agent.get_vehicle_state()
if goal.is_relative:
location = carla.Location(x=state[0] + goal_x / 2.,
y=state[1] - goal_y / 2.,
z=state[2] + 50)
else:
location = carla.Location(x=(state[0] + goal_x) / 2.,
y=(state[1] + goal_y) / 2.,
z=state[2] + 50)
# rotation = carla.Rotation(pitch=-70, yaw=-90, roll=20)
rotation = carla.Rotation(pitch=-90, yaw=0, roll=0)
# configure the spectator
self.world.get_spectator().set_transform(
carla.Transform(location, rotation))
location = carla.Location(goal_x, goal_y, state[2])
carlautil.debug_point(self.world,
location,
t=60.0,
label="goal")
n_burn_frames = self.scenario_params.n_burn_interval * self.online_config.record_interval
if self.ctrl_params.loop_type == LoopEnum.CLOSED_LOOP:
run_frames = self.scenario_params.run_interval * self.online_config.record_interval
else:
run_frames = self.ctrl_params.control_horizon * self.online_config.record_interval - 1
for idx in range(n_burn_frames):
control = None
for ctrl in self.scenario_params.controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
control = ctrl.control
break
agent.run_step(frame, control=control)
frame = self.world.tick()
for idx in range(run_frames):
agent.run_step(frame)
frame = self.world.tick()
stats.steps += 1
state = agent.get_vehicle_state(flip_y=True)
goal = agent.get_goal()
dist = math.sqrt((state[0] - goal.x)**2 +
(state[1] - goal.y)**2)
if dist < self.TOL:
stats.success = True
break
except InSimulationException as e:
stats.infeasibility = True
finally:
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
stats.plan_steps = stats.steps / self.online_config.record_interval
time.sleep(1)
logging.info(f"episode succeeded? {stats.success}. "
f"Infeasibility? {stats.infeasibility}. "
f"Ran planner over {stats.steps} total steps; "
f"{stats.plan_steps} planning steps.")
return pd.Series(stats)