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

simple_autonomous_car/constants.py

@@ -0,0 +1,112 @@
+"""Centralized constants for the Simple Autonomous Car SDK.
+
+This module contains all magic numbers and default values used throughout the codebase.
+Using constants makes the code more maintainable and allows easy tuning of parameters.
+"""
+
+# ============================================================================
+# Car Defaults
+# ============================================================================
+
+DEFAULT_WHEELBASE = 2.5  # meters
+DEFAULT_INITIAL_VELOCITY = 10.0  # m/s
+DEFAULT_MAX_VELOCITY = 25.0  # m/s
+DEFAULT_MAX_STEERING_ANGLE = 0.7853981633974483  # π/4 radians (45 degrees)
+
+# ============================================================================
+# Controller Defaults
+# ============================================================================
+
+DEFAULT_LOOKAHEAD_DISTANCE = 10.0  # meters (controller default)
+DEFAULT_LOOKAHEAD_GAIN = 2.0  # controller default (was 0.5, but controller uses 2.0)
+DEFAULT_MAX_STEERING_RATE = 1.0  # rad/s
+DEFAULT_TARGET_VELOCITY = 10.0  # m/s
+DEFAULT_VELOCITY_GAIN = 0.5
+DEFAULT_DT = 0.1  # seconds
+
+# Controller Visualization
+DEFAULT_ARC_LOOKAHEAD_DISTANCE = 20.0  # meters
+DEFAULT_MIN_TURNING_RADIUS = 0.0  # meters
+DEFAULT_MAX_TURNING_RADIUS = 1000.0  # meters
+DEFAULT_ARC_LINEWIDTH = 3.5
+DEFAULT_ARC_ALPHA = 0.95
+DEFAULT_LOOKAHEAD_LINEWIDTH = 2.5
+
+# Controller Thresholds
+MIN_LOOKAHEAD_DISTANCE = 0.1  # meters
+MIN_STEERING_ANGLE_THRESHOLD = 0.0  # radians
+SMALL_STEERING_ANGLE_THRESHOLD = 1e-3  # radians
+ADAPTIVE_GAIN_DENOMINATOR = 10.0  # used in adaptive gain calculation
+DEFAULT_GOAL_TOLERANCE = 2.0  # meters - distance to goal to consider reached
+
+# ============================================================================
+# Planner Defaults
+# ============================================================================
+
+DEFAULT_PLANNER_LOOKAHEAD_DISTANCE = 50.0  # meters
+DEFAULT_WAYPOINT_SPACING = 2.0  # meters
+DEFAULT_PLAN_LINEWIDTH = 2.5
+
+# Planner Thresholds
+MIN_SEGMENT_LENGTH = 1e-6  # meters
+
+# ============================================================================
+# Costmap Defaults
+# ============================================================================
+
+DEFAULT_COSTMAP_WIDTH = 50.0  # meters
+DEFAULT_COSTMAP_HEIGHT = 50.0  # meters
+DEFAULT_COSTMAP_RESOLUTION = 0.5  # meters per cell
+DEFAULT_INFLATION_RADIUS = 1.0  # meters
+
+# Costmap Values
+COST_FREE = 0.0
+COST_OCCUPIED = 1.0
+COST_THRESHOLD = 0.5  # threshold for occupied vs inflated
+
+# Costmap Visualization
+DEFAULT_COSTMAP_ALPHA = 0.6
+DEFAULT_COSTMAP_ZORDER = 2  # Behind plan/controller but above track
+
+# ============================================================================
+# Sensor Defaults
+# ============================================================================
+
+DEFAULT_LIDAR_MAX_RANGE = 40.0  # meters
+DEFAULT_LIDAR_ANGULAR_RESOLUTION = 0.1  # radians
+DEFAULT_POINT_NOISE_STD = 0.1  # meters
+
+# ============================================================================
+# Track Defaults
+# ============================================================================
+
+DEFAULT_TRACK_LENGTH = 100.0  # meters
+DEFAULT_TRACK_WIDTH = 50.0  # meters
+DEFAULT_TRACK_WIDTH_INNER = 5.0  # meters
+DEFAULT_TRACK_NUM_POINTS = 200
+
+# ============================================================================
+# Visualization Defaults
+# ============================================================================
+
+# Arrow visualization
+DEFAULT_ARROW_HEAD_WIDTH = 1.0
+DEFAULT_ARROW_HEAD_LENGTH = 1.0
+
+# Perception visualization
+DEFAULT_PERCEPTION_LINEWIDTH = 0.5
+
+# ============================================================================
+# Numerical Thresholds
+# ============================================================================
+
+EPSILON = 1e-6  # general small number threshold
+EPSILON_SMALL = 1e-3  # slightly larger threshold
+LARGE_NUMBER = 1e6  # used for "infinite" values
+
+# ============================================================================
+# Simulation Defaults
+# ============================================================================
+
+DEFAULT_SIMULATION_DT = 0.1  # seconds
+DEFAULT_PLOT_PAUSE = 0.01  # seconds

simple_autonomous_car/control/__init__.py

@@ -0,0 +1,7 @@
+"""Control system for autonomous vehicles."""
+
+from simple_autonomous_car.control.base_controller import BaseController
+from simple_autonomous_car.control.pid_controller import PIDController
+from simple_autonomous_car.control.pure_pursuit_controller import PurePursuitController
+
+__all__ = ["BaseController", "PurePursuitController", "PIDController"]

simple_autonomous_car/control/base_controller.py

@@ -0,0 +1,152 @@
+"""Base controller class for autonomous vehicle control."""
+
+from abc import ABC, abstractmethod
+from typing import TYPE_CHECKING, Any, Optional
+
+import numpy as np
+
+if TYPE_CHECKING:
+    from simple_autonomous_car.car.car import CarState
+    from simple_autonomous_car.costmap.base_costmap import BaseCostmap
+
+from simple_autonomous_car.perception.perception import PerceptionPoints
+
+
+class BaseController(ABC):
+    """
+    Base class for all controllers.
+
+    Controllers compute control commands (acceleration, steering_rate)
+    based on the current car state, perception data, and optionally a plan.
+
+    Attributes
+    ----------
+    name : str
+        Controller name/identifier.
+    enabled : bool
+        Whether the controller is enabled.
+    """
+
+    def __init__(self, name: str = "controller", enabled: bool = True):
+        """
+        Initialize base controller.
+
+        Parameters
+        ----------
+        name : str, default="controller"
+            Controller name/identifier.
+        enabled : bool, default=True
+            Whether the controller is enabled.
+        """
+        self.name = name
+        self.enabled = enabled
+
+    @abstractmethod
+    def compute_control(
+        self,
+        car_state: "CarState",
+        perception_data: dict[str, PerceptionPoints] | None = None,
+        costmap: Optional["BaseCostmap"] = None,
+        plan: np.ndarray | None = None,
+        goal: np.ndarray | None = None,
+        goal_tolerance: float | None = None,
+        dt: float = 0.1,
+    ) -> dict[str, float]:
+        """
+        Compute control commands.
+
+        Parameters
+        ----------
+        car_state : CarState
+            Current car state.
+        perception_data : dict, optional
+            Dictionary of perception data from sensors.
+            Keys are sensor names, values are PerceptionPoints.
+        costmap : BaseCostmap, optional
+            Costmap for obstacle avoidance.
+        plan : np.ndarray, optional
+            Planned path as array of shape (N, 2) with [x, y] waypoints.
+        goal : np.ndarray, optional
+            Goal position [x, y] for goal-based velocity adaptation.
+        dt : float, default=0.1
+            Time step in seconds.
+
+        Returns
+        -------
+        Dict[str, float]
+            Control commands with keys:
+            - "acceleration": Acceleration in m/s²
+            - "steering_rate": Steering rate in rad/s
+        """
+        pass
+
+    def is_enabled(self) -> bool:
+        """Check if controller is enabled."""
+        return self.enabled
+
+    def enable(self) -> None:
+        """Enable the controller."""
+        self.enabled = True
+
+    def disable(self) -> None:
+        """Disable the controller."""
+        self.enabled = False
+
+    def get_visualization_data(
+        self, car_state: "CarState", plan: np.ndarray | None = None, **kwargs: Any
+    ) -> dict[str, Any]:
+        """
+        Get visualization data for this controller.
+
+        This method should be overridden by subclasses to provide
+        controller-specific visualization data (e.g., lookahead points,
+        steering arcs, control commands).
+
+        Parameters
+        ----------
+        car_state : CarState
+            Current car state.
+        plan : np.ndarray, optional
+            Planned path.
+        **kwargs
+            Additional arguments.
+
+        Returns
+        -------
+        Dict
+            Dictionary containing visualization data. Default implementation
+            returns empty dict. Subclasses should override to provide specific data.
+        """
+        return {}
+
+    def visualize(
+        self,
+        ax: Any,
+        car_state: "CarState",
+        plan: np.ndarray | None = None,
+        frame: str = "global",
+        **kwargs: Any,
+    ) -> None:
+        """
+        Visualize controller state on the given axes.
+
+        This method should be overridden by subclasses to plot
+        controller-specific visualizations (e.g., lookahead points,
+        steering arcs, control commands).
+
+        Parameters
+        ----------
+        ax : matplotlib.axes.Axes
+            Axes to plot on.
+        car_state : CarState
+            Current car state.
+        plan : np.ndarray, optional
+            Planned path.
+        frame : str, default="global"
+            Frame to plot in: "global" or "ego".
+        **kwargs
+            Additional visualization arguments (colors, linewidths, etc.).
+        """
+        # Default implementation does nothing
+        # Subclasses should override to provide visualization
+        pass

simple_autonomous_car/control/controller_viz.py

@@ -0,0 +1,282 @@
+"""Controller-specific visualization utilities."""
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from simple_autonomous_car.car.car import CarState
+from simple_autonomous_car.control.pure_pursuit_controller import PurePursuitController
+
+
+def plot_pure_pursuit_state(
+    controller: PurePursuitController,
+    car_state: CarState,
+    plan: np.ndarray,
+    ax: plt.Axes | None = None,
+    show_curvature: bool = True,
+    show_steering_circle: bool = True,
+    show_lookahead: bool = True,
+) -> plt.Axes:
+    """
+    Plot Pure Pursuit controller state with detailed visualization.
+
+    Shows:
+    - Planned path
+    - Car position and heading
+    - Lookahead point and line
+    - Steering circle (turning radius)
+    - Path curvature
+    - Control commands
+
+    Parameters
+    ----------
+    controller : PurePursuitController
+        Pure Pursuit controller instance.
+    car_state : CarState
+        Current car state.
+    plan : np.ndarray
+        Planned path as array of shape (N, 2).
+    ax : plt.Axes, optional
+        Axes to plot on. If None, creates new figure.
+    show_curvature : bool, default=True
+        Whether to show path curvature.
+    show_steering_circle : bool, default=True
+        Whether to show steering circle (turning radius).
+    show_lookahead : bool, default=True
+        Whether to show lookahead point and line.
+
+    Returns
+    -------
+    plt.Axes
+        Axes object with controller state plotted.
+
+    Examples
+    --------
+    >>> from simple_autonomous_car.control.controller_viz import plot_pure_pursuit_state
+    >>> ax = plot_pure_pursuit_state(controller, car.state, plan)
+    >>> plt.show()
+    """
+    if ax is None:
+        fig, ax = plt.subplots(figsize=(12, 10))
+
+    car_pos = car_state.position()
+
+    # Compute control
+    control = controller.compute_control(car_state, plan=plan)
+
+    # Plot plan
+    if len(plan) > 0:
+        ax.plot(plan[:, 0], plan[:, 1], "g-o", linewidth=2, markersize=4, alpha=0.6, label="Plan")
+
+    # Plot car
+    ax.plot(car_pos[0], car_pos[1], "bo", markersize=12, label="Car")
+    # Car heading arrow
+    heading_length = 3.0
+    dx = heading_length * np.cos(car_state.heading)
+    dy = heading_length * np.sin(car_state.heading)
+    ax.arrow(
+        car_pos[0],
+        car_pos[1],
+        dx,
+        dy,
+        head_width=1.0,
+        head_length=0.8,
+        fc="blue",
+        ec="blue",
+        label="Heading",
+    )
+
+    # Find lookahead point (simplified version of controller logic)
+    if show_lookahead and len(plan) > 0:
+        distances_to_path = np.linalg.norm(plan - car_pos, axis=1)
+        closest_idx = np.argmin(distances_to_path)
+
+        # Calculate adaptive lookahead
+        adaptive_lookahead = (
+            controller.lookahead_distance + controller.lookahead_gain * car_state.velocity
+        )
+
+        # Find point at lookahead distance ahead
+        path_distances = np.zeros(len(plan))
+        for i in range(closest_idx, len(plan) - 1):
+            path_distances[i + 1] = path_distances[i] + np.linalg.norm(plan[i + 1] - plan[i])
+        for i in range(closest_idx - 1, -1, -1):
+            path_distances[i] = path_distances[i + 1] - np.linalg.norm(plan[i + 1] - plan[i])
+
+        target_distances = path_distances - path_distances[closest_idx]
+        ahead_mask = target_distances >= 0
+        ahead_indices = np.where(ahead_mask)[0]
+
+        if len(ahead_indices) > 0:
+            ahead_distances = target_distances[ahead_indices]
+            lookahead_idx = np.argmin(np.abs(ahead_distances - adaptive_lookahead))
+            target_idx = ahead_indices[lookahead_idx]
+            if target_idx < len(plan):
+                target_point = plan[target_idx]
+
+                # Plot lookahead point
+                ax.plot(
+                    target_point[0],
+                    target_point[1],
+                    "ro",
+                    markersize=10,
+                    label="Lookahead Point",
+                )
+
+                # Plot lookahead line
+                ax.plot(
+                    [car_pos[0], target_point[0]],
+                    [car_pos[1], target_point[1]],
+                    "r--",
+                    linewidth=2,
+                    alpha=0.7,
+                    label=f"Lookahead ({adaptive_lookahead:.1f}m)",
+                )
+
+    # Show steering circle (turning radius)
+    if show_steering_circle and abs(car_state.steering_angle) > 1e-6:
+        wheelbase = 2.5  # Standard wheelbase
+        if abs(car_state.steering_angle) > 1e-6:
+            turning_radius = wheelbase / np.tan(car_state.steering_angle)
+        else:
+            turning_radius = 1e6  # Straight line
+
+        # Calculate circle center (perpendicular to car heading)
+        perp_angle = car_state.heading + np.pi / 2
+        if turning_radius < 0:
+            perp_angle += np.pi
+            turning_radius = abs(turning_radius)
+
+        center_x = car_pos[0] + turning_radius * np.cos(perp_angle)
+        center_y = car_pos[1] + turning_radius * np.sin(perp_angle)
+
+        # Draw turning circle
+        circle = plt.Circle(
+            (center_x, center_y),
+            abs(turning_radius),
+            fill=False,
+            color="orange",
+            linestyle="--",
+            linewidth=2,
+            alpha=0.5,
+            label=f"Turning Radius ({abs(turning_radius):.1f}m)",
+        )
+        ax.add_patch(circle)
+
+    # Show curvature along path
+    if show_curvature and len(plan) > 2:
+        curvatures = []
+        for i in range(len(plan) - 2):
+            p1, p2, p3 = plan[i], plan[i + 1], plan[i + 2]
+            v1 = p2 - p1
+            v2 = p3 - p2
+            cross = v1[0] * v2[1] - v1[1] * v2[0]
+            norm1 = np.linalg.norm(v1)
+            norm2 = np.linalg.norm(v2)
+            if norm1 > 1e-6 and norm2 > 1e-6:
+                curvature = abs(cross) / (norm1 * norm2 * norm1)
+            else:
+                curvature = 0.0
+            curvatures.append(curvature)
+
+        if len(curvatures) > 0:
+            max_curvature = max(curvatures) if curvatures else 0.0
+            # Plot curvature as color-coded path segments
+            for i in range(len(plan) - 2):
+                if i < len(curvatures):
+                    color_intensity = min(curvatures[i] / max(max_curvature, 1e-6), 1.0)
+                    ax.plot(
+                        [plan[i][0], plan[i + 1][0]],
+                        [plan[i][1], plan[i + 1][1]],
+                        "-",
+                        color=plt.cm.RdYlGn(1.0 - color_intensity),  # type: ignore[attr-defined]
+                        linewidth=4,
+                        alpha=0.6,
+                    )
+
+    # Add text with control commands
+    info_text = (
+        f"Steering: {np.degrees(car_state.steering_angle):.1f}°\n"
+        f"Steering Rate: {control['steering_rate']:.2f} rad/s\n"
+        f"Acceleration: {control['acceleration']:.2f} m/s²\n"
+        f"Velocity: {car_state.velocity:.2f} m/s"
+    )
+    ax.text(
+        0.02,
+        0.98,
+        info_text,
+        transform=ax.transAxes,
+        fontsize=10,
+        verticalalignment="top",
+        bbox=dict(boxstyle="round", facecolor="wheat", alpha=0.8),
+    )
+
+    ax.set_aspect("equal")
+    ax.grid(True, alpha=0.3)
+    ax.set_xlabel("X (m)")
+    ax.set_ylabel("Y (m)")
+    ax.legend(loc="upper right")
+
+    return ax
+
+
+def plot_control_history(
+    control_history: list[dict],
+    ax: plt.Axes | None = None,
+    show_steering: bool = True,
+    show_acceleration: bool = True,
+    show_velocity: bool = True,
+) -> plt.Axes:
+    """
+    Plot control command history over time.
+
+    Parameters
+    ----------
+    control_history : List[Dict]
+        List of control dictionaries with keys: 'steering_rate', 'acceleration', 'velocity', 'time'.
+    ax : plt.Axes, optional
+        Axes to plot on. If None, creates new figure.
+    show_steering : bool, default=True
+        Whether to show steering rate.
+    show_acceleration : bool, default=True
+        Whether to show acceleration.
+    show_velocity : bool, default=True
+        Whether to show velocity.
+
+    Returns
+    -------
+    plt.Axes
+        Axes object with control history plotted.
+
+    Examples
+    --------
+    >>> history = [{'steering_rate': 0.1, 'acceleration': 0.5, 'velocity': 10.0, 'time': i*0.1}
+    ...            for i in range(100)]
+    >>> ax = plot_control_history(history)
+    >>> plt.show()
+    """
+    if ax is None:
+        fig, ax = plt.subplots(figsize=(12, 6))
+
+    if len(control_history) == 0:
+        return ax
+
+    times = [h.get("time", i) for i, h in enumerate(control_history)]
+
+    if show_steering:
+        steering_rates = [h.get("steering_rate", 0.0) for h in control_history]
+        ax.plot(times, steering_rates, "b-", linewidth=2, label="Steering Rate (rad/s)")
+
+    if show_acceleration:
+        accelerations = [h.get("acceleration", 0.0) for h in control_history]
+        ax.plot(times, accelerations, "r-", linewidth=2, label="Acceleration (m/s²)")
+
+    if show_velocity:
+        velocities = [h.get("velocity", 0.0) for h in control_history]
+        ax.plot(times, velocities, "g-", linewidth=2, label="Velocity (m/s)")
+
+    ax.set_xlabel("Time (s)")
+    ax.set_ylabel("Value")
+    ax.grid(True, alpha=0.3)
+    ax.legend()
+
+    return ax