agent id todo

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/BaseClasses.py

@@ -4,6 +4,7 @@
     Grid,
     Position,
 )
+from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId
 from PathPlanning.TimeBasedPathPlanning.Node import NodePath
 import random
 import numpy.random as numpy_random
@@ -26,8 +27,7 @@ def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: b
 @dataclass
 class StartAndGoal:
     # Index of this agent
-    # TODO: better name and use AgentId type
-    index: int
+    agent_id: AgentId
     # Start position of the robot
     start: Position
     # Goal position of the robot

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -37,8 +37,8 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
 
         # Generate initial solution (no constraints)
         for start_and_goal in start_and_goals:
-            path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.index, verbose)
-            initial_solution[AgentId(start_and_goal.index)] = path
+            path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose)
+            initial_solution[AgentId(start_and_goal.agent_id)] = path
 
         if verbose:
             print("Initial solution:")
@@ -62,7 +62,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
                 # This means we found a solution!
                 print(f"Found a path with constraints after {constraint_tree.expanded_node_count()} expansions:")
                 print(f"Final cost: {constraint_tree_node.cost}")
-                return (start_and_goals, [constraint_tree_node.paths[start_and_goal.index] for start_and_goal in start_and_goals])
+                return (start_and_goals, [constraint_tree_node.paths[start_and_goal.agent_id] for start_and_goal in start_and_goals])
 
             if not isinstance(constraint_tree_node.constraint, ForkingConstraint):
                 raise ValueError(f"Expected a ForkingConstraint, but got: {constraint_tree_node.constraint}")
@@ -108,7 +108,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
 
     def get_agents_start_and_goal(start_and_goal_list: list[StartAndGoal], target_index: AgentId) -> StartAndGoal:
         for item in start_and_goal_list:
-            if item.index == target_index:
+            if item.agent_id == target_index:
                 return item
         raise RuntimeError(f"Could not find agent with index {target_index} in {start_and_goal_list}")
 
@@ -142,11 +142,11 @@ def plan_for_agent(constrained_agent: ConstraintTreeNode,
         grid.apply_constraint_points(all_constraints)
 
         # Just plan for agent with new constraint
-        start_and_goal = ConflictBasedSearch.get_agents_start_and_goal(start_and_goals, constrained_agent.agent)
+        start_and_goal: StartAndGoal = ConflictBasedSearch.get_agents_start_and_goal(start_and_goals, constrained_agent.agent)
         try:
             if verbose:
                 print("\tplanning for: {}", start_and_goal)
-            new_path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.index, verbose)
+            new_path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose)
             return new_path
         except Exception as e:
             print(f"Error: {e}")

PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

@@ -3,9 +3,9 @@
 is also infrastructure to generate dynamic obstacles that move around the grid. The obstacles' paths
 are stored in the reservation matrix on creation.
 """
-import numpy as np
-import matplotlib.pyplot as plt
 from enum import Enum
+import matplotlib.pyplot as plt
+import numpy as np
 from dataclasses import dataclass
 from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position
 from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AppliedConstraint

PathPlanning/TimeBasedPathPlanning/Plotting.py

@@ -71,7 +71,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N
     start_and_goal_plots = []
     for i, path in enumerate(paths):
         marker_idx = i % len(markers)
-        agent_id = start_and_goals[i].index
+        agent_id = start_and_goals[i].agent_id
         start = start_and_goals[i].start
         goal = start_and_goals[i].goal
 
@@ -88,7 +88,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N
     # Create plots for each agent's path
     path_plots = []
     for i, path in enumerate(paths):
-        agent_id = start_and_goals[i].index
+        agent_id = start_and_goals[i].agent_id
         path_plot, = ax.plot([], [], "o", c=colors[i], ms=10, 
                             label=f"Agent {agent_id} Path")
         path_plots.append(path_plot)

PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py

@@ -33,7 +33,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
 
         # Reserve initial positions
         for start_and_goal in start_and_goals:
-            grid.reserve_position(start_and_goal.start, start_and_goal.index, Interval(0, 10))
+            grid.reserve_position(start_and_goal.start, start_and_goal.agent_id, Interval(0, 10))
 
         # Plan in descending order of distance from start to goal
         start_and_goals = sorted(start_and_goals,
@@ -45,14 +45,14 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
             if verbose:
                 print(f"\nPlanning for agent:  {start_and_goal}" )
 
-            grid.clear_initial_reservation(start_and_goal.start, start_and_goal.index)
+            grid.clear_initial_reservation(start_and_goal.start, start_and_goal.agent_id)
             path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, verbose)
 
             if path is None:
                 print(f"Failed to find path for {start_and_goal}")
                 return []
 
-            agent_index = start_and_goal.index
+            agent_index = start_and_goal.agent_id
             grid.reserve_path(path, agent_index)
             paths.append(path)