Simulator/scenarios/braking.py

"""
scenarios/braking.py
--------------------
Lead Vehicle Hard Braking Scenario.

A vehicle is spawned directly ahead of the ego on the same lane
and driven by the Traffic Manager. At a configurable frame, the
lead vehicle performs an emergency stop, creating a forward-collision
test scenario for ADAS evaluation.
"""

import sys
import os

sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

import math
import carla
from scenarios.base import ScenarioBase


class BrakingScenario(ScenarioBase):
    """
    Scenario: Lead vehicle hard braking.

    Timeline
    --------
    Frame 1 … BRAKE_FRAME-1 : Lead vehicle cruises ahead at cruise_speed km/h.
    Frame BRAKE_FRAME+       : Lead vehicle applies emergency stop.
    """

    LEAD_DISTANCE_M = 25      # metres ahead of ego for lead vehicle
    BRAKE_FRAME     = 80      # frame on which the lead vehicle emergency-brakes
    CRUISE_SPEED    = 30      # km/h, set via TM percentage of speed limit

    def __init__(self):
        super().__init__()
        self._lead_vehicle = None
        self._braked = False

    # ------------------------------------------------------------------
    # ScenarioBase interface
    # ------------------------------------------------------------------

    @property
    def name(self) -> str:
        return "braking"

    @property
    def ego_spawn_point(self):
        # Using the known-working Town10 intersection point
        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:
        self._world = world
        self._ego   = ego_vehicle
        self._tm    = traffic_manager

        bp_lib = world.get_blueprint_library()

        # Pick a distinct vehicle blueprint for the lead car
        lead_bps = bp_lib.filter("vehicle.tesla.model3")
        if not lead_bps:
            lead_bps = bp_lib.filter("vehicle.*")
        lead_bp = lead_bps[0]

        # Place lead vehicle ahead on the same lane
        spawn_wp = self._get_waypoint_ahead(self.LEAD_DISTANCE_M, lane_offset=0)
        if spawn_wp is None:
            raise RuntimeError(
                "[BrakingScenario] Could not find waypoint ahead of ego. "
                "Ensure ego is on a driveable lane."
            )

        # Lift actor slightly to avoid ground clipping
        spawn_transform = spawn_wp.transform
        spawn_transform.location.z += 0.5

        self._lead_vehicle = world.try_spawn_actor(lead_bp, spawn_transform)
        if self._lead_vehicle is None:
            raise RuntimeError(
                "[BrakingScenario] Failed to spawn lead vehicle at waypoint "
                f"{spawn_transform.location}. Spot may be occupied."
            )

        self._actors.append(self._lead_vehicle)

        # TM: cruise just below the speed limit
        self._lead_vehicle.set_autopilot(True, traffic_manager.get_port())
        traffic_manager.vehicle_percentage_speed_difference(
            self._lead_vehicle, -10   # 10 % above speed limit → ~33–44 km/h
        )
        traffic_manager.auto_lane_change(self._lead_vehicle, False)
        traffic_manager.distance_to_leading_vehicle(self._lead_vehicle, 0)

        print(
            f"[{self.name}] Lead vehicle spawned {self.LEAD_DISTANCE_M} m ahead. "
            f"Emergency brake at frame {self.BRAKE_FRAME}."
        )

    def step(self, frame_count: int, ego_vehicle, pbar=None) -> None:
        if frame_count == self.BRAKE_FRAME and not self._braked:
            if self._lead_vehicle and self._lead_vehicle.is_alive:
                # Disengage TM autopilot and apply full brake
                self._lead_vehicle.set_autopilot(False)
                control = carla.VehicleControl()
                control.throttle = 0.0
                control.brake    = 1.0
                control.hand_brake = True
                self._lead_vehicle.apply_control(control)
                self._braked = True
                print(f"[{self.name}] EMERGENCY BRAKE applied at frame {frame_count}.")

        # Unified Verbose Logging
        l_dist = -1.0
        if self._lead_vehicle and self._lead_vehicle.is_alive:
            l_dist = ego_vehicle.get_location().distance(self._lead_vehicle.get_location())
            
        extra = f"| LEAD DIST: {l_dist:.1f}m {'(BRAKING)' if self._braked else ''}"
        self.log_frame(frame_count, ego_vehicle, extra_msg=extra, pbar=pbar)

    def cleanup(self) -> None:
        self._destroy_actors()
        print(f"[{self.name}] Cleanup complete.")

    # ------------------------------------------------------------------
    # Optional overrides
    # ------------------------------------------------------------------

    def get_scenario_metadata(self) -> dict:
        return {
            "scenario":       self.name,
            "lead_distance_m": self.LEAD_DISTANCE_M,
            "brake_frame":    self.BRAKE_FRAME,
            "braked":         self._braked,
        }