simple-autonomous-car 0.1.2__py3-none-any.whl

simple_autonomous_car/__init__.py

@@ -0,0 +1,96 @@
+"""Simple Autonomous Car SDK.
+
+A comprehensive SDK for building autonomous vehicle systems with modular sensors,
+controllers, planners, and alert systems.
+"""
+
+__version__ = "0.3.0"
+
+# Core modules
+# Constants (available as simple_autonomous_car.constants)
+import simple_autonomous_car.constants
+from simple_autonomous_car.alerts import TrackBoundsAlert
+from simple_autonomous_car.car import Car, CarState
+from simple_autonomous_car.control import BaseController, PIDController, PurePursuitController
+from simple_autonomous_car.costmap import BaseCostmap, GridCostmap, inflate_obstacles
+from simple_autonomous_car.detection import LocalizationErrorDetector
+from simple_autonomous_car.filters import BaseFilter, KalmanFilter, ParticleFilter
+from simple_autonomous_car.frames import (
+    FrenetFrame,
+    ego_to_frenet,
+    ego_to_sensor,
+    frenet_to_ego,
+    frenet_to_global,
+    global_to_frenet,
+    sensor_to_ego,
+)
+from simple_autonomous_car.maps import FrenetMap, GridMap, GroundTruthMap, PerceivedMap
+from simple_autonomous_car.perception import PerceptionPoints
+from simple_autonomous_car.planning import BasePlanner, GoalPlanner, TrackPlanner
+from simple_autonomous_car.sensors import BaseSensor, LiDARSensor
+from simple_autonomous_car.track import Track
+from simple_autonomous_car.visualization import (
+    AlertVisualizer,
+    plot_car,
+    plot_control_history,
+    plot_perception,
+    plot_pure_pursuit_state,
+)
+
+__all__ = [
+    # Version
+    "__version__",
+    # Track
+    "Track",
+    # Car
+    "Car",
+    "CarState",
+    # Maps
+    "GroundTruthMap",
+    "PerceivedMap",
+    "FrenetMap",
+    "GridMap",
+    # Perception
+    "PerceptionPoints",
+    # Frames
+    "FrenetFrame",
+    "global_to_frenet",
+    "frenet_to_global",
+    "ego_to_frenet",
+    "frenet_to_ego",
+    "sensor_to_ego",
+    "ego_to_sensor",
+    # Sensors
+    "BaseSensor",
+    "LiDARSensor",
+    # Control
+    "BaseController",
+    "PurePursuitController",
+    "PIDController",
+    # Planning
+    "BasePlanner",
+    "TrackPlanner",
+    "GoalPlanner",
+    # Costmap
+    "BaseCostmap",
+    "GridCostmap",
+    "inflate_obstacles",
+    # Detection
+    "LocalizationErrorDetector",
+    # Alerts
+    "TrackBoundsAlert",
+    # Filters
+    "BaseFilter",
+    "KalmanFilter",
+    "ParticleFilter",
+    # Footprints
+    "BaseFootprint",
+    "RectangularFootprint",
+    "CircularFootprint",
+    # Visualization
+    "AlertVisualizer",
+    "plot_perception",
+    "plot_car",
+    "plot_pure_pursuit_state",
+    "plot_control_history",
+]

simple_autonomous_car/alerts/__init__.py

@@ -0,0 +1,5 @@
+"""Alert system for localization and perception errors."""
+
+from simple_autonomous_car.alerts.track_bounds_alert import TrackBoundsAlert
+
+__all__ = ["TrackBoundsAlert"]

simple_autonomous_car/alerts/track_bounds_alert.py

@@ -0,0 +1,276 @@
+"""Track bounds alert system - detects when perceived lines are too far from map lines."""
+
+import numpy as np
+
+from simple_autonomous_car.car.car import CarState
+from simple_autonomous_car.maps.frenet_map import FrenetMap
+from simple_autonomous_car.perception.perception import PerceptionPoints
+
+
+class TrackBoundsAlert:
+    """
+    Alert system for detecting when perceived track boundaries deviate significantly
+    from the ground truth map boundaries.
+
+    This is the primary alert system for track bounds violations. It converts
+    perception data to Frenet coordinates and compares against ground truth bounds
+    to detect when the car is approaching or leaving the track.
+
+    Parameters
+    ----------
+    frenet_map : FrenetMap
+        Frenet map with ground truth boundaries.
+    warning_threshold : float, default=1.0
+        Distance threshold for warnings in meters. Deviations above this trigger warnings.
+    critical_threshold : float, default=2.0
+        Distance threshold for critical alerts in meters. Deviations above this trigger critical alerts.
+        Must be greater than warning_threshold.
+    lookahead_distance : float, default=20.0
+        Distance ahead to check in meters. Only points within this distance are evaluated.
+
+    Attributes
+    ----------
+    frenet_map : FrenetMap
+        Frenet map with ground truth boundaries.
+    warning_threshold : float
+        Warning threshold in meters.
+    critical_threshold : float
+        Critical threshold in meters.
+    lookahead_distance : float
+        Lookahead distance in meters.
+    alert_history : List[Dict]
+        History of alert checks.
+
+    Examples
+    --------
+    >>> from simple_autonomous_car import Track, FrenetMap, TrackBoundsAlert
+    >>> track = Track.create_simple_track()
+    >>> frenet_map = FrenetMap(track)
+    >>> alert_system = TrackBoundsAlert(
+    ...     frenet_map,
+    ...     warning_threshold=1.0,
+    ...     critical_threshold=2.0,
+    ...     lookahead_distance=20.0
+    ... )
+    >>> result = alert_system.check(perception_points, car_state)
+    >>> if result["has_critical"]:
+    ...     print(f"CRITICAL: Max deviation = {result['max_deviation']:.2f}m")
+    """
+
+    def __init__(
+        self,
+        frenet_map: FrenetMap,
+        warning_threshold: float = 1.0,
+        critical_threshold: float = 2.0,
+        lookahead_distance: float = 20.0,
+    ):
+        """
+        Initialize track bounds alert system.
+
+        Parameters
+        ----------
+        frenet_map : FrenetMap
+            Frenet map with ground truth boundaries.
+        warning_threshold : float, default=1.0
+            Distance threshold for warnings (meters).
+        critical_threshold : float, default=2.0
+            Distance threshold for critical alerts (meters).
+            Must be >= warning_threshold.
+        lookahead_distance : float, default=20.0
+            Distance ahead to check (meters). Must be positive.
+        """
+        if warning_threshold < 0:
+            raise ValueError("warning_threshold must be non-negative")
+        if critical_threshold < warning_threshold:
+            raise ValueError("critical_threshold must be >= warning_threshold")
+        if lookahead_distance <= 0:
+            raise ValueError("lookahead_distance must be positive")
+
+        self.frenet_map = frenet_map
+        self.warning_threshold = warning_threshold
+        self.critical_threshold = critical_threshold
+        self.lookahead_distance = lookahead_distance
+        self.alert_history: list[dict] = []
+
+    def check(
+        self,
+        perception_points: PerceptionPoints,
+        car_state: CarState,
+    ) -> dict:
+        """
+        Check for track bounds violations.
+
+        This method converts perception points to Frenet coordinates, filters them
+        by lookahead distance, and compares against ground truth bounds to detect
+        deviations.
+
+        Parameters
+        ----------
+        perception_points : PerceptionPoints
+            Perceived track boundaries. Can be in any frame (will be converted to ego).
+        car_state : CarState
+            Current car state for coordinate transformations.
+
+        Returns
+        -------
+        Dict
+            Dictionary with alert information containing:
+            - has_warning : bool
+                True if any deviation exceeds warning_threshold.
+            - has_critical : bool
+                True if any deviation exceeds critical_threshold.
+            - max_deviation : float
+                Maximum deviation from bounds in meters.
+            - mean_deviation : float
+                Mean deviation from bounds in meters.
+            - deviations : np.ndarray
+                Array of deviations for each point (meters).
+            - alert_points : List[Dict]
+                List of points with alerts, each containing:
+                - s : float - Longitudinal position
+                - d : float - Lateral position
+                - deviation : float - Deviation from bounds
+
+        Examples
+        --------
+        >>> result = alert_system.check(perception_points, car_state)
+        >>> if result["has_critical"]:
+        ...     print(f"CRITICAL: {result['max_deviation']:.2f}m deviation")
+        >>> print(f"Found {len(result['alert_points'])} alert points")
+        """
+        # Convert perception points to Frenet frame
+        # First ensure points are in ego frame
+        if perception_points.frame != "ego":
+            perception_points = perception_points.to_ego_frame(car_state)
+
+        if len(perception_points.points) == 0:
+            return self._empty_result()
+
+        # Convert points to Frenet frame (vectorized where possible)
+        frenet_points = []
+        for point in perception_points.points:
+            try:
+                # Convert ego to global, then global to Frenet
+                point_global = car_state.transform_to_world_frame(point)
+                s, d = self.frenet_map.frenet_frame.global_to_frenet(point_global)
+                # Only include valid points (s >= 0)
+                if s >= 0:
+                    frenet_points.append([s, d])
+            except (ValueError, RuntimeError):
+                # Skip points that can't be converted (e.g., outside track)
+                continue
+
+        if len(frenet_points) == 0:
+            return self._empty_result()
+
+        frenet_points = np.asarray(frenet_points, dtype=np.float64)  # type: ignore[assignment]
+
+        # Get current position in Frenet frame
+        car_pos_global = car_state.position()
+        try:
+            s_car, d_car = self.frenet_map.frenet_frame.global_to_frenet(car_pos_global)
+        except (ValueError, RuntimeError):
+            # Car position can't be converted (shouldn't happen, but handle gracefully)
+            return self._empty_result()
+
+        # Check points within lookahead distance (ahead of car)
+        s_end = s_car + self.lookahead_distance
+        mask = (frenet_points[:, 0] >= s_car) & (frenet_points[:, 0] <= s_end)  # type: ignore[call-overload]
+        relevant_points = np.asarray(frenet_points[mask], dtype=np.float64)  # type: ignore[call-overload]
+
+        if len(relevant_points) == 0:
+            return self._empty_result()
+
+        # Get ground truth bounds for each point and calculate deviations
+        deviations = []
+        alert_points = []
+
+        for s, d_perceived in relevant_points:
+            try:
+                d_inner, d_outer = self.frenet_map.get_bounds_at_s(s)
+            except (ValueError, IndexError):
+                # Skip if bounds can't be retrieved for this s
+                continue
+
+            # Check if perceived point is within bounds
+            if d_perceived < d_inner:
+                # Too far left (inside track) - negative d means left of centerline
+                deviation = abs(d_perceived - d_inner)
+            elif d_perceived > d_outer:
+                # Too far right (outside track) - positive d means right of centerline
+                deviation = abs(d_perceived - d_outer)
+            else:
+                # Within bounds
+                deviation = 0.0
+
+            deviations.append(deviation)
+
+            # Store alert points that exceed warning threshold
+            if deviation > self.warning_threshold:
+                alert_points.append({"s": s, "d": d_perceived, "deviation": deviation})
+
+        deviations = np.asarray(deviations, dtype=np.float64)  # type: ignore[assignment]
+
+        max_deviation = np.max(deviations) if len(deviations) > 0 else 0.0
+        mean_deviation = np.mean(deviations) if len(deviations) > 0 else 0.0
+
+        has_warning = max_deviation > self.warning_threshold
+        has_critical = max_deviation > self.critical_threshold
+
+        result = {
+            "has_warning": has_warning,
+            "has_critical": has_critical,
+            "max_deviation": max_deviation,
+            "mean_deviation": mean_deviation,
+            "deviations": deviations,
+            "alert_points": alert_points,
+        }
+
+        # Store in history
+        self.alert_history.append(
+            {
+                "step": len(self.alert_history),
+                "car_state": car_state,
+                "result": result,
+            }
+        )
+
+        return result
+
+    def _empty_result(self) -> dict:
+        """
+        Return empty result dictionary.
+
+        Returns
+        -------
+        Dict
+            Empty result with no alerts.
+        """
+        return {
+            "has_warning": False,
+            "has_critical": False,
+            "max_deviation": 0.0,
+            "mean_deviation": 0.0,
+            "deviations": np.array([]),
+            "alert_points": [],
+        }
+
+    def get_recent_alerts(self, num_recent: int = 10) -> list[dict]:
+        """
+        Get recent alert history.
+
+        Parameters
+        ----------
+        num_recent : int, default=10
+            Number of recent alerts to return.
+
+        Returns
+        -------
+        List[Dict]
+            List of recent alert history entries.
+        """
+        return self.alert_history[-num_recent:] if num_recent > 0 else []
+
+    def reset_history(self) -> None:
+        """Clear alert history."""
+        self.alert_history = []

simple_autonomous_car/car/__init__.py

@@ -0,0 +1,5 @@
+"""Car model and dynamics."""
+
+from simple_autonomous_car.car.car import Car, CarState
+
+__all__ = ["Car", "CarState"]

simple_autonomous_car/car/car.py

@@ -0,0 +1,234 @@
+"""Car model with state and reference frame handling."""
+
+from dataclasses import dataclass
+from typing import TYPE_CHECKING, Optional
+
+import numpy as np
+
+if TYPE_CHECKING:
+    from simple_autonomous_car.perception.perception import PerceptionPoints
+    from simple_autonomous_car.sensors.base_sensor import BaseSensor
+
+
+@dataclass
+class CarState:
+    """State of the car."""
+
+    x: float  # x position in world frame
+    y: float  # y position in world frame
+    heading: float  # heading angle in radians (0 = east, counterclockwise)
+    velocity: float = 0.0  # forward velocity
+    steering_angle: float = 0.0  # steering angle in radians
+
+    def position(self) -> np.ndarray:
+        """Get position as numpy array."""
+        return np.array([self.x, self.y])
+
+    def rotation_matrix(self) -> np.ndarray:
+        """Get rotation matrix from world to car frame."""
+        cos_h = np.cos(self.heading)
+        sin_h = np.sin(self.heading)
+        return np.array([[cos_h, sin_h], [-sin_h, cos_h]])
+
+    def transform_to_car_frame(self, point: np.ndarray) -> np.ndarray:
+        """Transform point from world frame to car frame."""
+        rotation = self.rotation_matrix()
+        translated = point - self.position()
+        result = rotation @ translated
+        return np.asarray(result, dtype=np.float64)
+
+    def transform_to_world_frame(self, point: np.ndarray) -> np.ndarray:
+        """Transform point from car frame to world frame."""
+        rotation = self.rotation_matrix().T  # Inverse rotation
+        rotated = rotation @ point
+        result = rotated + self.position()
+        return np.asarray(result, dtype=np.float64)
+
+
+class Car:
+    """
+    Autonomous car model with sensor support.
+
+    This class represents an autonomous vehicle that can:
+    - Move using bicycle model dynamics
+    - Have multiple sensors attached
+    - Be controlled by controllers
+    - Follow plans from planners
+
+    Attributes
+    ----------
+    state : CarState
+        Current state of the car.
+    wheelbase : float
+        Distance between front and rear axles (meters).
+    max_velocity : float
+        Maximum velocity (m/s).
+    max_steering_angle : float
+        Maximum steering angle (radians).
+    sensors : List[BaseSensor]
+        List of sensors attached to the car.
+    """
+
+    def __init__(
+        self,
+        initial_state: CarState | None = None,
+        wheelbase: float = 2.5,
+        max_velocity: float = 30.0,
+        max_steering_angle: float = np.pi / 6,
+    ):
+        """
+        Initialize car.
+
+        Parameters
+        ----------
+        initial_state : CarState, optional
+            Initial state of the car. If None, creates car at origin.
+        wheelbase : float, default=2.5
+            Distance between front and rear axles in meters.
+        max_velocity : float, default=30.0
+            Maximum velocity in m/s.
+        max_steering_angle : float, default=π/6
+            Maximum steering angle in radians (30 degrees).
+        """
+        self.state = (
+            initial_state if initial_state is not None else CarState(x=0.0, y=0.0, heading=0.0)
+        )
+        self.wheelbase = wheelbase
+        self.max_velocity = max_velocity
+        self.max_steering_angle = max_steering_angle
+        self.sensors: list[BaseSensor] = []
+
+    def add_sensor(self, sensor: "BaseSensor") -> None:
+        """
+        Add a sensor to the car.
+
+        Parameters
+        ----------
+        sensor : BaseSensor
+            Sensor instance to add. Can be LiDAR, Camera, Radar, etc.
+        """
+        self.sensors.append(sensor)
+
+    def remove_sensor(self, sensor_name: str) -> bool:
+        """
+        Remove a sensor by name.
+
+        Parameters
+        ----------
+        sensor_name : str
+            Name of the sensor to remove.
+
+        Returns
+        -------
+        bool
+            True if sensor was found and removed, False otherwise.
+        """
+        for i, sensor in enumerate(self.sensors):
+            if sensor.name == sensor_name:
+                self.sensors.pop(i)
+                return True
+        return False
+
+    def get_sensor(self, sensor_name: str) -> Optional["BaseSensor"]:
+        """
+        Get a sensor by name.
+
+        Parameters
+        ----------
+        sensor_name : str
+            Name of the sensor.
+
+        Returns
+        -------
+        BaseSensor, optional
+            Sensor instance if found, None otherwise.
+        """
+        for sensor in self.sensors:
+            if sensor.name == sensor_name:
+                return sensor
+        return None
+
+    def sense_all(self, environment_data: dict | None = None) -> dict[str, "PerceptionPoints"]:
+        """
+        Get perception data from all enabled sensors.
+
+        Parameters
+        ----------
+        environment_data : dict, optional
+            Environment data to pass to sensors.
+
+        Returns
+        -------
+        Dict[str, PerceptionPoints]
+            Dictionary mapping sensor names to their perception data.
+        """
+        # Import here to avoid circular import
+
+        perception_data = {}
+        for sensor in self.sensors:
+            if sensor.is_enabled():
+                env_data = environment_data if environment_data is not None else {}
+                perception_data[sensor.name] = sensor.sense(self.state, env_data)
+        return perception_data
+
+    def update(self, dt: float, acceleration: float = 0.0, steering_rate: float = 0.0) -> None:
+        """
+        Update car state using bicycle model.
+
+        Args:
+            dt: Time step
+            acceleration: Acceleration (can be negative for braking)
+            steering_rate: Rate of change of steering angle
+        """
+        # Update steering angle
+        self.state.steering_angle += steering_rate * dt
+        self.state.steering_angle = np.clip(
+            self.state.steering_angle, -self.max_steering_angle, self.max_steering_angle
+        )
+
+        # Update velocity
+        self.state.velocity += acceleration * dt
+        self.state.velocity = np.clip(self.state.velocity, 0.0, self.max_velocity)
+
+        # Bicycle model dynamics
+        if abs(self.state.steering_angle) > 1e-6:
+            turning_radius = self.wheelbase / np.tan(self.state.steering_angle)
+            angular_velocity = self.state.velocity / turning_radius
+        else:
+            angular_velocity = 0.0
+
+        # Update position and heading
+        self.state.heading += angular_velocity * dt
+        self.state.x += self.state.velocity * np.cos(self.state.heading) * dt
+        self.state.y += self.state.velocity * np.sin(self.state.heading) * dt
+
+    def get_corners(self, length: float = 4.0, width: float = 1.8) -> np.ndarray:
+        """
+        Get car corner points in world frame.
+
+        Args:
+            length: Car length
+            width: Car width
+
+        Returns:
+            Array of shape (4, 2) with corner coordinates
+        """
+        half_length = length / 2.0
+        half_width = width / 2.0
+
+        # Corners in car frame (rear-left, rear-right, front-right, front-left)
+        corners_car = np.array(
+            [
+                [-half_length, -half_width],
+                [-half_length, half_width],
+                [half_length, half_width],
+                [half_length, -half_width],
+            ]
+        )
+
+        # Transform to world frame
+        corners_world = np.array(
+            [self.state.transform_to_world_frame(corner) for corner in corners_car]
+        )
+
+        return corners_world