nav-sim2d 0.0.1__tar.gz

nav_sim2d-0.0.1/PKG-INFO

@@ -0,0 +1,236 @@
+Metadata-Version: 2.4
+Name: nav-sim2d
+Version: 0.0.1
+Summary: Educational library of 2D simulation for mobile robot navigation.
+Author: Elias J. R. Freitas
+Requires-Python: >=3.10
+Description-Content-Type: text/markdown
+Requires-Dist: pygame>=2.5.0
+Requires-Dist: pygame_gui>=0.6.9
+Requires-Dist: numpy>=1.24
+Requires-Dist: PyYAML>=6.0
+
+# Nav Sim
+
+A lightweight 2D simulation framework for mobile robot navigation using **Pygame**, with support for sensors, external controllers, and interactive visualization.
+
+---
+
+## Supported Robot Models
+
+- `holonomic`  
+  Command:
+  ```python
+  {"vx": ..., "vy": ...}
+  ```
+
+- `differential`  
+  Command:
+  ```python
+  {"v": ..., "omega": ...}
+  ```
+
+- `ackermann`  
+  Command:
+  ```python
+  {"speed": ..., "steer": ...}
+  ```
+
+---
+
+## 🧠 Architecture
+
+The simulator follows a modular design:
+
+- ✅ **Simulator (`nav_sim`)**  
+  Handles physics, rendering, environment, and sensors
+
+- ✅ **External Controllers**  
+  Implement navigation strategies (APF, Vortex, etc.)
+
+> Controllers are not part of the core library. They live in `examples/` or in the user’s project.
+
+---
+
+## 📡 Sensors
+
+### LiDAR (2D)
+
+The simulator provides a configurable 2D LiDAR:
+
+- Angular scan
+- Range-limited
+- Returns:
+
+```python
+(angle, distance, hit_point)
+```
+
+- Passed to controllers as:
+
+```python
+lidar_readings
+```
+
+---
+
+## ⚙️ Installation
+
+```bash
+python -m venv .venv
+
+# Linux / macOS
+source .venv/bin/activate  
+
+# Windows
+.venv\Scripts\activate
+
+pip install -r requirements.txt
+```
+
+---
+
+## ▶️ Run Examples
+
+```bash
+python examples/run_holonomic.py
+python examples/run_differential.py
+python examples/run_ackermann.py
+```
+
+---
+
+## 🧪 Basic Usage
+
+```python
+from pathlib import Path
+from nav_sim import Simulator, load_config
+from examples.controllers.potential_field import potential_field_controller
+
+config = load_config(Path("config/default.yaml"))
+config["robot"]["type"] = "differential"
+
+sim = Simulator(
+    config=config,
+    controller=potential_field_controller
+)
+
+sim.run()
+```
+
+---
+
+## 🧩 Controller Interface
+
+```python
+def controller(robot, world, goal, config, dt, lidar_readings=None) -> dict:
+    ...
+```
+
+### Parameters
+
+- `robot` → robot state (position, heading, limits)
+- `world` → obstacle representation
+- `goal` → current waypoint
+- `config` → YAML configuration
+- `dt` → simulation timestep
+- `lidar_readings` → sensor data
+
+---
+
+## 🧠 Artificial Potential Fields (APF)
+
+This project includes a classical APF implementation based on Khatib’s method:
+
+### Total force
+
+F(q) = F_att(q) + F_rep(q)
+
+### Attractive force
+
+F_att = k_att (q_goal - q)
+
+### Repulsive force
+
+F_rep,i = k_r (1/d_i - 1/d_s) (1/d_i^2) r_hat_i
+
+Where:
+
+- d_i → LiDAR distance corrected by robot radius  
+- d_s → influence distance  
+- r_hat_i → unit vector away from obstacle  
+
+---
+
+## 🛑 Collision Handling
+
+The simulator detects collisions using:
+
+    d <= r_robot + r_obstacle
+
+### Behavior
+
+- The robot stops immediately
+- A visual indicator (💥) is displayed
+
+---
+
+## 🎨 Custom Drawing
+
+You can extend visualization with:
+
+```python
+def custom_draw(sim, screen):
+    ...
+```
+
+Usage:
+
+```python
+sim = Simulator(
+    config=config,
+    controller=controller,
+    draw_callback=custom_draw
+)
+```
+
+---
+
+## 🖱️ User Interaction
+
+- Left click → add waypoint
+- Right click → add obstacle
+- Scroll → zoom
+- Middle button + drag → pan
+
+### GUI
+
+- Center Camera → reset view
+- Reset → restart simulation
+
+---
+
+## ✅ Features
+
+- ✅ Multiple robot models
+- ✅ 2D LiDAR sensor
+- ✅ External controllers
+- ✅ Artificial Potential Fields (APF)
+- ✅ Collision detection with geometry
+- ✅ Custom rendering
+- ✅ Real-time interaction
+
+---
+
+## 🚀 Roadmap
+
+- Vortex APF
+- Harmonic (Laplace) fields
+- Local minima avoidance
+- Advanced visualization
+
+---
+
+## 📄 License
+
+Free for educational and research use.

nav_sim2d-0.0.1/README.md

@@ -0,0 +1,224 @@
+# Nav Sim
+
+A lightweight 2D simulation framework for mobile robot navigation using **Pygame**, with support for sensors, external controllers, and interactive visualization.
+
+---
+
+## Supported Robot Models
+
+- `holonomic`  
+  Command:
+  ```python
+  {"vx": ..., "vy": ...}
+  ```
+
+- `differential`  
+  Command:
+  ```python
+  {"v": ..., "omega": ...}
+  ```
+
+- `ackermann`  
+  Command:
+  ```python
+  {"speed": ..., "steer": ...}
+  ```
+
+---
+
+## 🧠 Architecture
+
+The simulator follows a modular design:
+
+- ✅ **Simulator (`nav_sim`)**  
+  Handles physics, rendering, environment, and sensors
+
+- ✅ **External Controllers**  
+  Implement navigation strategies (APF, Vortex, etc.)
+
+> Controllers are not part of the core library. They live in `examples/` or in the user’s project.
+
+---
+
+## 📡 Sensors
+
+### LiDAR (2D)
+
+The simulator provides a configurable 2D LiDAR:
+
+- Angular scan
+- Range-limited
+- Returns:
+
+```python
+(angle, distance, hit_point)
+```
+
+- Passed to controllers as:
+
+```python
+lidar_readings
+```
+
+---
+
+## ⚙️ Installation
+
+```bash
+python -m venv .venv
+
+# Linux / macOS
+source .venv/bin/activate  
+
+# Windows
+.venv\Scripts\activate
+
+pip install -r requirements.txt
+```
+
+---
+
+## ▶️ Run Examples
+
+```bash
+python examples/run_holonomic.py
+python examples/run_differential.py
+python examples/run_ackermann.py
+```
+
+---
+
+## 🧪 Basic Usage
+
+```python
+from pathlib import Path
+from nav_sim import Simulator, load_config
+from examples.controllers.potential_field import potential_field_controller
+
+config = load_config(Path("config/default.yaml"))
+config["robot"]["type"] = "differential"
+
+sim = Simulator(
+    config=config,
+    controller=potential_field_controller
+)
+
+sim.run()
+```
+
+---
+
+## 🧩 Controller Interface
+
+```python
+def controller(robot, world, goal, config, dt, lidar_readings=None) -> dict:
+    ...
+```
+
+### Parameters
+
+- `robot` → robot state (position, heading, limits)
+- `world` → obstacle representation
+- `goal` → current waypoint
+- `config` → YAML configuration
+- `dt` → simulation timestep
+- `lidar_readings` → sensor data
+
+---
+
+## 🧠 Artificial Potential Fields (APF)
+
+This project includes a classical APF implementation based on Khatib’s method:
+
+### Total force
+
+F(q) = F_att(q) + F_rep(q)
+
+### Attractive force
+
+F_att = k_att (q_goal - q)
+
+### Repulsive force
+
+F_rep,i = k_r (1/d_i - 1/d_s) (1/d_i^2) r_hat_i
+
+Where:
+
+- d_i → LiDAR distance corrected by robot radius  
+- d_s → influence distance  
+- r_hat_i → unit vector away from obstacle  
+
+---
+
+## 🛑 Collision Handling
+
+The simulator detects collisions using:
+
+    d <= r_robot + r_obstacle
+
+### Behavior
+
+- The robot stops immediately
+- A visual indicator (💥) is displayed
+
+---
+
+## 🎨 Custom Drawing
+
+You can extend visualization with:
+
+```python
+def custom_draw(sim, screen):
+    ...
+```
+
+Usage:
+
+```python
+sim = Simulator(
+    config=config,
+    controller=controller,
+    draw_callback=custom_draw
+)
+```
+
+---
+
+## 🖱️ User Interaction
+
+- Left click → add waypoint
+- Right click → add obstacle
+- Scroll → zoom
+- Middle button + drag → pan
+
+### GUI
+
+- Center Camera → reset view
+- Reset → restart simulation
+
+---
+
+## ✅ Features
+
+- ✅ Multiple robot models
+- ✅ 2D LiDAR sensor
+- ✅ External controllers
+- ✅ Artificial Potential Fields (APF)
+- ✅ Collision detection with geometry
+- ✅ Custom rendering
+- ✅ Real-time interaction
+
+---
+
+## 🚀 Roadmap
+
+- Vortex APF
+- Harmonic (Laplace) fields
+- Local minima avoidance
+- Advanced visualization
+
+---
+
+## 📄 License
+
+Free for educational and research use.

nav_sim2d-0.0.1/nav_sim2d/__init__.py

@@ -0,0 +1,4 @@
+from .config import load_config
+from .simulator import Simulator
+
+__all__ = ["Simulator", "load_config"]

nav_sim2d-0.0.1/nav_sim2d/camera.py

@@ -0,0 +1,72 @@
+from __future__ import annotations
+from dataclasses import dataclass
+import numpy as np
+
+
+@dataclass
+class Camera2D:
+    world_width: float
+    world_height: float
+    viewport_width: int
+    viewport_height: int
+    initial_zoom: float = 1.0
+    min_zoom: float = 0.25
+    max_zoom: float = 8.0
+    zoom_step: float = 1.1
+    margin_x: float = 1.0
+    margin_y: float = 1.0
+
+    def __post_init__(self):
+        self.zoom = self.initial_zoom
+        self.offset = np.array([-self.margin_x, -self.margin_y], dtype=float)
+        self.compute_scale()
+
+    def compute_scale(self):
+        scale_x = self.viewport_width / self.world_width
+        scale_y = self.viewport_height / self.world_height
+        self.base_scale = min(scale_x, scale_y)
+        self.scale = self.base_scale * self.zoom
+
+    def resize(self, viewport_width: int, viewport_height: int):
+        self.viewport_width = max(1, int(viewport_width))
+        self.viewport_height = max(1, int(viewport_height))
+        self.compute_scale()
+
+    def reset(self):
+        self.zoom = self.initial_zoom
+        self.offset = np.array([-self.margin_x, -self.margin_y], dtype=float)
+        self.compute_scale()
+
+    def center_on_origin_with_margin(self):
+        self.offset = np.array([-self.margin_x, -self.margin_y], dtype=float)
+
+    def zoom_at(self, mouse_pos, wheel_y: int):
+        old_world = self.screen_to_world(*mouse_pos)
+        factor = self.zoom_step if wheel_y > 0 else 1.0 / self.zoom_step
+        self.zoom = float(np.clip(self.zoom * factor, self.min_zoom, self.max_zoom))
+        self.compute_scale()
+        new_world = self.screen_to_world(*mouse_pos)
+        self.offset += old_world - new_world
+
+    def pan_pixels(self, dx: float, dy: float):
+        self.offset[0] -= dx / self.scale
+        self.offset[1] += dy / self.scale
+
+    def _display_offsets(self):
+        ox = (self.viewport_width - self.world_width * self.scale) / 2
+        oy = (self.viewport_height - self.world_height * self.scale) / 2
+        return ox, oy
+
+    def world_to_screen(self, px: float, py: float):
+        ox, oy = self._display_offsets()
+        px_cam = px - self.offset[0]
+        py_cam = py - self.offset[1]
+        sx = int(px_cam * self.scale + ox)
+        sy = int(self.viewport_height - (py_cam * self.scale + oy))
+        return sx, sy
+
+    def screen_to_world(self, sx: float, sy: float):
+        ox, oy = self._display_offsets()
+        px = (sx - ox) / self.scale + self.offset[0]
+        py = ((self.viewport_height - sy) - oy) / self.scale + self.offset[1]
+        return np.array([px, py], dtype=float)

nav_sim2d-0.0.1/nav_sim2d/config.py

@@ -0,0 +1,10 @@
+from __future__ import annotations
+from pathlib import Path
+from typing import Any
+import yaml
+
+
+def load_config(path: str | Path) -> dict[str, Any]:
+    path = Path(path)
+    with path.open("r", encoding="utf-8") as f:
+        return yaml.safe_load(f)

nav_sim2d-0.0.1/nav_sim2d/geometry.py

@@ -0,0 +1,14 @@
+from __future__ import annotations
+import numpy as np
+
+
+def wrap_to_pi(angle: float) -> float:
+    return float((angle + np.pi) % (2 * np.pi) - np.pi)
+
+
+def clamp(value: float, min_value: float, max_value: float) -> float:
+    return max(min_value, min(value, max_value))
+
+
+def distance_between(a, b) -> float:
+    return float(np.linalg.norm(np.asarray(a, dtype=float) - np.asarray(b, dtype=float)))

nav_sim2d-0.0.1/nav_sim2d/lidar.py

@@ -0,0 +1,54 @@
+from __future__ import annotations
+import math
+import numpy as np
+import pygame
+
+
+class Lidar:
+    def __init__(self, config: dict):
+        c = config["lidar"]
+        self.enabled = bool(c.get("enabled", True))
+        self.num_rays = int(c.get("num_rays", 41))
+        self.range = float(c.get("range", 4.0))
+        self.fov = math.radians(float(c.get("fov_deg", 180)))
+        self.colors = config["colors"]
+
+    def ray_circle_intersection(self, ray_origin, ray_dir, obs):
+        circle_radius = obs[2]
+        circle_center = np.array(obs[:2])
+        oc = ray_origin - circle_center
+        a = np.dot(ray_dir, ray_dir)
+        b = 2.0 * np.dot(oc, ray_dir)
+        c = np.dot(oc, oc) - circle_radius ** 2
+        disc = b*b - 4*a*c
+        if disc < 0:
+            return None
+        sq = math.sqrt(disc)
+        valid = [t for t in [(-b - sq)/(2*a), (-b + sq)/(2*a)] if 0 <= t <= self.range]
+        return min(valid) if valid else None
+
+    def cast_rays(self, robot, world):
+        readings = []
+        angles = np.linspace(robot.theta - self.fov/2, robot.theta + self.fov/2, self.num_rays)
+        for angle in angles:
+            ray_dir = np.array([math.cos(angle), math.sin(angle)])
+            min_d = self.range
+            hit = robot.position + ray_dir * self.range
+            for obs in world.obstacles:
+                d = self.ray_circle_intersection(robot.position, ray_dir, obs)
+                if d is not None and d < min_d:
+                    min_d = d
+                    hit = robot.position + ray_dir * d
+            readings.append((angle, min_d, hit))
+        return readings
+
+    def draw(self, screen, camera, robot, world):
+        if not self.enabled:
+            return
+        origin = camera.world_to_screen(robot.x, robot.y)
+        color = tuple(self.colors["lidar"])
+        far = (80, 120, 120)
+        for _, dist, hit in self.cast_rays(robot, world):
+            pygame.draw.line(screen, color if dist < self.range else far, origin, camera.world_to_screen(*hit), 1)
+            if dist < self.range:
+                pygame.draw.circle(screen, color, camera.world_to_screen(*hit), 3)

nav_sim2d-0.0.1/nav_sim2d/robots/__init__.py

@@ -0,0 +1,16 @@
+from .holonomic import HolonomicRobot
+from .differential import DifferentialRobot
+from .ackermann import AckermannRobot
+
+
+def create_robot(config: dict):
+    robot_type = config["robot"].get("type", "ackermann").lower()
+    if robot_type in ["holonomic", "omni", "omnidirectional"]:
+        return HolonomicRobot(config)
+    if robot_type in ["differential", "diff", "unicycle", "nonholonomic"]:
+        return DifferentialRobot(config)
+    if robot_type in ["ackermann", "car", "racecar"]:
+        return AckermannRobot(config)
+    raise ValueError(f"Tipo de robô desconhecido: {robot_type}")
+
+__all__ = ["create_robot", "HolonomicRobot", "DifferentialRobot", "AckermannRobot"]