Merge pull request #6 from movingpandas/ships

Ship movement model: simulates ships moving from origin to destination port, avoiding obstacles, using path planning.

Author: anitagraser

mesa/examples/basic/ship_navigation_planning/CFG.py

@@ -0,0 +1,28 @@
+import numpy as np
+
+
+class Config:
+    MAP_WIDTH = 150
+    MAP_HEIGHT = 150
+    RESOLUTION = 1.0
+    OBSTACLE_THRESHOLD = 3
+
+    START_POINT = (10, 10)
+    GOAL_POINT = (140, 140)
+
+    LOOKAHEAD = 3
+
+    MODEL_CONFIG = {
+        "max_speed": 3.0,
+        "min_speed": 0.1,
+        "max_yaw_rate": np.deg2rad(15.0),
+        "max_acceleration": 0.5,
+        "max_delta_yaw_rate": np.deg2rad(15.0),
+        "dt": 0.1,
+        "predict_time": 1,
+        "robot_radius": 1,
+        "to_goal_cost_gain": 1,
+        "speed_cost_gain": 1,
+        "obstacle_cost_gain": 1.0,
+        "turn_cost_gain": 1.0,
+    }

mesa/examples/basic/ship_navigation_planning/requirements.txt

@@ -0,0 +1,3 @@
+matplotlib==3.10.0
+numpy==2.2.2
+Shapely==2.0.7

mesa/examples/basic/ship_navigation_planning/run.py

@@ -0,0 +1,7 @@
+from util.simulation_v2_dynamic_obstacle_on_click import Simulation
+
+
+if __name__ == '__main__':
+    sim = Simulation()
+
+    sim.run()

mesa/examples/basic/ship_navigation_planning/util/compute_path.py

@@ -0,0 +1,69 @@
+import math
+from heapq import heappush, heappop
+import numpy as np
+from shapely import vectorized
+
+
+def create_occupancy_grid(map_width, map_height, resolution, obstacles, threshold=0):
+    grid_width = int(map_width / resolution)
+    grid_height = int(map_height / resolution)
+    grid = np.zeros((grid_width, grid_height), dtype=int)
+
+    xs = (np.arange(grid_width) + 0.5) * resolution
+    ys = (np.arange(grid_height) + 0.5) * resolution
+    xx, yy = np.meshgrid(xs, ys, indexing='ij')
+
+    for obs in obstacles:
+        if obs["type"] == "circle":
+            shape = obs["circle"]
+        else:
+            shape = obs["polygon"]
+        poly = shape.buffer(threshold)
+        mask = vectorized.contains(poly, xx, yy)
+        grid[mask] = 1
+
+    return grid
+
+
+def heuristic(a, b):
+    return math.hypot(a[0] - b[0], a[1] - b[1])
+
+
+def reconstruct_path(came_from, current):
+    path = [current]
+    while current in came_from:
+        current = came_from[current]
+        path.append(current)
+    return path[::-1]
+
+
+def astar(grid, start, goal):
+    print(f'Grid shape = {grid.shape}')
+    width, height = grid.shape
+    open_set = []
+    heappush(open_set, (0, start))
+    came_from = {}
+    g_score = {start: 0}
+    f_score = {start: heuristic(start, goal)}
+
+    while open_set:
+        current_f, current = heappop(open_set)
+        if current == goal:
+            return reconstruct_path(came_from, current)
+        for dx in [-1, 0, 1]:
+            for dy in [-1, 0, 1]:
+                if dx == 0 and dy == 0:
+                    continue
+                neighbor = (current[0] + dx, current[1] + dy)
+                if 0 <= neighbor[0] < width and 0 <= neighbor[1] < height:
+                    if grid[neighbor[0], neighbor[1]] == 1:
+                        continue
+                    tentative_g = g_score[current] + math.hypot(dx, dy)
+                    if neighbor not in g_score or tentative_g < g_score[neighbor]:
+                        came_from[neighbor] = current
+                        g_score[neighbor] = tentative_g
+                        f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
+                        heappush(open_set, (f_score[neighbor], neighbor))
+    return None
+
+

mesa/examples/basic/ship_navigation_planning/util/dwa.py

@@ -0,0 +1,170 @@
+import math
+import numpy as np
+from shapely.geometry import Point, Polygon
+
+
+def motion(state, v, w, dt):
+    x, y, theta, _, _ = state
+    # print(f'Current state x = {x} | y = {y} | theta = {theta} | v = {v} | w = {w} | dt = {dt}')
+
+    x += v * math.cos(theta) * dt
+    y += v * math.sin(theta) * dt
+    theta += w * dt
+
+    # print(f'NEW state x = {x} | y = {y} | theta = {theta} | v = {v} | w = {w} | dt = {dt}')
+    # print(f'-' * 50)
+
+    return x, y, theta, v, w
+
+
+def calc_trajectory(state, v, w, config):
+    trajectory = []
+    t = 0.0
+    new_state = state
+    while t <= config["predict_time"]:
+        new_state = motion(new_state, v, w, config["dt"])
+        trajectory.append(new_state)
+        t += config["dt"]
+
+    return trajectory
+
+
+def calc_to_goal_cost(trajectory, goal, cost_gain):
+    x, y, _, _, _ = trajectory[-1]
+    distance = math.hypot(goal[0] - x, goal[1] - y)
+    return cost_gain * distance
+
+
+def calc_turn_cost(state, local_goal, cost_gain):
+    x, y, theta, _, _ = state
+    dx = local_goal[0] - x
+    dy = local_goal[1] - y
+    desired_theta = math.atan2(dy, dx)
+    turn_angle = abs(desired_theta - theta)
+    turn_angle = min(turn_angle, 2 * math.pi - turn_angle)
+    return cost_gain * turn_angle
+
+
+def calc_speed_cost(v, config):
+    return config["speed_cost_gain"] * (config["max_speed"] - v)
+
+
+def calc_obstacle_cost_v2(trajectory, obstacle_tree, buffered_obstacles, config):
+    min_distance = float("inf")
+    for state in trajectory:
+        x, y, _, _, _ = state
+        point = Point(x, y)
+
+        nearby_indices = obstacle_tree.query(point)
+        for idx in nearby_indices:
+            buffered_obs = buffered_obstacles[idx]
+            if buffered_obs.contains(point):
+                return float("inf")
+            distance = buffered_obs.distance(point)
+            if distance < min_distance:
+                min_distance = distance
+    return config["obstacle_cost_gain"] / (min_distance + 1e-6)
+
+
+def calc_obstacle_cost(trajectory, buffered_obstacles, config):
+    min_distance = float("inf")
+    for state in trajectory:
+        x, y, _, _, _ = state
+        point = Point(x, y)
+        for buffered_obs in buffered_obstacles:
+            if buffered_obs.contains(point):
+                return float("inf")
+            distance = buffered_obs.distance(point)
+            if distance < min_distance:
+                min_distance = distance
+    return config["obstacle_cost_gain"] / (min_distance + 1e-6)
+
+
+def calc_combined_turn_speed_cost(state, local_goal, v, cost_gain):
+    x, y, theta, _, _ = state
+    dx = local_goal[0] - x
+    dy = local_goal[1] - y
+    desired_theta = math.atan2(dy, dx)
+    turn_angle = abs(desired_theta - theta)
+    turn_angle = min(turn_angle, 2 * math.pi - turn_angle)
+    # If the robot is moving fast and needs to turn a lot, the cost increases.
+    return cost_gain * turn_angle * v
+
+
+def dwa_control(state, config, buffered_obstacles, local_goal):
+    best_cost = float("inf")
+    best_control = (0.0, 0.0)
+    best_trajectory = []
+    best_cost_info = {}
+
+    v_min = max(config["min_speed"], state[3] - config["max_acceleration"] * config["dt"])
+    v_max = min(config["max_speed"], state[3] + config["max_acceleration"] * config["dt"])
+    w_min = -config["max_yaw_rate"]
+    w_max = config["max_yaw_rate"]
+
+    v_samples = np.linspace(v_min, v_max, num=5)
+    w_samples = np.linspace(w_min, w_max, num=5)
+
+    for v in v_samples:
+        for w in w_samples:
+            trajectory = calc_trajectory(state, v, w, config)
+            to_goal_cost = calc_to_goal_cost(trajectory, local_goal, config["to_goal_cost_gain"])
+            speed_cost = calc_speed_cost(v, config)
+            obstacle_cost = calc_obstacle_cost(trajectory, buffered_obstacles, config)
+            turn_cost = calc_turn_cost(state, local_goal, config["turn_cost_gain"])
+
+            total_cost = to_goal_cost + speed_cost + obstacle_cost + turn_cost
+            if total_cost < best_cost:
+                best_cost = total_cost
+                best_control = (v, w)
+                best_trajectory = trajectory
+                best_cost_info = {
+                    "total_cost": total_cost,
+                    "to_goal_cost": to_goal_cost,
+                    "speed_cost": speed_cost,
+                    "obstacle_cost": obstacle_cost,
+                    "turn_cost": turn_cost
+                }
+    return best_control, best_trajectory, best_cost_info
+
+
+def dwa_control_v2(state, config, obstacle_tree, buffered_obstacles, local_goal):
+    best_cost = float("inf")
+    best_control = (0.0, 0.0)
+    best_trajectory = []
+    best_cost_info = {}
+
+    # v_min = max(config["min_speed"], state[3] - config["max_acceleration"] * config["dt"])
+    # v_max = min(config["max_speed"], state[3] + config["max_acceleration"] * config["dt"])
+
+    v_lower = config["min_speed"]
+    v_upper = min(config["max_speed"], state[3] + config["max_acceleration"] * config["dt"])
+    v_samples = np.linspace(v_lower, v_upper, num=5)
+
+    w_min = -config["max_yaw_rate"]
+    w_max = config["max_yaw_rate"]
+
+    # v_samples = np.linspace(v_min, v_max, num=5)
+    w_samples = np.linspace(w_min, w_max, num=5)
+
+    for v in v_samples:
+        for w in w_samples:
+            trajectory = calc_trajectory(state, v, w, config)
+            to_goal_cost = calc_to_goal_cost(trajectory, local_goal, config["to_goal_cost_gain"])
+            speed_cost = calc_speed_cost(v, config)
+            obstacle_cost = calc_obstacle_cost_v2(trajectory, obstacle_tree, buffered_obstacles, config)
+            # turn_cost = calc_turn_cost(state, local_goal, config["turn_cost_gain"])
+            turn_cost = calc_combined_turn_speed_cost(state, local_goal, v, config["turn_cost_gain"])
+            total_cost = to_goal_cost + speed_cost + obstacle_cost + turn_cost
+            if total_cost < best_cost:
+                best_cost = total_cost
+                best_control = (v, w)
+                best_trajectory = trajectory
+                best_cost_info = {
+                    "total_cost": total_cost,
+                    "to_goal_cost": to_goal_cost,
+                    "speed_cost": speed_cost,
+                    "obstacle_cost": obstacle_cost,
+                    "turn_cost": turn_cost
+                }
+    return best_control, best_trajectory, best_cost_info