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

simple_autonomous_car/visualization/alert_viz.py

@@ -0,0 +1,316 @@
+"""Visualization utilities for alert systems."""
+
+from typing import Any
+
+import matplotlib.pyplot as plt
+import numpy as np
+from matplotlib.patches import Polygon
+
+from simple_autonomous_car.maps.frenet_map import FrenetMap
+from simple_autonomous_car.perception.perception import PerceptionPoints
+from simple_autonomous_car.track.track import Track
+
+
+class AlertVisualizer:
+    """Visualization utilities for alert systems."""
+
+    def __init__(
+        self,
+        track: Track,
+        frenet_map: FrenetMap | None = None,
+        figsize: tuple = (16, 8),
+    ):
+        """
+        Initialize alert visualizer.
+
+        Args:
+            track: Track object
+            frenet_map: Optional Frenet map (will be created if not provided)
+            figsize: Figure size
+        """
+        self.track = track
+        self.frenet_map = frenet_map if frenet_map is not None else FrenetMap(track)
+        self.figsize = figsize
+        self.fig: Any = None
+        self.axes: Any = None
+
+    def create_figure(self) -> None:
+        """Create figure with world and car frame views."""
+        fig, (ax_world, ax_car) = plt.subplots(1, 2, figsize=self.figsize)
+        self.fig = fig
+        self.ax_world = ax_world
+        self.ax_car = ax_car
+        self._setup_axes()
+
+    def _setup_axes(self) -> None:
+        """Set up axis labels and properties."""
+        # World frame
+        self.ax_world.set_title("World Frame - Track Bounds Alert", fontsize=14, fontweight="bold")
+        self.ax_world.set_xlabel("X (m)")
+        self.ax_world.set_ylabel("Y (m)")
+        self.ax_world.set_aspect("equal")
+        self.ax_world.grid(True, alpha=0.3)
+
+        # Car frame
+        self.ax_car.set_title("Car Frame - Perception Points", fontsize=14, fontweight="bold")
+        self.ax_car.set_xlabel("Forward (m)")
+        self.ax_car.set_ylabel("Left (m)")
+        self.ax_car.set_aspect("equal")
+        self.ax_car.grid(True, alpha=0.3)
+
+    def plot_track(self, ax: Any = None) -> None:
+        """
+        Plot full track boundaries.
+
+        Args:
+            ax: Matplotlib axis (uses self.ax_world if None)
+        """
+        if ax is None:
+            ax = self.ax_world
+
+        ax.plot(
+            self.track.inner_bound[:, 0],
+            self.track.inner_bound[:, 1],
+            "k-",
+            linewidth=2.5,
+            label="Map (Inner)",
+            alpha=0.8,
+        )
+        ax.plot(
+            self.track.outer_bound[:, 0],
+            self.track.outer_bound[:, 1],
+            "k-",
+            linewidth=2.5,
+            label="Map (Outer)",
+            alpha=0.8,
+        )
+
+    def plot_perception(
+        self,
+        perception_points: PerceptionPoints,
+        car_state: Any,
+        car: Any,
+        highlight_alerts: bool = False,
+        alert_points: list[Any] | None = None,
+    ) -> None:
+        """
+        Plot perception points in both world and car frames.
+
+        Args:
+            perception_points: Perception points
+            car: Car object
+            highlight_alerts: Whether to highlight alert points
+            alert_points: List of alert points (if highlight_alerts is True)
+        """
+        # Ensure points are in ego frame
+        if perception_points.frame != "ego":
+            perception_points = perception_points.to_ego_frame(car.state)
+
+        # Car frame view
+        if len(perception_points) > 0:
+            self.ax_car.scatter(
+                perception_points.points[:, 0],
+                perception_points.points[:, 1],
+                c="red",
+                s=15,
+                alpha=0.6,
+                label="Perception",
+                marker=".",
+            )
+
+        # World frame view
+        if len(perception_points) > 0:
+            perception_global = perception_points.to_global_frame(car.state)
+            self.ax_world.scatter(
+                perception_global.points[:, 0],
+                perception_global.points[:, 1],
+                c="red",
+                s=15,
+                alpha=0.6,
+                label="Perception",
+                marker=".",
+            )
+
+            # Highlight alert points if requested
+            if highlight_alerts and alert_points:
+                alert_points_global = []
+                for alert_point in alert_points:
+                    s, d = alert_point["s"], alert_point["d"]
+                    point_global = self.frenet_map.frenet_frame.frenet_to_global(s, d)
+                    alert_points_global.append(point_global)
+
+                if alert_points_global:
+                    alert_points_global_arr = np.asarray(alert_points_global, dtype=np.float64)  # type: ignore[assignment]
+                    self.ax_world.scatter(
+                        alert_points_global_arr[:, 0],
+                        alert_points_global_arr[:, 1],
+                        c="orange",
+                        s=150,
+                        marker="x",
+                        linewidths=3,
+                        label="Alert Points",
+                        zorder=10,
+                    )
+
+    def plot_car(self, car: Any, ax: Any = None) -> None:
+        """
+        Plot car position.
+
+        Args:
+            car: Car object
+            ax: Matplotlib axis (uses self.ax_world if None)
+        """
+        if ax is None:
+            ax = self.ax_world
+
+        car_corners = car.get_corners()
+        car_poly = Polygon(car_corners, closed=True, color="blue", alpha=0.7)
+        ax.add_patch(car_poly)
+
+        # Also plot in car frame (at origin)
+        car_corners_car = np.array([[-2, -0.9], [-2, 0.9], [2, 0.9], [2, -0.9]])
+        car_poly_car = Polygon(car_corners_car, closed=True, color="blue", alpha=0.7)
+        self.ax_car.add_patch(car_poly_car)
+
+    def plot_alert_result(
+        self,
+        alert_result: dict[str, Any],
+        perception_points: PerceptionPoints,
+        car_state: Any,
+        car: Any,
+    ) -> None:
+        """
+        Plot complete alert visualization.
+
+        Args:
+            alert_result: Result from alert system check
+            perception_points: Perception points
+            car: Car object
+        """
+        self.ax_world.clear()
+        self.ax_car.clear()
+        self._setup_axes()
+
+        # Plot track
+        self.plot_track()
+
+        # Plot perception with alerts
+        self.plot_perception(
+            perception_points,
+            car_state,
+            car,
+            highlight_alerts=True,
+            alert_points=alert_result.get("alert_points", []),
+        )
+
+        # Plot car
+        self.plot_car(car)
+
+        # Add alert info as text
+        if alert_result.get("has_critical"):
+            status_text = f"CRITICAL: Max deviation = {alert_result['max_deviation']:.2f}m"
+            color = "red"
+        elif alert_result.get("has_warning"):
+            status_text = f"WARNING: Max deviation = {alert_result['max_deviation']:.2f}m"
+            color = "orange"
+        else:
+            status_text = f"OK: Max deviation = {alert_result['max_deviation']:.2f}m"
+            color = "green"
+
+        self.ax_world.text(
+            0.02,
+            0.98,
+            status_text,
+            transform=self.ax_world.transAxes,
+            fontsize=12,
+            verticalalignment="top",
+            bbox=dict(boxstyle="round", facecolor="white", alpha=0.8, edgecolor=color, linewidth=2),
+        )
+
+        # Set limits
+        all_points = np.vstack([self.track.inner_bound, self.track.outer_bound])
+        padding = 10.0
+        self.ax_world.set_xlim(
+            np.min(all_points[:, 0]) - padding, np.max(all_points[:, 0]) + padding
+        )
+        self.ax_world.set_ylim(
+            np.min(all_points[:, 1]) - padding, np.max(all_points[:, 1]) + padding
+        )
+
+        self.ax_car.set_xlim(-50, 50)
+        self.ax_car.set_ylim(-50, 50)
+
+        self.ax_world.legend(loc="upper right", fontsize=9)
+        self.ax_car.legend(loc="upper right", fontsize=9)
+
+        plt.tight_layout()
+
+    def plot_alert_history(self, alert_history: list) -> None:
+        """
+        Plot alert history over time.
+
+        Args:
+            alert_history: List of alert history entries
+        """
+        if not alert_history:
+            return
+
+        fig, ax = plt.subplots(figsize=(12, 6))
+
+        steps = [h["step"] for h in alert_history]
+        deviations = [h["max_deviation"] for h in alert_history]
+        warnings = [h.get("has_warning", False) for h in alert_history]
+        criticals = [h.get("has_critical", False) for h in alert_history]
+
+        ax.plot(steps, deviations, "b-", label="Max Deviation", linewidth=2)
+
+        # Add threshold lines if available
+        if hasattr(self, "warning_threshold"):
+            ax.axhline(
+                y=self.warning_threshold,
+                color="orange",
+                linestyle="--",
+                label="Warning Threshold",
+            )
+        if hasattr(self, "critical_threshold"):
+            ax.axhline(
+                y=self.critical_threshold,
+                color="red",
+                linestyle="--",
+                label="Critical Threshold",
+            )
+
+        # Fill warning and critical zones
+        if any(warnings):
+            ax.fill_between(
+                steps,
+                0,
+                deviations,
+                where=np.array(warnings),  # type: ignore[arg-type]
+                alpha=0.3,
+                color="orange",
+                label="Warning Zone",
+            )
+        if any(criticals):
+            ax.fill_between(
+                steps,
+                0,
+                deviations,
+                where=np.array(criticals),  # type: ignore[arg-type]
+                alpha=0.3,
+                color="red",
+                label="Critical Zone",
+            )
+
+        ax.set_xlabel("Simulation Step")
+        ax.set_ylabel("Max Deviation (m)")
+        ax.set_title("Alert History - Deviation Over Time")
+        ax.legend()
+        ax.grid(True, alpha=0.3)
+        plt.tight_layout()
+        plt.show()
+
+    def show(self) -> None:
+        """Display the figure."""
+        if self.fig is not None:
+            plt.show()

simple_autonomous_car/visualization/utils.py

@@ -0,0 +1,169 @@
+"""Utility visualization functions for non-component data (perception, car)."""
+
+from typing import Any
+
+import matplotlib.pyplot as plt
+import numpy as np
+from matplotlib.patches import Polygon
+
+from simple_autonomous_car.car.car import Car, CarState
+from simple_autonomous_car.perception.perception import PerceptionPoints
+
+
+def plot_perception(
+    perception_points: PerceptionPoints,
+    car_state: CarState,
+    ax: plt.Axes | None = None,
+    frame: str = "global",
+    color: str = "red",
+    label: str = "Perception",
+    **kwargs: Any,
+) -> plt.Axes:
+    """
+    Plot perception points from sensors.
+
+    Parameters
+    ----------
+    perception_points : PerceptionPoints
+        Perception data points.
+    car_state : CarState
+        Car state for coordinate transformation.
+    ax : plt.Axes, optional
+        Axes to plot on. If None, creates new figure.
+    frame : str, default="global"
+        Frame to plot in: "global" or "ego".
+    color : str, default="red"
+        Color for perception points.
+    label : str, default="Perception"
+        Label for legend.
+    **kwargs
+        Additional arguments passed to scatter.
+
+    Returns
+    -------
+    plt.Axes
+        Axes object with perception plotted.
+    """
+    if ax is None:
+        fig, ax = plt.subplots(figsize=(10, 8))
+
+    if len(perception_points.points) == 0:
+        return ax
+
+    # Convert to desired frame
+    if frame == "global":
+        if perception_points.frame != "global":
+            points = perception_points.to_global_frame(car_state).points
+        else:
+            points = perception_points.points
+    else:  # ego frame
+        if perception_points.frame != "ego":
+            points = perception_points.to_ego_frame(car_state).points
+        else:
+            points = perception_points.points
+
+    # Extract alpha from kwargs if provided, otherwise use default
+    scatter_alpha = kwargs.pop("alpha", 0.6)
+
+    ax.scatter(
+        points[:, 0],
+        points[:, 1],
+        c=color,
+        s=25,
+        alpha=scatter_alpha,
+        label=label,
+        marker=".",
+        edgecolors="darkred" if color in ["crimson", "red"] else None,
+        linewidths=0.5 if color in ["crimson", "red"] else 0,
+        **kwargs,
+    )
+
+    ax.set_aspect("equal")
+    ax.grid(True, alpha=0.3)
+
+    if frame == "global":
+        ax.set_xlabel("X (m)")
+        ax.set_ylabel("Y (m)")
+    else:
+        ax.set_xlabel("Forward (m)")
+        ax.set_ylabel("Left (m)")
+
+    return ax
+
+
+def plot_car(
+    car: Car,
+    ax: plt.Axes | None = None,
+    frame: str = "global",
+    color: str = "blue",
+    show_heading: bool = True,
+    **kwargs: Any,
+) -> plt.Axes:
+    """
+    Plot car position and orientation.
+
+    Parameters
+    ----------
+    car : Car
+        Car to plot.
+    ax : plt.Axes, optional
+        Axes to plot on. If None, creates new figure.
+    frame : str, default="global"
+        Frame to plot in: "global" or "ego".
+    color : str, default="blue"
+        Color for car.
+    show_heading : bool, default=True
+        Whether to show heading arrow.
+    **kwargs
+        Additional arguments passed to Polygon.
+
+    Returns
+    -------
+    plt.Axes
+        Axes object with car plotted.
+    """
+    if ax is None:
+        fig, ax = plt.subplots(figsize=(10, 8))
+
+    if frame == "global":
+        car_corners = car.get_corners()
+        car_pos = car.state.position()
+    else:  # ego frame
+        car_corners = np.array([[-2, -0.9], [-2, 0.9], [2, 0.9], [2, -0.9]])
+        car_pos = np.array([0.0, 0.0])
+
+    car_poly = Polygon(car_corners, closed=True, color=color, alpha=0.7, label="Car", **kwargs)
+    ax.add_patch(car_poly)
+
+    if show_heading and frame == "global":
+        # Draw heading arrow
+        arrow_length = 3.0
+        dx = arrow_length * np.cos(car.state.heading)
+        dy = arrow_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=color,
+            ec=color,
+        )
+    elif show_heading and frame == "ego":
+        # Draw forward arrow in ego frame
+        ax.arrow(
+            0,
+            0,
+            3.0,
+            0,
+            head_width=1.2,
+            head_length=1.0,
+            fc=color,
+            ec=color,
+        )
+
+    ax.set_aspect("equal")
+    ax.grid(True, alpha=0.3)
+
+    return ax