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

simple_autonomous_car/frames/frenet.py

@@ -0,0 +1,267 @@
+"""Frenet frame coordinate system utilities.
+
+Frenet frame: (s, d) where:
+- s: distance along the path (longitudinal)
+- d: lateral offset from the path (positive = left, negative = right)
+"""
+
+from typing import Any
+
+import numpy as np
+
+from simple_autonomous_car.car.car import CarState
+from simple_autonomous_car.track.track import Track
+
+
+class FrenetFrame:
+    """Frenet frame representation along a path."""
+
+    def __init__(self, track: Track):
+        """
+        Initialize Frenet frame with a reference path (track centerline).
+
+        Args:
+            track: Track object providing the reference path
+        """
+        self.track = track
+        self._compute_cumulative_distances()
+
+    def _compute_cumulative_distances(self) -> None:
+        """Precompute cumulative distances along the 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 get_closest_point(self, point: np.ndarray) -> tuple[int, float]:
+        """
+        Find closest point on centerline to given point.
+
+        Args:
+            point: Point in global frame [x, y]
+
+        Returns:
+            Tuple of (index, distance_along_path)
+        """
+        distances = np.linalg.norm(self.track.centerline - point, axis=1)
+        closest_idx_int = np.argmin(distances)
+        closest_idx: int = int(closest_idx_int)
+        s = self.cumulative_distances[closest_idx]
+        return closest_idx, float(s)
+
+    def get_tangent_normal(self, s: float) -> tuple[np.ndarray, np.ndarray]:
+        """
+        Get tangent and normal vectors at distance s along path.
+
+        Args:
+            s: Distance along path
+
+        Returns:
+            Tuple of (tangent_vector, normal_vector)
+        """
+        s = s % self.total_length  # Wrap around
+
+        # Find segment
+        idx = np.searchsorted(self.cumulative_distances, s)
+        if idx == 0:  # type: ignore[assignment]
+            idx = 1  # type: ignore[assignment]
+        if idx >= len(self.track.centerline):  # type: ignore[assignment]
+            idx = len(self.track.centerline) - 1  # type: ignore[assignment]
+
+        # Get direction
+        direction = self.track.centerline[idx] - self.track.centerline[idx - 1]
+        direction_norm = np.linalg.norm(direction)
+        if direction_norm > 1e-6:
+            tangent = direction / direction_norm
+        else:
+            tangent = np.array([1.0, 0.0])
+
+        # Normal (perpendicular, pointing left)
+        normal = np.array([-tangent[1], tangent[0]])
+
+        return tangent, normal
+
+    def global_to_frenet(self, point: np.ndarray) -> tuple[float, float]:
+        """
+        Convert point from global frame to Frenet frame.
+
+        Args:
+            point: Point in global frame [x, y]
+
+        Returns:
+            Tuple of (s, d) where s is distance along path, d is lateral offset
+        """
+        closest_idx, s = self.get_closest_point(point)
+
+        # Get point on centerline
+        centerline_point = self.track.centerline[closest_idx]
+
+        # Get tangent and normal at this point
+        tangent, normal = self.get_tangent_normal(s)
+
+        # Compute lateral offset
+        vector_to_point = point - centerline_point
+        d = np.dot(vector_to_point, normal)
+
+        return s, d
+
+    def frenet_to_global(self, s: float, d: float) -> np.ndarray:
+        """
+        Convert point from Frenet frame to global frame.
+
+        Args:
+            s: Distance along path
+            d: Lateral offset (positive = left)
+
+        Returns:
+            Point in global frame [x, y]
+        """
+        s = s % self.total_length  # Wrap around
+
+        # Find segment
+        idx_int = np.searchsorted(self.cumulative_distances, s)
+        idx: int = int(idx_int.item() if hasattr(idx_int, "item") else idx_int)
+        if idx == 0:
+            idx = 1
+        if idx >= len(self.track.centerline):
+            idx = len(self.track.centerline) - 1
+
+        # Interpolate along centerline
+        segment_dist = s - self.cumulative_distances[idx - 1]
+        segment_length = self.cumulative_distances[idx] - self.cumulative_distances[idx - 1]
+        if segment_length > 1e-6:
+            t = segment_dist / segment_length
+        else:
+            t = 0.0
+
+        centerline_point = self.track.centerline[idx - 1] + t * (
+            self.track.centerline[idx] - self.track.centerline[idx - 1]
+        )
+
+        # Get tangent and normal
+        tangent, normal = self.get_tangent_normal(s)
+
+        # Offset by d in normal direction
+        global_point = centerline_point + d * normal
+
+        return np.asarray(global_point, dtype=np.float64)
+
+
+# Convenience functions
+def global_to_frenet(point: np.ndarray, frenet_frame: FrenetFrame) -> tuple[float, float]:
+    """Convert point from global to Frenet frame."""
+    return frenet_frame.global_to_frenet(point)
+
+
+def frenet_to_global(s: float, d: float, frenet_frame: FrenetFrame) -> np.ndarray:
+    """Convert point from Frenet to global frame."""
+    return frenet_frame.frenet_to_global(s, d)
+
+
+def ego_to_frenet(
+    point_ego: np.ndarray, car_state: CarState, frenet_frame: FrenetFrame
+) -> tuple[float, float]:
+    """
+    Convert point from ego frame to Frenet frame.
+
+    Args:
+        point_ego: Point in ego frame [x, y] (x=forward, y=left)
+        car_state: Current car state
+        frenet_frame: Frenet frame instance
+
+    Returns:
+        Tuple of (s, d) in Frenet frame
+    """
+    # Convert ego to global
+    point_global = car_state.transform_to_world_frame(point_ego)
+    # Convert global to Frenet
+    return frenet_frame.global_to_frenet(point_global)
+
+
+# Add method to FrenetFrame class for convenience
+def _ego_to_frenet_method(
+    self: Any, point_ego: np.ndarray, car_state: CarState
+) -> tuple[float, float]:
+    """Convert point from ego frame to Frenet frame (instance method)."""
+    return ego_to_frenet(point_ego, car_state, self)
+
+
+FrenetFrame.ego_to_frenet = _ego_to_frenet_method  # type: ignore[attr-defined]
+
+
+def frenet_to_ego(s: float, d: float, car_state: CarState, frenet_frame: FrenetFrame) -> np.ndarray:
+    """
+    Convert point from Frenet frame to ego frame.
+
+    Args:
+        s: Distance along path
+        d: Lateral offset
+        car_state: Current car state
+        frenet_frame: Frenet frame instance
+
+    Returns:
+        Point in ego frame [x, y]
+    """
+    # Convert Frenet to global
+    point_global = frenet_frame.frenet_to_global(s, d)
+    # Convert global to ego
+    return car_state.transform_to_car_frame(point_global)
+
+
+def sensor_to_ego(
+    point_sensor: np.ndarray, sensor_pose_ego: np.ndarray | None = None
+) -> np.ndarray:
+    """
+    Convert point from sensor frame to ego frame.
+
+    Args:
+        point_sensor: Point in sensor frame [x, y]
+        sensor_pose_ego: Sensor pose in ego frame [x, y, heading].
+                        If None, assumes sensor is at ego origin.
+
+    Returns:
+        Point in ego frame [x, y]
+    """
+    if sensor_pose_ego is None:
+        # Sensor at ego origin, no transformation needed
+        return point_sensor
+
+    sensor_x, sensor_y, sensor_heading = sensor_pose_ego
+    cos_h = np.cos(sensor_heading)
+    sin_h = np.sin(sensor_heading)
+
+    # Rotation matrix from sensor to ego
+    rotation = np.array([[cos_h, -sin_h], [sin_h, cos_h]])
+
+    # Transform
+    point_ego = rotation @ point_sensor + np.array([sensor_x, sensor_y])
+    return np.asarray(point_ego, dtype=np.float64)
+
+
+def ego_to_sensor(point_ego: np.ndarray, sensor_pose_ego: np.ndarray | None = None) -> np.ndarray:
+    """
+    Convert point from ego frame to sensor frame.
+
+    Args:
+        point_ego: Point in ego frame [x, y]
+        sensor_pose_ego: Sensor pose in ego frame [x, y, heading].
+                        If None, assumes sensor is at ego origin.
+
+    Returns:
+        Point in sensor frame [x, y]
+    """
+    if sensor_pose_ego is None:
+        # Sensor at ego origin, no transformation needed
+        return point_ego
+
+    sensor_x, sensor_y, sensor_heading = sensor_pose_ego
+    cos_h = np.cos(sensor_heading)
+    sin_h = np.sin(sensor_heading)
+
+    # Rotation matrix from ego to sensor (inverse)
+    rotation = np.array([[cos_h, sin_h], [-sin_h, cos_h]])
+
+    # Transform
+    point_sensor = rotation @ (point_ego - np.array([sensor_x, sensor_y]))
+    return np.asarray(point_sensor, dtype=np.float64)

simple_autonomous_car/maps/__init__.py

@@ -0,0 +1,9 @@
+"""Map representations for ground truth and perception."""
+
+from simple_autonomous_car.maps.frenet_map import FrenetMap
+from simple_autonomous_car.maps.grid_ground_truth_map import GridGroundTruthMap
+from simple_autonomous_car.maps.grid_map import GridMap
+from simple_autonomous_car.maps.ground_truth_map import GroundTruthMap
+from simple_autonomous_car.maps.perceived_map import PerceivedMap
+
+__all__ = ["GroundTruthMap", "PerceivedMap", "FrenetMap", "GridMap", "GridGroundTruthMap"]

simple_autonomous_car/maps/frenet_map.py

@@ -0,0 +1,97 @@
+"""Map representation in Frenet frame."""
+
+import numpy as np
+
+from simple_autonomous_car.frames.frenet import FrenetFrame
+from simple_autonomous_car.track.track import Track
+
+
+class FrenetMap:
+    """Map representation in Frenet frame coordinates."""
+
+    def __init__(self, track: Track):
+        """
+        Initialize Frenet map.
+
+        Args:
+            track: Track object
+        """
+        self.track = track
+        self.frenet_frame = FrenetFrame(track)
+        self._compute_frenet_bounds()
+
+    def _compute_frenet_bounds(self) -> None:
+        """Compute track boundaries in Frenet frame."""
+        # Convert inner and outer bounds to Frenet coordinates
+        inner_frenet = []
+        outer_frenet = []
+
+        for i in range(len(self.track.centerline)):
+            inner_point = self.track.inner_bound[i]
+            outer_point = self.track.outer_bound[i]
+
+            s_inner, d_inner = self.frenet_frame.global_to_frenet(inner_point)
+            s_outer, d_outer = self.frenet_frame.global_to_frenet(outer_point)
+
+            inner_frenet.append([s_inner, d_inner])
+            outer_frenet.append([s_outer, d_outer])
+
+        self.inner_bound_frenet = np.array(inner_frenet)
+        self.outer_bound_frenet = np.array(outer_frenet)
+
+    def get_bounds_at_s(self, s: float) -> tuple[float, float]:
+        """
+        Get inner and outer boundary lateral offsets at distance s.
+
+        Args:
+            s: Distance along path
+
+        Returns:
+            Tuple of (d_inner, d_outer) lateral offsets
+        """
+        # Find closest point
+        s_wrapped = s % self.frenet_frame.total_length
+
+        # Find segment
+        idx = np.searchsorted(self.frenet_frame.cumulative_distances, s_wrapped)
+        if idx == 0:
+            idx = 1
+        if idx >= len(self.track.centerline):
+            idx = len(self.track.centerline) - 1
+
+        # Get bounds at this point
+        d_inner = self.inner_bound_frenet[idx, 1]
+        d_outer = self.outer_bound_frenet[idx, 1]
+
+        return d_inner, d_outer
+
+    def get_bounds_in_range(self, s_start: float, s_end: float) -> tuple[np.ndarray, np.ndarray]:
+        """
+        Get inner and outer bounds in a range of s values.
+
+        Args:
+            s_start: Start distance along path
+            s_end: End distance along path
+
+        Returns:
+            Tuple of (inner_bounds, outer_bounds) as arrays of shape (N, 2) with [s, d]
+        """
+        # Find points in range
+        s_start_wrapped = s_start % self.frenet_frame.total_length
+        s_end_wrapped = s_end % self.frenet_frame.total_length
+
+        # Get all points in range
+        if s_start_wrapped <= s_end_wrapped:
+            mask = (self.inner_bound_frenet[:, 0] >= s_start_wrapped) & (
+                self.inner_bound_frenet[:, 0] <= s_end_wrapped
+            )
+        else:
+            # Wraps around
+            mask = (self.inner_bound_frenet[:, 0] >= s_start_wrapped) | (
+                self.inner_bound_frenet[:, 0] <= s_end_wrapped
+            )
+
+        inner_bounds = self.inner_bound_frenet[mask]
+        outer_bounds = self.outer_bound_frenet[mask]
+
+        return inner_bounds, outer_bounds

simple_autonomous_car/maps/grid_ground_truth_map.py

@@ -0,0 +1,83 @@
+"""Ground truth map for grid-based environments."""
+
+import numpy as np
+
+from simple_autonomous_car.maps.grid_map import GridMap
+
+
+class GridGroundTruthMap:
+    """
+    Ground truth map for grid-based environments with obstacles.
+
+    This provides a compatible interface with GroundTruthMap for sensors
+    to work with grid map environments.
+
+    Parameters
+    ----------
+    grid_map : GridMap
+        Grid map with obstacles.
+    """
+
+    def __init__(self, grid_map: GridMap):
+        """Initialize ground truth map from grid map."""
+        self.grid_map = grid_map
+        self.track = None  # No track for grid maps
+
+    def get_visible_segments(
+        self, car_position: np.ndarray, car_heading: float, horizon: float, fov: float = 2 * np.pi
+    ) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+        """
+        Get visible obstacles within horizon and field of view.
+
+        Parameters
+        ----------
+        car_position : np.ndarray
+            Car position [x, y] in world frame.
+        car_heading : float
+            Car heading angle in radians.
+        horizon : float
+            Maximum distance to consider.
+        fov : float, default=2*pi
+            Field of view angle in radians.
+
+        Returns
+        -------
+        Tuple[np.ndarray, np.ndarray, np.ndarray]
+            (visible_obstacles, empty, empty) - compatible with track-based interface.
+        """
+        # Get obstacles within range
+        obstacles = self.grid_map.obstacles
+        if len(obstacles) == 0:
+            return (
+                np.array([]).reshape(0, 2),
+                np.array([]).reshape(0, 2),
+                np.array([]).reshape(0, 2),
+            )
+
+        # Calculate distances from car
+        vectors = obstacles - car_position
+        distances = np.linalg.norm(vectors, axis=1)
+
+        # Filter by distance (consider obstacle size)
+        within_horizon = distances <= (horizon + self.grid_map.obstacle_size)
+
+        # Filter by FOV (if not 360 degrees)
+        if fov < 2 * np.pi:
+            angles = np.arctan2(vectors[:, 1], vectors[:, 0]) - car_heading
+            # Normalize angles to [-pi, pi]
+            angles = np.arctan2(np.sin(angles), np.cos(angles))
+            within_fov = np.abs(angles) <= fov / 2.0
+            visible_mask = within_horizon & within_fov
+        else:
+            visible_mask = within_horizon
+
+        visible_obstacles = obstacles[visible_mask]
+
+        # Return in compatible format (centerline, inner, outer)
+        # For grid maps, we return obstacles as "centerline", empty arrays for bounds
+        return visible_obstacles, np.array([]).reshape(0, 2), np.array([]).reshape(0, 2)
+
+    def get_all_bounds(self) -> tuple[np.ndarray, np.ndarray]:
+        """Get map boundaries (for compatibility)."""
+        bounds, _ = self.grid_map.get_bounds()
+        return bounds, bounds.copy()