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

simple_autonomous_car/planning/track_planner.py

@@ -0,0 +1,199 @@
+"""Track-based path planner."""
+
+from typing import TYPE_CHECKING, Any, Optional
+
+import numpy as np
+
+from simple_autonomous_car.car.car import CarState
+from simple_autonomous_car.constants import (
+    DEFAULT_PLAN_LINEWIDTH,
+    DEFAULT_PLANNER_LOOKAHEAD_DISTANCE,
+    DEFAULT_WAYPOINT_SPACING,
+    MIN_SEGMENT_LENGTH,
+)
+from simple_autonomous_car.planning.base_planner import BasePlanner
+from simple_autonomous_car.track.track import Track
+
+if TYPE_CHECKING:
+    from simple_autonomous_car.costmap.base_costmap import BaseCostmap
+
+
+class TrackPlanner(BasePlanner):
+    """
+    Simple planner that follows the track centerline.
+
+    This planner generates a path along the track centerline ahead of the car.
+    It's useful for basic track following scenarios.
+
+    Parameters
+    ----------
+    track : Track
+        Track to plan on.
+    lookahead_distance : float, default=50.0
+        Distance ahead to plan (meters).
+    waypoint_spacing : float, default=2.0
+        Spacing between waypoints in meters.
+    name : str, default="track_planner"
+        Planner name.
+    """
+
+    def __init__(
+        self,
+        track: Track,
+        lookahead_distance: float = DEFAULT_PLANNER_LOOKAHEAD_DISTANCE,
+        waypoint_spacing: float = DEFAULT_WAYPOINT_SPACING,
+        name: str = "track_planner",
+    ):
+        super().__init__(name=name)
+        self.track = track
+        self.lookahead_distance = lookahead_distance
+        self.waypoint_spacing = waypoint_spacing
+
+        # Precompute cumulative distances
+        self._compute_cumulative_distances()
+
+    def _compute_cumulative_distances(self) -> None:
+        """Precompute cumulative distances along track."""
+        self.cumulative_distances = np.zeros(len(self.track.centerline))
+        for i in range(1, len(self.track.centerline)):
+            dist = np.linalg.norm(self.track.centerline[i] - self.track.centerline[i - 1])
+            self.cumulative_distances[i] = self.cumulative_distances[i - 1] + dist
+        self.total_length = self.cumulative_distances[-1]
+
+    def plan(
+        self,
+        car_state: CarState,
+        perception_data: dict | None = None,
+        costmap: Optional["BaseCostmap"] = None,
+        goal: np.ndarray | None = None,
+    ) -> np.ndarray:
+        """
+        Generate plan along track centerline.
+
+        Parameters
+        ----------
+        car_state : CarState
+            Current car state.
+        perception_data : dict, optional
+            Not used by track planner.
+        costmap : BaseCostmap, optional
+            Costmap for obstacle avoidance (not used by basic track planner).
+        goal : np.ndarray, optional
+            Not used by track planner.
+
+        Returns
+        -------
+        np.ndarray
+            Planned path as array of shape (N, 2) with [x, y] waypoints.
+        """
+        if not self.enabled:
+            return np.array([]).reshape(0, 2)
+
+        # Find closest point on track
+        car_pos = car_state.position()
+        distances = np.linalg.norm(self.track.centerline - car_pos, axis=1)
+        closest_idx = np.argmin(distances)
+        s_start = self.cumulative_distances[closest_idx]
+
+        # Generate waypoints ahead
+        num_waypoints = int(self.lookahead_distance / self.waypoint_spacing) + 1
+
+        waypoints = []
+        for i in range(num_waypoints):
+            s = s_start + (i / (num_waypoints - 1)) * self.lookahead_distance
+            s = s % self.total_length  # Wrap around
+
+            # Find point at distance s
+            idx = np.searchsorted(self.cumulative_distances, s)
+            if idx == 0:
+                idx = 1
+            if idx >= len(self.track.centerline):
+                idx = len(self.track.centerline) - 1
+
+            # Interpolate
+            segment_dist = s - self.cumulative_distances[idx - 1]
+            segment_length = self.cumulative_distances[idx] - self.cumulative_distances[idx - 1]
+            if segment_length > MIN_SEGMENT_LENGTH:
+                t = segment_dist / segment_length
+            else:
+                t = 0.0
+
+            point = self.track.centerline[idx - 1] + t * (
+                self.track.centerline[idx] - self.track.centerline[idx - 1]
+            )
+            waypoints.append(point)
+
+        return np.array(waypoints)
+
+    def visualize(
+        self,
+        ax: Any,
+        car_state: CarState,
+        plan: np.ndarray | None = None,
+        frame: str = "global",
+        **kwargs: Any,
+    ) -> None:
+        """
+        Visualize planned path on the given axes.
+
+        Parameters
+        ----------
+        ax : matplotlib.axes.Axes
+            Axes to plot on.
+        car_state : CarState
+            Current car state.
+        plan : np.ndarray, optional
+            Planned path (if already computed, otherwise will be computed).
+        frame : str, default="global"
+            Frame to plot in: "global" or "ego".
+        **kwargs
+            Additional visualization arguments:
+            - color: str, color for plan line
+            - linewidth: float, linewidth for plan
+            - linestyle: str, linestyle for plan
+            - show_waypoints: bool, whether to show waypoint markers
+        """
+        # Compute plan if not provided
+        if plan is None:
+            plan = self.plan(car_state)
+
+        if len(plan) == 0:
+            return
+
+        # Transform plan to desired frame
+        if frame == "ego":
+            plan_plot = np.array([car_state.transform_to_car_frame(point) for point in plan])
+        else:
+            plan_plot = plan
+
+        # Extract visualization parameters
+        color = kwargs.pop("color", "green")
+        linewidth = kwargs.pop("linewidth", DEFAULT_PLAN_LINEWIDTH)
+        kwargs.pop("linestyle", "--")
+        show_waypoints = kwargs.pop("show_waypoints", frame == "global")
+        alpha = kwargs.pop("alpha", 0.8)
+
+        # Plot plan
+        if show_waypoints:
+            ax.plot(
+                plan_plot[:, 0],
+                plan_plot[:, 1],
+                "o-",
+                color=color,
+                linewidth=linewidth,
+                markersize=4,
+                label="Plan",
+                alpha=alpha,
+                **kwargs,
+            )
+        else:
+            ax.plot(
+                plan_plot[:, 0],
+                plan_plot[:, 1],
+                "-",
+                color=color,
+                linewidth=linewidth,
+                label="Plan",
+                alpha=alpha,
+                **kwargs,
+            )

simple_autonomous_car/sensors/__init__.py

@@ -0,0 +1,6 @@
+"""Sensor system for autonomous vehicles."""
+
+from simple_autonomous_car.sensors.base_sensor import BaseSensor
+from simple_autonomous_car.sensors.lidar_sensor import LiDARSensor
+
+__all__ = ["BaseSensor", "LiDARSensor"]

simple_autonomous_car/sensors/base_sensor.py

@@ -0,0 +1,105 @@
+"""Base sensor class for modular sensor system."""
+
+from abc import ABC, abstractmethod
+from typing import TYPE_CHECKING
+
+import numpy as np
+
+if TYPE_CHECKING:
+    from simple_autonomous_car.car.car import CarState
+
+from simple_autonomous_car.perception.perception import PerceptionPoints
+
+
+class BaseSensor(ABC):
+    """
+    Base class for all sensors.
+
+    This abstract class defines the interface that all sensors must implement.
+    Sensors can be added to a car and will provide perception data in ego frame.
+
+    Attributes
+    ----------
+    name : str
+        Name/identifier of the sensor.
+    pose_ego : np.ndarray
+        Sensor pose in ego frame [x, y, heading]. Default is [0, 0, 0] (at car origin).
+    max_range : float
+        Maximum sensor range in meters.
+    enabled : bool
+        Whether the sensor is enabled.
+    """
+
+    def __init__(
+        self,
+        name: str = "sensor",
+        pose_ego: np.ndarray | None = None,
+        max_range: float = 50.0,
+        enabled: bool = True,
+    ):
+        """
+        Initialize base sensor.
+
+        Parameters
+        ----------
+        name : str, default="sensor"
+            Name/identifier for this sensor instance.
+        pose_ego : np.ndarray, optional
+            Sensor pose in ego frame [x, y, heading] in meters and radians.
+            If None, sensor is at car origin with zero heading.
+        max_range : float, default=50.0
+            Maximum sensor range in meters.
+        enabled : bool, default=True
+            Whether the sensor is enabled. Disabled sensors return empty data.
+        """
+        self.name = name
+        self.pose_ego = pose_ego if pose_ego is not None else np.array([0.0, 0.0, 0.0])
+        self.max_range = max_range
+        self.enabled = enabled
+
+    @abstractmethod
+    def sense(self, car_state: "CarState", environment_data: dict) -> PerceptionPoints:
+        """
+        Sense the environment and return perception data.
+
+        This is the main method that all sensors must implement.
+        It takes the car state and environment data, and returns
+        perception points in ego frame.
+
+        Parameters
+        ----------
+        car_state : CarState
+            Current state of the car.
+        environment_data : dict
+            Dictionary containing environment data (e.g., ground truth map,
+            obstacles, etc.). Structure depends on sensor type.
+
+        Returns
+        -------
+        PerceptionPoints
+            Perception data in ego frame. Returns empty points if sensor is disabled.
+        """
+        pass
+
+    def is_enabled(self) -> bool:
+        """Check if sensor is enabled."""
+        return self.enabled
+
+    def enable(self) -> None:
+        """Enable the sensor."""
+        self.enabled = True
+
+    def disable(self) -> None:
+        """Disable the sensor."""
+        self.enabled = False
+
+    def set_pose(self, pose_ego: np.ndarray) -> None:
+        """
+        Set sensor pose in ego frame.
+
+        Parameters
+        ----------
+        pose_ego : np.ndarray
+            Sensor pose [x, y, heading] in ego frame.
+        """
+        self.pose_ego = np.asarray(pose_ego, dtype=np.float64)

simple_autonomous_car/sensors/lidar_sensor.py

@@ -0,0 +1,145 @@
+"""LiDAR sensor implementation."""
+
+import numpy as np
+
+# Import CarState here to avoid circular import
+from simple_autonomous_car.car.car import CarState
+from simple_autonomous_car.frames.frenet import sensor_to_ego
+from simple_autonomous_car.maps.ground_truth_map import GroundTruthMap
+from simple_autonomous_car.maps.perceived_map import PerceivedMap
+from simple_autonomous_car.perception.perception import PerceptionPoints
+from simple_autonomous_car.sensors.base_sensor import BaseSensor
+
+
+class LiDARSensor(BaseSensor):
+    """
+    LiDAR sensor that detects track boundaries.
+
+    This sensor simulates a 360° LiDAR that detects track boundaries
+    and returns a point cloud. It combines ground truth map data with
+    perception errors and sensor noise.
+
+    Parameters
+    ----------
+    ground_truth_map : GroundTruthMap
+        Ground truth map for reference.
+    perceived_map : PerceivedMap
+        Perceived map that adds localization errors.
+    angular_resolution : float, default=0.1
+        Angular resolution in radians (spacing between rays).
+    point_noise_std : float, default=0.1
+        Standard deviation of point measurement noise in meters.
+    name : str, default="lidar"
+        Sensor name/identifier.
+    pose_ego : np.ndarray, optional
+        Sensor pose in ego frame [x, y, heading].
+    max_range : float, default=50.0
+        Maximum sensor range in meters.
+    """
+
+    def __init__(
+        self,
+        ground_truth_map: GroundTruthMap,
+        perceived_map: PerceivedMap,
+        angular_resolution: float = 0.1,
+        point_noise_std: float = 0.1,
+        name: str = "lidar",
+        pose_ego: np.ndarray | None = None,
+        max_range: float = 50.0,
+    ):
+        super().__init__(name=name, pose_ego=pose_ego, max_range=max_range)
+        self.ground_truth_map = ground_truth_map
+        self.perceived_map = perceived_map
+        self.angular_resolution = angular_resolution
+        self.point_noise_std = point_noise_std
+
+    def sense(self, car_state: CarState, environment_data: dict | None = None) -> PerceptionPoints:
+        """
+        Sense track boundaries and return point cloud.
+
+        Parameters
+        ----------
+        car_state : CarState
+            Current car state.
+        environment_data : dict, optional
+            Additional environment data (not used for LiDAR).
+
+        Returns
+        -------
+        PerceptionPoints
+            Point cloud in ego frame. Returns empty if sensor is disabled.
+        """
+        if not self.enabled:
+            return PerceptionPoints(np.array([]).reshape(0, 2), frame="ego")
+
+        # Update perceived state
+        self.perceived_map.update_perceived_state(car_state)
+
+        # Get perceived segments (360 degree view)
+        perceived_centerline, perceived_inner, perceived_outer = (
+            self.perceived_map.get_perceived_segments(self.max_range, fov=2 * np.pi)
+        )
+
+        # Combine inner and outer bounds into perception points
+        if len(perceived_inner) > 0 and len(perceived_outer) > 0:
+            perception_points_car = np.vstack([perceived_inner, perceived_outer])
+        elif len(perceived_inner) > 0:
+            perception_points_car = perceived_inner
+        elif len(perceived_outer) > 0:
+            perception_points_car = perceived_outer
+        else:
+            perception_points_car = np.array([]).reshape(0, 2)
+
+        # Add sensor noise
+        if len(perception_points_car) > 0:
+            point_noise = np.random.normal(0, self.point_noise_std, perception_points_car.shape)
+            perception_points_car = perception_points_car + point_noise
+
+        # Transform from sensor frame to ego frame if sensor is not at origin
+        if not np.allclose(self.pose_ego, [0.0, 0.0, 0.0]):
+            perception_points_ego = np.array(
+                [sensor_to_ego(point, self.pose_ego) for point in perception_points_car]
+            )
+        else:
+            perception_points_ego = perception_points_car
+
+        return PerceptionPoints(perception_points_ego, frame="ego")
+
+    def get_map_lines_car_frame(self, car_state: CarState) -> tuple[np.ndarray, np.ndarray]:
+        """
+        Get ground truth map lines in car frame.
+
+        Parameters
+        ----------
+        car_state : CarState
+            Current car state.
+
+        Returns
+        -------
+        tuple[np.ndarray, np.ndarray]
+            Tuple of (map_lines_car_frame, map_lines_world_frame).
+        """
+        # Get all visible segments (360 degree view)
+        gt_centerline, gt_inner, gt_outer = self.ground_truth_map.get_visible_segments(
+            car_state.position(), car_state.heading, self.max_range, fov=2 * np.pi
+        )
+
+        # Combine inner and outer bounds
+        if len(gt_inner) > 0 and len(gt_outer) > 0:
+            map_lines_world = np.vstack([gt_inner, gt_outer])
+        elif len(gt_inner) > 0:
+            map_lines_world = gt_inner
+        elif len(gt_outer) > 0:
+            map_lines_world = gt_outer
+        else:
+            map_lines_world = np.array([]).reshape(0, 2)
+
+        # Transform to car frame
+        if len(map_lines_world) > 0:
+            map_lines_car = np.array(
+                [car_state.transform_to_car_frame(point) for point in map_lines_world]
+            )
+        else:
+            map_lines_car = np.array([]).reshape(0, 2)
+
+        return map_lines_car, map_lines_world

simple_autonomous_car/track/__init__.py

@@ -0,0 +1,5 @@
+"""Track generation and management."""
+
+from simple_autonomous_car.track.track import Track
+
+__all__ = ["Track"]