Realistic speed and route variations

Author: razvancruceanu

.gitignore

@@ -92,3 +92,5 @@ dmypy.json
 # JS dependencies
 mesa/visualization/templates/external/
 mesa/visualization/templates/js/external/
+
+ships_hybrid_algorithm/ship-venv/

ships_hybrid_algorithm/agents/ship.py

@@ -14,7 +14,10 @@ def __init__(self, model, id, start_port, all_ports, dwa_config):
         self.destination_port = self.assign_destination(all_ports, start_port)
         self.global_path = self.calculate_global_path(start_port.pos, self.destination_port.pos)
         self.dwa_config = dwa_config
-
+        
+        self.heading_deviation = 0.0
+        self.heading_drift_duration = 0
+        
         # Assign a random max speed within the speed range
         dwa_config["max_speed"] = self.random.uniform(self.model.max_speed_range[0], self.model.max_speed_range[1])
         self.original_max_speed = self.dwa_config["max_speed"]
@@ -43,8 +46,40 @@ def step(self):
         if self.global_path and len(self.global_path) > 1:
             local_goal = self.get_local_goal(self.state, self.global_path, lookahead=self.model.lookahead)
             self.dwa_config["max_speed"] = self.get_speed_limit()
+
+            if self.model.speed_variation["enabled"]:
+                self.dwa_config["max_speed"] = self.get_noisy_speed()
+
+            # if self.model.directional_variation["enabled"]:
+            #     noisy_state = self.get_noisy_state()  
+            # else:
+            #     noisy_state = self.state                         
+
+            if self.model.directional_variation["enabled"]:
+                # Randomly trigger heading deviation
+                if self.heading_drift_duration > 0:
+                    # Continue existing deviation
+                    noisy_theta = self.state[2] + self.heading_deviation
+                    self.heading_drift_duration -= 1
+                    logging.info(f"Continue deviation. Ship {self.unique_id}, Theta = {self.heading_deviation}")
+                else:
+                    # Random chance to start a new deviation
+                    if self.random.random() < self.model.deviation_chance:
+                        self.heading_deviation = self.random.uniform(-self.model.max_heading_deviation, self.model.max_heading_deviation)
+                        self.heading_drift_duration = self.model.deviation_duration
+                        noisy_theta = self.state[2] + self.heading_deviation
+                        logging.info(f"Starting directional deviation. Ship {self.unique_id}, Theta = {self.heading_deviation}")
+                    else:
+                        noisy_theta = self.state[2]
+
+                # Normalize heading
+                noisy_theta = (noisy_theta + math.pi) % (2 * math.pi) - math.pi
+                noisy_state = (self.state[0], self.state[1], noisy_theta, self.state[3], self.state[4])   
+            else:
+                noisy_state = self.state         
+
             control, predicted_trajectory, cost_info = dwa_control(
-                self.state, self.dwa_config, self.model.obstacle_tree, 
+                noisy_state, self.dwa_config, self.model.obstacle_tree, 
                 self.model.buffered_obstacles, local_goal
             )
 
@@ -54,6 +89,42 @@ def step(self):
             else:
                 self.state = motion(self.state, control[0], control[1], self.dwa_config["dt"])
                 self.move_position()
+            
+            # After motion, restore true max speed
+            self.dwa_config["max_speed"] = self.original_max_speed
+
+    def get_noisy_speed(self):
+            # Assign ± speed variation as a fraction of max_speed
+            max_speed_variation = self.model.max_speed_variation
+            variation_amount = self.dwa_config["max_speed"] * max_speed_variation
+            noisy_speed = self.dwa_config["max_speed"] + self.random.uniform(-variation_amount, variation_amount)
+
+            # Clamp to valid range
+            noisy_speed = max(self.dwa_config["min_speed"], min(noisy_speed, self.dwa_config["max_speed"]))
+            return noisy_speed
+    
+    def get_noisy_state(self):
+        # Randomly trigger heading deviation
+        if self.heading_drift_duration > 0:
+            # Continue existing deviation
+            noisy_theta = self.state[2] + self.heading_deviation
+            self.heading_drift_duration -= 1
+            logging.info(f"Continue deviation. Ship {self.unique_id}, Theta = {self.heading_deviation}")
+        else:
+            # Random chance to start a new deviation
+            if self.random.random() < self.model.deviation_chance:
+                self.heading_deviation = self.random.uniform(-self.model.max_heading_deviation, self.model.max_heading_deviation)
+                self.heading_drift_duration = self.model.deviation_duration
+                noisy_theta = self.state[2] + self.heading_deviation
+                logging.info(f"Starting directional deviation. Ship {self.unique_id}, Theta = {self.heading_deviation}")
+            else:
+                noisy_theta = self.state[2]
+
+        # Normalize heading
+        noisy_theta = (noisy_theta + math.pi) % (2 * math.pi) - math.pi
+        noisy_state = (self.state[0], self.state[1], noisy_theta, self.state[3], self.state[4])
+
+        return noisy_state
 
     def should_dock(self, current_speed):
         """Determine if the ship should dock at its destination."""

ships_hybrid_algorithm/config/config.json

@@ -1,9 +1,19 @@
 {
-    "simulation_steps": 10000,
+    "simulation_steps": 3000,
     "width": 100,
     "height": 100,
-    "num_ships": 1000,
+    "num_ships": 35,
     "max_speed_range": [1.0, 1.5],
+    "speed_variation": {
+        "enabled": true,
+        "max_speed_variation": 0.05
+    },
+    "directional_variation": {
+        "enabled": true,
+        "max_deviation_deg": 45.0,
+        "activation_chance": 0.02,
+        "duration_steps": 15
+    },
     "resolution": 1,
     "obstacle_threshold": 2,
     "lookahead": 5,

ships_hybrid_algorithm/main.py

@@ -24,6 +24,8 @@ def run_simulation(config_file="config/config.json"):
         height=config["height"],
         num_ships=config["num_ships"],
         max_speed_range=config["max_speed_range"],
+        speed_variation=config["speed_variation"],
+        directional_variation=config["directional_variation"],
         ports=config["ports"],
         speed_limit_zones=config.get("speed_limit_zones", []),
         obstacles=config["obstacles"],

ships_hybrid_algorithm/model.py

@@ -5,23 +5,33 @@
 from shapely.geometry import Polygon
 import random
 from shapely.strtree import STRtree
+import math
 
 from agents.ship import Ship
 from agents.port import Port
 from agents.obstacle import Obstacle
 from a_star import create_occupancy_grid
 
 class ShipModel(Model):
-    def __init__(self, width, height, num_ships, max_speed_range, ports, speed_limit_zones, obstacles, dwa_config, resolution=1, obstacle_threshold=0, lookahead=3.0):
+    def __init__(self, width, height, num_ships, max_speed_range, speed_variation, directional_variation, ports, speed_limit_zones, obstacles, dwa_config, resolution=1, obstacle_threshold=0, lookahead=3.0):
         super().__init__()
         self.width = width
         self.height = height
         self.max_speed_range = max_speed_range
+        self.speed_variation = speed_variation
+        self.directional_variation = directional_variation
         self.dwa_config = dwa_config
         self.resolution = resolution
         self.obstacle_threshold = obstacle_threshold
         self.lookahead = lookahead
 
+        if speed_variation["enabled"]:
+            self.max_speed_variation = speed_variation["max_speed_variation"]
+        if directional_variation["enabled"]:
+            self.max_heading_deviation = math.radians(directional_variation["max_deviation_deg"])
+            self.deviation_duration = directional_variation["duration_steps"]
+            self.deviation_chance = directional_variation["activation_chance"]
+
         self.space = ContinuousSpace(self.width, self.height, torus=False)
         self.schedule = RandomActivation(self)