You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
130 lines
5.1 KiB
130 lines
5.1 KiB
"""
|
|
scenarios/showcase.py
|
|
---------------------
|
|
Custom high-impact demo scene: Left Turn Across Path (LTAP).
|
|
Built step-by-step for full control and learning.
|
|
|
|
Phase 1: Ego Foundation
|
|
"""
|
|
|
|
import carla
|
|
import math
|
|
from scenarios.base import ScenarioBase
|
|
|
|
class ShowcaseScenario(ScenarioBase):
|
|
@property
|
|
def name(self):
|
|
return "showcase"
|
|
|
|
@property
|
|
def ego_spawn_point(self):
|
|
# Working Town10 intersection point (Southbound)
|
|
return carla.Transform(carla.Location(x=107.412, y=45.309, z=0.5), carla.Rotation(yaw=-87.7))
|
|
|
|
def setup(self, world, ego_vehicle, traffic_manager) -> None:
|
|
"""
|
|
Step 11: Deep Intersection Guidance for Ego.
|
|
"""
|
|
self._world = world
|
|
self._ego = ego_vehicle
|
|
self._tm = traffic_manager
|
|
|
|
# User Coordinates
|
|
self.P1 = carla.Transform(carla.Location(x=101.573, y=-8.183, z=0.5), carla.Rotation(yaw=98.4))
|
|
self.P2 = carla.Transform(carla.Location(x=107.412, y=45.309, z=0.5), carla.Rotation(yaw=-87.7))
|
|
self.P3 = carla.Location(x=82.312, y=24.801, z=0.5)
|
|
|
|
# NPC Waypoint Guidance
|
|
self.P1_MID = carla.Location(x=101.573, y=18.0, z=0.5)
|
|
self._npc_reached_mid = False
|
|
|
|
# NEW: EGO Mid-point to force a larger turn radius
|
|
# Aligned with P2, but 10m further south into intersection
|
|
self.EGO_MID = carla.Location(x=107.412, y=32.0, z=0.5)
|
|
self._ego_reached_mid = False
|
|
|
|
# 1. Setup Ego (Already spawned at ego_spawn_point by main.py)
|
|
self._ego.set_autopilot(False)
|
|
|
|
# 2. Spawn NPC
|
|
bp_lib = world.get_blueprint_library()
|
|
npc_bp = bp_lib.filter("vehicle.nissan.micra")[0]
|
|
self._npc = world.try_spawn_actor(npc_bp, self.P1)
|
|
|
|
if self._npc:
|
|
self._actors.append(self._npc)
|
|
self._npc.set_autopilot(False)
|
|
print(f"[SUCCESS] Actors ready. Multi-stage pathing ENABLED.")
|
|
|
|
# 3. Visual Debug: Dimmed Boxes (Commented out for final presentation)
|
|
# world.debug.draw_box(
|
|
# carla.BoundingBox(self.P3 + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)),
|
|
# carla.Rotation(), thickness=0.1, color=carla.Color(178, 0, 0), life_time=20.0
|
|
# )
|
|
# world.debug.draw_box(
|
|
# carla.BoundingBox(self.P1_MID + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)),
|
|
# carla.Rotation(), thickness=0.1, color=carla.Color(0, 178, 0), life_time=20.0
|
|
# )
|
|
# world.debug.draw_box(
|
|
# carla.BoundingBox(self.EGO_MID + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)),
|
|
# carla.Rotation(), thickness=0.1, color=carla.Color(0, 0, 178), life_time=20.0
|
|
# )
|
|
|
|
def _get_steering_to_target(self, actor, target_loc):
|
|
t = actor.get_transform()
|
|
v = target_loc - t.location
|
|
target_yaw = math.degrees(math.atan2(v.y, v.x))
|
|
delta_yaw = target_yaw - t.rotation.yaw
|
|
while delta_yaw > 180: delta_yaw -= 360
|
|
while delta_yaw < -180: delta_yaw += 360
|
|
return max(-1.0, min(1.0, delta_yaw / 60.0))
|
|
|
|
def step(self, frame: int, ego_vehicle, pbar=None) -> None:
|
|
"""
|
|
Step 11: Multi-stage steering logic.
|
|
"""
|
|
# --- Ego Control ---
|
|
e_loc = ego_vehicle.get_location()
|
|
if not self._ego_reached_mid:
|
|
e_target = self.EGO_MID
|
|
if e_loc.distance(self.EGO_MID) < 2.5:
|
|
self._ego_reached_mid = True
|
|
msg = "[DEBUG] Ego reached MID point. Swinging into turn."
|
|
if pbar: pbar.write(msg)
|
|
else: print(msg)
|
|
else:
|
|
e_target = self.P3
|
|
|
|
e_dist = e_loc.distance(e_target)
|
|
ego_steer = self._get_steering_to_target(ego_vehicle, e_target) if e_dist > 1.0 else 0.0
|
|
ego_vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=ego_steer))
|
|
|
|
# --- NPC Control ---
|
|
n_steer, n_dist = 0.0, 0.0
|
|
if hasattr(self, "_npc") and self._npc and self._npc.is_alive:
|
|
n_loc = self._npc.get_location()
|
|
|
|
if not self._npc_reached_mid:
|
|
n_target = self.P1_MID
|
|
if n_loc.distance(self.P1_MID) < 2.5:
|
|
self._npc_reached_mid = True
|
|
msg = "[DEBUG] NPC reached MID point. Switching to P3."
|
|
if pbar: pbar.write(msg)
|
|
else: print(msg)
|
|
else:
|
|
n_target = self.P3
|
|
|
|
n_dist = n_loc.distance(n_target)
|
|
n_steer = self._get_steering_to_target(self._npc, n_target) if n_dist > 1.0 else 0.0
|
|
|
|
# Physics Injection (35 km/h)
|
|
fwd = self._npc.get_transform().get_forward_vector()
|
|
self._npc.set_target_velocity(fwd * 9.72)
|
|
self._npc.apply_control(carla.VehicleControl(throttle=1.0, steer=n_steer))
|
|
|
|
# Unified Verbose Logging
|
|
extra = f"target={'MID' if not self._ego_reached_mid else 'P3'} | NPC target={'MID' if not self._npc_reached_mid else 'P3'}"
|
|
self.log_frame(frame, ego_vehicle, extra_msg=extra, pbar=pbar)
|
|
|
|
def cleanup(self):
|
|
self._destroy_actors()
|