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

simple_autonomous_car/maps/grid_map.py

@@ -0,0 +1,361 @@
+"""Grid-based map with obstacles for goal-based navigation."""
+
+from typing import TYPE_CHECKING, Any, Optional
+
+import matplotlib.patches as patches
+import numpy as np
+
+if TYPE_CHECKING:
+    from simple_autonomous_car.car.car import CarState
+
+
+class GridMap:
+    """
+    Represents a grid-based map with obstacles for goal-based navigation.
+
+    This is similar to Track but for grid-based environments with obstacles.
+    It provides a compatible interface for visualization and planning.
+
+    Parameters
+    ----------
+    width : float
+        Width of the map in meters.
+    height : float
+        Height of the map in meters.
+    resolution : float, default=0.5
+        Resolution in meters per cell.
+    obstacles : np.ndarray, optional
+        Array of obstacle positions [[x1, y1], [x2, y2], ...] or None.
+    obstacle_size : float, default=1.0
+        Size of each obstacle (radius or half-width).
+    """
+
+    def __init__(
+        self,
+        width: float,
+        height: float,
+        resolution: float = 0.5,
+        obstacles: np.ndarray | None = None,
+        obstacle_size: float = 1.0,
+    ):
+        self.width = width
+        self.height = height
+        self.resolution = resolution
+        self.obstacle_size = obstacle_size
+
+        # Store obstacles
+        if obstacles is None:
+            self.obstacles = np.array([]).reshape(0, 2)
+        else:
+            self.obstacles = np.asarray(obstacles, dtype=np.float64)
+            if self.obstacles.ndim != 2 or self.obstacles.shape[1] != 2:
+                raise ValueError("obstacles must be shape (N, 2)")
+
+        # Create grid representation for fast collision checking
+        self._create_grid()
+
+    def _create_grid(self) -> None:
+        """Create internal grid representation of obstacles."""
+        self.width_cells = int(self.width / self.resolution)
+        self.height_cells = int(self.height / self.resolution)
+        self.grid = np.zeros((self.height_cells, self.width_cells), dtype=bool)
+
+        # Mark obstacles in grid
+        for obstacle in self.obstacles:
+            x, y = obstacle
+            # Convert to grid coordinates
+            grid_x = int((x + self.width / 2) / self.resolution)
+            grid_y = int((y + self.height / 2) / self.resolution)
+
+            # Mark cells within obstacle_size
+            radius_cells = int(self.obstacle_size / self.resolution)
+            for dy in range(-radius_cells, radius_cells + 1):
+                for dx in range(-radius_cells, radius_cells + 1):
+                    gx = grid_x + dx
+                    gy = grid_y + dy
+                    if 0 <= gx < self.width_cells and 0 <= gy < self.height_cells:
+                        dist = np.sqrt(dx**2 + dy**2) * self.resolution
+                        if dist <= self.obstacle_size:
+                            self.grid[gy, gx] = True
+
+    def is_obstacle(self, position: np.ndarray) -> bool:
+        """
+        Check if a position is occupied by an obstacle.
+
+        Parameters
+        ----------
+        position : np.ndarray
+            Position [x, y] in world coordinates.
+
+        Returns
+        -------
+        bool
+            True if position is occupied, False otherwise.
+        """
+        # Convert to grid coordinates
+        grid_x = int((position[0] + self.width / 2) / self.resolution)
+        grid_y = int((position[1] + self.height / 2) / self.resolution)
+
+        if 0 <= grid_x < self.width_cells and 0 <= grid_y < self.height_cells:
+            result = self.grid[grid_y, grid_x]
+            return bool(result)
+        return True  # Out of bounds is considered obstacle
+
+    def is_valid_position(self, position: np.ndarray) -> bool:
+        """
+        Check if a position is valid (within bounds and not obstacle).
+
+        Parameters
+        ----------
+        position : np.ndarray
+            Position [x, y] in world coordinates.
+
+        Returns
+        -------
+        bool
+            True if position is valid, False otherwise.
+        """
+        x, y = position
+        # Check bounds
+        if x < -self.width / 2 or x > self.width / 2:
+            return False
+        if y < -self.height / 2 or y > self.height / 2:
+            return False
+
+        # Check obstacles
+        return not self.is_obstacle(position)
+
+    def get_bounds(self) -> tuple[np.ndarray, np.ndarray]:
+        """
+        Get map boundaries (for compatibility with Track interface).
+
+        Returns
+        -------
+        Tuple[np.ndarray, np.ndarray]
+            (inner_bound, outer_bound) - both are the same for grid maps.
+        """
+        bounds = np.array(
+            [
+                [-self.width / 2, -self.height / 2],
+                [self.width / 2, -self.height / 2],
+                [self.width / 2, self.height / 2],
+                [-self.width / 2, self.height / 2],
+                [-self.width / 2, -self.height / 2],  # Close the loop
+            ]
+        )
+        return bounds, bounds.copy()
+
+    def get_boundary_obstacles(self, spacing: float | None = None) -> np.ndarray:
+        """
+        Get boundary points as obstacles for costmap.
+
+        Samples points along the map boundaries at regular intervals.
+
+        Parameters
+        ----------
+        spacing : float, optional
+            Spacing between boundary points in meters.
+            If None, uses resolution.
+
+        Returns
+        -------
+        np.ndarray
+            Array of boundary obstacle positions [[x1, y1], [x2, y2], ...]
+        """
+        if spacing is None:
+            spacing = self.resolution
+
+        boundary_points = []
+
+        # Bottom edge (left to right)
+        num_points_x = int(self.width / spacing) + 1
+        for i in range(num_points_x):
+            x = -self.width / 2 + i * spacing
+            if x > self.width / 2:
+                x = self.width / 2
+            boundary_points.append([x, -self.height / 2])
+
+        # Right edge (bottom to top)
+        num_points_y = int(self.height / spacing) + 1
+        for i in range(1, num_points_y):  # Skip corner already added
+            y = -self.height / 2 + i * spacing
+            if y > self.height / 2:
+                y = self.height / 2
+            boundary_points.append([self.width / 2, y])
+
+        # Top edge (right to left)
+        for i in range(1, num_points_x):  # Skip corner already added
+            x = self.width / 2 - i * spacing
+            if x < -self.width / 2:
+                x = -self.width / 2
+            boundary_points.append([x, self.height / 2])
+
+        # Left edge (top to bottom)
+        for i in range(1, num_points_y - 1):  # Skip both corners already added
+            y = self.height / 2 - i * spacing
+            if y < -self.height / 2:
+                y = -self.height / 2
+            boundary_points.append([-self.width / 2, y])
+
+        return np.array(boundary_points)
+
+    @classmethod
+    def create_random_map(
+        cls,
+        width: float = 50.0,
+        height: float = 50.0,
+        resolution: float = 0.5,
+        num_obstacles: int = 10,
+        obstacle_size: float = 2.0,
+        seed: int | None = None,
+    ) -> "GridMap":
+        """
+        Create a random grid map with obstacles.
+
+        Parameters
+        ----------
+        width : float, default=50.0
+            Width of map in meters.
+        height : float, default=50.0
+            Height of map in meters.
+        resolution : float, default=0.5
+            Resolution in meters per cell.
+        num_obstacles : int, default=10
+            Number of obstacles to place.
+        obstacle_size : float, default=2.0
+            Size of each obstacle.
+        seed : int, optional
+            Random seed for reproducibility.
+
+        Returns
+        -------
+        GridMap
+            Random grid map instance.
+        """
+        if seed is not None:
+            np.random.seed(seed)
+
+        # Generate random obstacle positions
+        obstacles = []
+        margin = obstacle_size + 2.0  # Keep obstacles away from edges
+
+        for _ in range(num_obstacles):
+            x = np.random.uniform(-width / 2 + margin, width / 2 - margin)
+            y = np.random.uniform(-height / 2 + margin, height / 2 - margin)
+            obstacles.append([x, y])
+
+        return cls(
+            width=width,
+            height=height,
+            resolution=resolution,
+            obstacles=np.array(obstacles),
+            obstacle_size=obstacle_size,
+        )
+
+    def visualize(
+        self,
+        ax: Any,
+        car_state: Optional["CarState"] = None,
+        frame: str = "global",
+        goal: np.ndarray | None = None,
+        horizon: float | None = None,
+        **kwargs: Any,
+    ) -> None:
+        """
+        Visualize the grid map.
+
+        Parameters
+        ----------
+        ax : matplotlib.axes.Axes
+            Axes to plot on.
+        car_state : CarState, optional
+            Current car state (for frame transformation).
+        frame : str, default="global"
+            Frame to plot in: "global" or "ego".
+        goal : np.ndarray, optional
+            Goal position [x, y] to display.
+        horizon : float, optional
+            Maximum distance to display (for ego frame filtering).
+        **kwargs
+            Additional visualization arguments.
+        """
+        # Extract visualization parameters
+        alpha = kwargs.pop("alpha", 0.3)
+        obstacle_color = kwargs.pop("obstacle_color", "red")
+        goal_color = kwargs.pop("goal_color", "green")
+        goal_size = kwargs.pop("goal_size", 1.0)
+
+        # Draw map boundaries
+        bounds, _ = self.get_bounds()
+        if frame == "ego" and horizon is not None and car_state is not None:
+            # Transform boundaries to ego frame and filter by horizon
+            bounds_ego = np.array([car_state.transform_to_car_frame(b) for b in bounds])
+            distances = np.linalg.norm(bounds_ego, axis=1)
+            # Show boundaries that are within horizon OR form a visible segment
+            # Keep at least 4 points to form a rectangle
+            if len(bounds_ego) >= 4:
+                # For a rectangle, show all points but clip to horizon if needed
+                # Draw segments that are within horizon
+                for i in range(len(bounds_ego)):
+                    next_i = (i + 1) % len(bounds_ego)
+                    if distances[i] <= horizon or distances[next_i] <= horizon:
+                        # Draw segment if at least one endpoint is visible
+                        ax.plot(
+                            [bounds_ego[i, 0], bounds_ego[next_i, 0]],
+                            [bounds_ego[i, 1], bounds_ego[next_i, 1]],
+                            "k-",
+                            linewidth=2,
+                            label="Map Bounds" if i == 0 else "",
+                            **kwargs,
+                        )
+        else:
+            ax.plot(bounds[:, 0], bounds[:, 1], "k-", linewidth=2, label="Map Bounds", **kwargs)
+
+        # Draw obstacles (filter by horizon in ego frame)
+        for obstacle in self.obstacles:
+            if frame == "ego" and car_state is not None:
+                # Transform to ego frame
+                obstacle_ego = car_state.transform_to_car_frame(obstacle)
+                circle = patches.Circle(
+                    (float(obstacle_ego[0]), float(obstacle_ego[1])),
+                    self.obstacle_size,
+                    color=obstacle_color,
+                    alpha=alpha,
+                    **kwargs,
+                )
+            else:
+                circle = patches.Circle(
+                    (float(obstacle[0]), float(obstacle[1])),
+                    self.obstacle_size,
+                    color=obstacle_color,
+                    alpha=alpha,
+                    **kwargs,
+                )
+            ax.add_patch(circle)
+
+        # Draw goal if provided (filter by horizon in ego frame)
+        if goal is not None:
+            if frame == "ego" and car_state is not None:
+                goal_plot = car_state.transform_to_car_frame(goal)
+                # Filter by horizon
+                if horizon is not None:
+                    distance = np.linalg.norm(goal_plot)
+                    if distance > horizon:
+                        return  # Goal outside horizon, don't draw
+            else:
+                goal_plot = goal
+
+            ax.plot(
+                goal_plot[0],
+                goal_plot[1],
+                "o",
+                color=goal_color,
+                markersize=goal_size * 10,
+                label="Goal",
+                markeredgecolor="black",
+                markeredgewidth=2,
+                **kwargs,
+            )
+
+        ax.set_aspect("equal")
+        ax.grid(True, alpha=0.3)

simple_autonomous_car/maps/ground_truth_map.py

@@ -0,0 +1,64 @@
+"""Ground truth map representation."""
+
+import numpy as np
+
+from simple_autonomous_car.track.track import Track
+
+
+class GroundTruthMap:
+    """Ground truth map of the track in world coordinates."""
+
+    def __init__(self, track: Track):
+        """
+        Initialize ground truth map.
+
+        Args:
+            track: Track object containing centerline and bounds
+        """
+        self.track = track
+        self.centerline = track.centerline.copy()
+        self.inner_bound = track.inner_bound.copy()
+        self.outer_bound = track.outer_bound.copy()
+
+    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 segments of the track within horizon and field of view.
+
+        Args:
+            car_position: Car position [x, y] in world frame
+            car_heading: Car heading angle in radians
+            horizon: Maximum distance to consider
+            fov: Field of view angle in radians (default: 2*pi = 360 degrees)
+
+        Returns:
+            Tuple of (visible_centerline, visible_inner_bound, visible_outer_bound)
+        """
+        # Calculate distances and angles from car
+        vectors = self.centerline - car_position
+        distances = np.linalg.norm(vectors, axis=1)
+
+        # Filter by distance
+        within_horizon = distances <= horizon
+
+        # 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:
+            # 360 degree view - only filter by distance
+            visible_mask = within_horizon
+
+        visible_centerline = self.centerline[visible_mask]
+        visible_inner = self.inner_bound[visible_mask]
+        visible_outer = self.outer_bound[visible_mask]
+
+        return visible_centerline, visible_inner, visible_outer
+
+    def get_all_bounds(self) -> tuple[np.ndarray, np.ndarray]:
+        """Get all inner and outer bounds."""
+        return self.inner_bound, self.outer_bound

simple_autonomous_car/maps/perceived_map.py

@@ -0,0 +1,169 @@
+"""Perceived map with noise and reference frame transformations."""
+
+from typing import TYPE_CHECKING
+
+import numpy as np
+
+if TYPE_CHECKING:
+    from simple_autonomous_car.car.car import CarState
+
+from simple_autonomous_car.maps.ground_truth_map import GroundTruthMap
+
+
+class PerceivedMap:
+    """Perceived map with potential errors and noise."""
+
+    def __init__(
+        self,
+        ground_truth: GroundTruthMap,
+        position_noise_std: float = 0.1,
+        orientation_noise_std: float = 0.05,
+        measurement_noise_std: float = 0.2,
+    ):
+        """
+        Initialize perceived map.
+
+        Args:
+            ground_truth: Ground truth map
+            position_noise_std: Standard deviation of position estimation error
+            orientation_noise_std: Standard deviation of orientation estimation error
+            measurement_noise_std: Standard deviation of measurement noise
+        """
+        self.ground_truth = ground_truth
+        self.position_noise_std = position_noise_std
+        self.orientation_noise_std = orientation_noise_std
+        self.measurement_noise_std = measurement_noise_std
+
+        # Perceived car state (with localization errors)
+        self.perceived_car_state: CarState | None = None
+
+    def update_perceived_state(self, true_car_state: "CarState") -> None:
+        """
+        Update perceived car state with localization errors.
+
+        Args:
+            true_car_state: True car state
+        """
+        # Add position noise
+        position_noise = np.random.normal(0, self.position_noise_std, 2)
+        perceived_x = true_car_state.x + position_noise[0]
+        perceived_y = true_car_state.y + position_noise[1]
+
+        # Add orientation noise
+        orientation_noise = np.random.normal(0, self.orientation_noise_std)
+        perceived_heading = true_car_state.heading + orientation_noise
+
+        # Import here to avoid circular import
+        from simple_autonomous_car.car.car import CarState
+
+        self.perceived_car_state = CarState(
+            x=perceived_x,
+            y=perceived_y,
+            heading=perceived_heading,
+            velocity=true_car_state.velocity,
+            steering_angle=true_car_state.steering_angle,
+        )
+
+    def get_perceived_segments(
+        self, horizon: float, fov: float = 2 * np.pi
+    ) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+        """
+        Get perceived track segments in perceived car frame.
+
+        Args:
+            horizon: Maximum distance to consider
+            fov: Field of view angle in radians
+
+        Returns:
+            Tuple of (perceived_centerline_car_frame, perceived_inner_car_frame, perceived_outer_car_frame)
+        """
+        if self.perceived_car_state is None:
+            raise ValueError("Perceived car state not set. Call update_perceived_state first.")
+
+        # Get visible segments from ground truth in world frame
+        visible_centerline, visible_inner, visible_outer = self.ground_truth.get_visible_segments(
+            self.perceived_car_state.position(),
+            self.perceived_car_state.heading,
+            horizon,
+            fov,
+        )
+
+        if len(visible_centerline) == 0:
+            return (
+                np.array([]).reshape(0, 2),
+                np.array([]).reshape(0, 2),
+                np.array([]).reshape(0, 2),
+            )
+
+        # Add measurement noise
+        centerline_noise = np.random.normal(0, self.measurement_noise_std, visible_centerline.shape)
+        inner_noise = np.random.normal(0, self.measurement_noise_std, visible_inner.shape)
+        outer_noise = np.random.normal(0, self.measurement_noise_std, visible_outer.shape)
+
+        perceived_centerline_world = visible_centerline + centerline_noise
+        perceived_inner_world = visible_inner + inner_noise
+        perceived_outer_world = visible_outer + outer_noise
+
+        # Transform to perceived car frame
+        perceived_centerline_car = np.array(
+            [
+                self.perceived_car_state.transform_to_car_frame(point)
+                for point in perceived_centerline_world
+            ]
+        )
+        perceived_inner_car = np.array(
+            [
+                self.perceived_car_state.transform_to_car_frame(point)
+                for point in perceived_inner_world
+            ]
+        )
+        perceived_outer_car = np.array(
+            [
+                self.perceived_car_state.transform_to_car_frame(point)
+                for point in perceived_outer_world
+            ]
+        )
+
+        return perceived_centerline_car, perceived_inner_car, perceived_outer_car
+
+    def get_perceived_segments_world_frame(
+        self, horizon: float, fov: float = 2 * np.pi
+    ) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+        """
+        Get perceived track segments in world frame.
+
+        Args:
+            horizon: Maximum distance to consider
+            fov: Field of view angle in radians
+
+        Returns:
+            Tuple of (perceived_centerline_world, perceived_inner_world, perceived_outer_world)
+        """
+        if self.perceived_car_state is None:
+            raise ValueError("Perceived car state not set. Call update_perceived_state first.")
+
+        # Get visible segments from ground truth
+        visible_centerline, visible_inner, visible_outer = self.ground_truth.get_visible_segments(
+            self.perceived_car_state.position(),
+            self.perceived_car_state.heading,
+            horizon,
+            fov,
+        )
+
+        if len(visible_centerline) == 0:
+            return (
+                np.array([]).reshape(0, 2),
+                np.array([]).reshape(0, 2),
+                np.array([]).reshape(0, 2),
+            )
+
+        # Add measurement noise
+        centerline_noise = np.random.normal(0, self.measurement_noise_std, visible_centerline.shape)
+        inner_noise = np.random.normal(0, self.measurement_noise_std, visible_inner.shape)
+        outer_noise = np.random.normal(0, self.measurement_noise_std, visible_outer.shape)
+
+        perceived_centerline_world = visible_centerline + centerline_noise
+        perceived_inner_world = visible_inner + inner_noise
+        perceived_outer_world = visible_outer + outer_noise
+
+        return perceived_centerline_world, perceived_inner_world, perceived_outer_world

simple_autonomous_car/perception/__init__.py

@@ -0,0 +1,5 @@
+"""Perception data structures and utilities."""
+
+from simple_autonomous_car.perception.perception import PerceptionPoints
+
+__all__ = ["PerceptionPoints"]