def test_give_way_no_oncoming(self):
config = ManeuverConfig({
'termination_point': (31.7, -19.8),
'junction_road_id': 6,
'junction_lane_id': -1
})
agent_id = 0
position = np.array((10.6, -4.1))
heading = -0.6
speed = 10
velocity = speed * np.array([np.cos(heading), np.sin(heading)])
acceleration = np.array([0, 0])
frame = {
0:
AgentState(0,
position=position,
velocity=velocity,
acceleration=acceleration,
heading=heading)
}
maneuver = GiveWay(config, agent_id, frame, scenario)
plot_map(scenario, markings=True, midline=False)
plt.plot(*position, marker="o", color="b")
plt.plot(*list(zip(*maneuver.trajectory.path)), color="b")
plt.show()
# there should be no stops
assert np.all(maneuver.trajectory.velocity > 1)
def test_get_goals_heckstrasse(self):
frame = {
0:
AgentState(time=0,
position=np.array([6.0, 0.7]),
velocity=1.5,
acceleration=0.0,
heading=-0.6),
}
agent_id = 0
scenario_map = scenarios["heckstrasse"]
agent = MCTSAgent(agent_id,
frame[agent_id],
1,
scenario_map,
PointGoal(np.array([186.23, -2.03]), 2.0),
view_radius=100)
agent.get_goals(Observation(frame, scenario_map))
plot_map(scenario_map, markings=True)
plt.plot(*frame[agent_id].position, marker="o")
view_circle = Point(*frame[agent_id].position).buffer(
agent.view_radius).boundary
plt.plot(*list(zip(*view_circle.coords)))
for goal in agent._goals:
plt.plot(*goal.center, marker="x", color="b")
plt.show()
def test_get_goals_town01(self):
basic_meta = AgentMetadata(**AgentMetadata.CAR_DEFAULT)
frame = {
0:
AgentState(0, np.array([88.38, -38.62]), np.array([0.0, -5.0]),
np.array([0.0, 0.0]), -np.pi / 2),
1:
AgentState(0, np.array([334.58, 1.22]), np.array([1.0, -5.0]),
np.array([0.0, 0.0]), -np.pi / 4)
}
agent_id = 1
scenario_map = scenarios["town1"]
agent = MCTSAgent(agent_id,
frame[agent_id],
1,
scenario_map,
PointGoal(np.array([186.23, -2.03]), 2.0),
view_radius=5)
agent.get_goals(Observation(frame, scenario_map))
plot_map(scenario_map, markings=True)
plt.plot(*frame[agent_id].position, marker="o")
view_circle = Point(*frame[agent_id].position).buffer(
agent.view_radius).boundary
plt.plot(*list(zip(*view_circle.coords)))
for goal in agent._goals:
plt.plot(*goal.center, marker="x", color="b")
plt.show()
def test_search_bendplatz(self):
scenario_map = SCENARIOS["bendplatz"]
frame = {
0: AgentState(time=0,
position=np.array([30.76, -10.31]),
velocity=10,
acceleration=0.0,
heading=-np.pi / 4),
1: AgentState(time=0,
position=np.array([28.71, -2.10]),
velocity=4,
acceleration=0.0,
heading=-np.pi / 4),
2: AgentState(time=0,
position=np.array([74.58, -49.29]),
velocity=10,
acceleration=0.0,
heading=3 * np.pi / 4),
3: AgentState(time=0,
position=np.array([78.55, -4.67]),
velocity=4,
acceleration=0.0,
heading=-3 * np.pi / 4),
4: AgentState(time=0,
position=np.array([38.06, -18.65]),
velocity=3,
acceleration=0.0,
heading=-3 * np.pi / 4),
}
goals = {
0: PointGoal(np.array([37.78, -50.40]), 2.0),
1: PointGoal(np.array([74.14, -57.66]), 2.0),
2: PointGoal(np.array([31.29, -56.72]), 2.0),
3: PointGoal(np.array([76.54, -60.40]), 2.0)
}
colors = "rgbyk"
plot_map(scenario_map, markings=True, midline=False)
for agent_id, agent in frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o", color=colors[agent_id % len(colors)])
plt.text(*agent.position, agent_id)
astar = AStar()
for agent_id in goals:
goal = goals[agent_id]
trajectories, actions = astar.search(agent_id, frame, goal, scenario_map)
for traj in trajectories:
plt.plot(*list(zip(*traj.path)), color=colors[agent_id % len(colors)])
plt.show()
def test_lane_change_heckstrasse(self):
scenario_map = SCENARIOS["heckstrasse"]
frame = {
0:
AgentState(time=0,
position=np.array([26.9, -19.3]),
velocity=5.5,
acceleration=0.0,
heading=-np.pi / 8),
1:
AgentState(time=0,
position=np.array([6.0, 0.7]),
velocity=1.5,
acceleration=0.0,
heading=-np.pi / 8),
2:
AgentState(time=0,
position=np.array([22.89, -12.9]),
velocity=2,
acceleration=0.0,
heading=-np.pi / 8),
3:
AgentState(time=0,
position=np.array([29.9, -21.9]),
velocity=1.5,
acceleration=0.0,
heading=-np.pi / 8),
}
plot_map(scenario_map, markings=True)
for agent_id, agent in frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o")
foo = ChangeLaneLeft.applicable(frame[0], scenario_map)
lane_change = ChangeLaneLeft(0, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
lane_change = ChangeLaneRight(1, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
lane_change = ChangeLaneRight(2, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
# lane_change = ChangeLaneLeft(3, frame, scenario_map, True)
# trajectory = lane_change.get_trajectory().path
# plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
plt.show()
def test_lane_change_bendplatz(self):
scenario_map = SCENARIOS["bendplatz"]
frame = {
0:
AgentState(time=0,
position=np.array([29.0, -2.3]),
velocity=5.5,
acceleration=0.0,
heading=-np.pi / 4),
1:
AgentState(time=0,
position=np.array([31.1, -11.0]),
velocity=5.5,
acceleration=0.0,
heading=-np.pi / 4),
2:
AgentState(time=0,
position=np.array([41.6, -21.4]),
velocity=5.5,
acceleration=0.0,
heading=-np.pi / 8),
3:
AgentState(time=0,
position=np.array([68.0, -46.6]),
velocity=4.5,
acceleration=0.0,
heading=3 * np.pi / 4),
}
plot_map(scenario_map, markings=True)
for agent_id, agent in frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o")
lane_change = ChangeLaneRight(0, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
lane_change = ChangeLaneLeft(1, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
lane_change = Continue(2, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
lane_change = ChangeLaneRight(3, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
plt.show()
def test_search_round(self):
scenario_map = SCENARIOS["round"]
frame = {
0: AgentState(time=0,
position=np.array([41.1, -41.0]),
velocity=1.5,
acceleration=0.0,
heading=-0.3),
1: AgentState(time=0,
position=np.array([58.31, -50.6]),
velocity=1.5,
acceleration=0.0,
heading=-np.pi / 3),
2: AgentState(time=0,
position=np.array([79.2, -28.65]),
velocity=1.5,
acceleration=0.0,
heading=-17 * np.pi / 18),
3: AgentState(time=0,
position=np.array([147.1, -58.7]),
velocity=1.5,
acceleration=0.0,
heading=17 * np.pi / 18),
}
goals = {
0: PointGoal(np.array([104.3, -3.8]), 2),
1: PointGoal(np.array([13.5, -22.7]), 2),
2: PointGoal(np.array([136.0, -66.5]), 2),
3: PointGoal(np.array([18.54, -28.1]), 2)
}
colors = {0: "r", 1: "g", 2: "b", 3: "y"}
plot_map(scenario_map, markings=True, midline=False)
for agent_id, agent in frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o", color=colors[agent_id])
plt.text(*agent.position, agent_id)
astar = AStar()
for agent_id in goals:
goal = goals[agent_id]
trajectories, actions = astar.search(agent_id, frame, goal, scenario_map)
plt.plot(*goal.center, marker="x")
for traj in trajectories:
plt.plot(*list(zip(*traj.path)), color=colors[agent_id])
plt.show()
def test_search_frankenberg(self):
scenario_map = SCENARIOS["frankenberg"]
frame = {
0: AgentState(time=0,
position=np.array([32.0, -36.54]),
velocity=8,
acceleration=0.0,
heading=np.pi / 12),
1: AgentState(time=0,
position=np.array([64.1, -70.13]),
velocity=10,
acceleration=0.0,
heading=10 * np.pi / 18),
2: AgentState(time=0,
position=np.array([81.3, -23.5]),
velocity=10,
acceleration=0.0,
heading=-17 * np.pi / 18),
3: AgentState(time=0,
position=np.array([21.6, 1.23]),
velocity=4,
acceleration=0.0,
heading=-np.pi / 3),
}
goals = {
0: PointGoal(np.array([26.6, 1.91]), 2),
1: PointGoal(np.array([101.6, -24.08]), 2),
2: PointGoal(np.array([5.5, -39.27]), 2),
3: PointGoal(np.array([52.3, -52.9]), 2)
}
colors = {0: "r", 1: "g", 2: "b", 3: "y"}
plot_map(scenario_map, markings=True, midline=True)
for agent_id, agent in frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o", color=colors[agent_id])
plt.text(*agent.position, agent_id)
astar = AStar()
for agent_id in goals:
goal = goals[agent_id]
trajectories, actions = astar.search(agent_id, frame, goal, scenario_map)
plt.plot(*goal.center, marker="x")
for traj in trajectories:
plt.plot(*list(zip(*traj.path)), color=colors[agent_id])
plt.show()
def test_switch_lane_left(self):
agent_id = 0
position = np.array((10, -6.8))
heading = -0.6
speed = 10
velocity = speed * np.array([np.cos(heading), np.sin(heading)])
acceleration = np.array([0, 0])
frame = {
0:
AgentState(0,
position=position,
velocity=velocity,
acceleration=acceleration,
heading=heading)
}
config = ManeuverConfig({
'type': 'switch-lane',
'termination_point': (31.3, -19.2)
})
maneuver = SwitchLaneLeft(config, agent_id, frame, scenario)
# spacing between points should be roughly 1m
plot_map(scenario, markings=True, midline=False)
plt.plot(*position, marker="o", color="b")
plt.plot(*list(zip(*maneuver.trajectory.path)), color="b")
plt.show()
path_lengths = np.linalg.norm(np.diff(maneuver.trajectory.path,
axis=0),
axis=1)
assert np.all(path_lengths < 1.1 * maneuver.POINT_SPACING)
assert np.all(path_lengths > 0.9 * maneuver.POINT_SPACING)
# start and end should be close to state/config
assert np.allclose(maneuver.trajectory.path[0], (10, -6.8), atol=1)
assert np.allclose(maneuver.trajectory.path[-1], (31.3, -19.2), atol=1)
# each point should be with 90 degrees of overall direction
directions = np.diff(maneuver.trajectory.path, axis=0)
overall_direction = (config.termination_point - position).reshape(
(2, 1))
assert np.all((directions @ overall_direction) > 0)
def test_follow_lane(self):
config = ManeuverConfig({
'type': 'follow-lane',
'initial_lane_id': 2,
'final_lane_id': 2,
'termination_point': (27.1, -19.8)
})
agent_id = 0
position = np.array((8.4, -6.0))
heading = -0.6
speed = 10
velocity = speed * np.array([np.cos(heading), np.sin(heading)])
acceleration = np.array([0, 0])
agent_0_state = AgentState(time=0,
position=position,
velocity=velocity,
acceleration=acceleration,
heading=heading)
frame = {0: agent_0_state}
maneuver = FollowLane(config, agent_id, frame, scenario)
plot_map(scenario, markings=True, midline=False)
plt.plot(*position, marker="o", color="b")
plt.plot(*list(zip(*maneuver.trajectory.path)), color="b")
plt.show()
assert 21 < maneuver.trajectory.length < 26
assert 2.1 < maneuver.trajectory.duration < 2.6
# spacing between points should be roughly 1m
path_lengths = np.linalg.norm(np.diff(maneuver.trajectory.path,
axis=0),
axis=1)
assert np.all(path_lengths < 1.1 * maneuver.POINT_SPACING)
assert np.all(path_lengths > 0.9 * maneuver.POINT_SPACING)
# velocities should be close to 10
assert np.all(np.abs(maneuver.trajectory.velocity - 10) < 0.5)
# start and end should be close to state/config
assert np.allclose(maneuver.trajectory.path[0], (8.4, -6.0), atol=1)
assert np.allclose(maneuver.trajectory.path[-1], (27.1, -19.8), atol=1)
def test_turn_heckstrasse(self):
scenario_map = SCENARIOS["heckstrasse"]
frame = {
0:
AgentState(time=0,
position=np.array([6.0, 0.7]),
velocity=1.5,
acceleration=0.0,
heading=-0.6),
1:
AgentState(time=0,
position=np.array([19.7, -13.5]),
velocity=8.5,
acceleration=0.0,
heading=-0.6),
2:
AgentState(time=0,
position=np.array([73.2, -47.1]),
velocity=11.5,
acceleration=0.0,
heading=np.pi - 0.6),
}
plot_map(scenario_map, markings=True, midline=False)
for agent_id, agent in frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o")
lane_change = Exit(np.array([49.44, -23.1]), 0, frame, scenario_map,
True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="blue")
lane_change = Exit(np.array([62.34, -46.67]), 1, frame, scenario_map,
True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
lane_change = Exit(np.array([63.2, -18.5]), 2, frame, scenario_map,
True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="green")
plt.show()
def test_turn(self):
config = ManeuverConfig({
'termination_point': (61.7, -46.3),
'junction_road_id': 6,
'junction_lane_id': -1
})
agent_id = 0
position = np.array((45.55, -20.1))
heading = -2.8
speed = 10
velocity = speed * np.array([np.cos(heading), np.sin(heading)])
acceleration = np.array([0, 0])
frame = {
0:
AgentState(time=0,
position=position,
velocity=velocity,
acceleration=acceleration,
heading=heading)
}
maneuver = Turn(config, agent_id, frame, scenario)
plot_map(scenario, markings=True, midline=False)
plt.plot(*position, marker="o", color="b")
plt.plot(*list(zip(*maneuver.trajectory.path)), color="b")
plt.show()
# spacing between points should be roughly 1m
path_lengths = np.linalg.norm(np.diff(maneuver.trajectory.path,
axis=0),
axis=1)
assert np.all(path_lengths < 1.1 * maneuver.POINT_SPACING)
assert np.all(path_lengths > 0.9 * maneuver.POINT_SPACING)
# start and end should be close to state/config
assert np.allclose(maneuver.trajectory.path[0], (45.55, -20.1), atol=1)
assert np.allclose(maneuver.trajectory.path[-1], (61.7, -46.3), atol=1)
data_loader = InDDataLoader(f"scenarios/configs/{SCENARIO}.json",
["valid"])
data_loader.load()
goals = np.array(data_loader.scenario.config.goals)
astar = AStar()
for episode in data_loader:
print(f"#agents: {len(episode.agents)}")
Maneuver.MAX_SPEED = episode.metadata.max_speed # Can be set explicitly if the episode provides a speed limit
# Iterate over each time step and keep track of visible agents' observed trajectories
current_agents = {}
for frame in episode.frames:
if frame.time % 15 != 0:
continue
if PLOT: plot_map(scenario_map, markings=True, midline=False)
for agent_id, state in frame.agents.items():
print(f"t={frame.time}; aid={agent_id}")
if PLOT:
plt.plot(*state.position,
marker="o",
color=colors[agent_id % 5])
plt.text(*state.position, agent_id)
for i, goal in enumerate(goals):
if PLOT:
plt.plot(*goal, marker="x")
plt.text(*goal, str(i))
trajectories, _ = astar.search(agent_id, frame.agents,
PointGoal(goal, 1.0),
scenario_map)
scenario_map = Map.parse_from_opendrive(f"scenarios/maps/{SCENARIO}.xodr")
data_loader = InDDataLoader(f"scenarios/configs/{SCENARIO}.json",
["valid"])
data_loader.load()
goals_data = data_loader.scenario.config.goals
goals = extract_goal_data(goals_data)
astar = AStar()
colors = "rgbky" * 5
for episode in data_loader:
Maneuver.MAX_SPEED = episode.metadata.max_speed # Can be set explicitly if the episode provides a speed limit
# Iterate over each time step and keep track of visible agents' observed trajectories
for frame in episode.frames:
PLOT = frame.time > 150
if PLOT: plot_map(scenario_map, markings=True)
for agent_id, agent in frame.agents.items():
c = colors[agent_id]
if PLOT: plt.plot(*agent.position, color=c, marker="o")
for goal in goals:
trajectories, actions = astar.search(
agent_id, frame.agents, goal, scenario_map)
if PLOT: plt.plot(*goal.center, marker="x")
for trajectory in trajectories:
if PLOT:
plt.plot(*list(zip(*trajectory.path)), color=c)
if PLOT: plt.show()
def test_turn_round(self):
scenario_map = SCENARIOS["round"]
frame = {
0:
AgentState(time=0,
position=np.array([41.30, -39.2]),
velocity=1.5,
acceleration=0.0,
heading=-0.3),
1:
AgentState(time=0,
position=np.array([54.21, -50.4]),
velocity=1.5,
acceleration=0.0,
heading=-np.pi / 5),
2:
AgentState(time=0,
position=np.array([64.72, -27.65]),
velocity=1.5,
acceleration=0.0,
heading=-4 * np.pi / 3),
3:
AgentState(time=0,
position=np.array([78.78, -22.10]),
velocity=1.5,
acceleration=0.0,
heading=-np.pi / 2 - np.pi / 6),
4:
AgentState(time=0,
position=np.array([86.13, -25.47]),
velocity=1.5,
acceleration=0.0,
heading=np.pi / 2),
}
plot_map(scenario_map, markings=True, midline=False)
turn = Exit(np.array([53.44, -47.522]), 0, frame, scenario_map, True)
trajectory = turn.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="blue")
turn.final_frame[0].position += 0.15 * np.array([
np.cos(turn.final_frame[0].heading),
np.sin(turn.final_frame[0].heading)
])
lane_change = ChangeLaneLeft(0, turn.final_frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="blue")
continue_next_exit = ContinueNextExit(0, lane_change.final_frame,
scenario_map, True)
trajectory = continue_next_exit.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="blue")
continue_next_exit.final_frame[0].position += 0.15 * np.array([
np.cos(continue_next_exit.final_frame[0].heading),
np.sin(continue_next_exit.final_frame[0].heading)
])
lane_change = ChangeLaneRight(0, continue_next_exit.final_frame,
scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="blue")
for agent_id, agent in lane_change.final_frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o")
turn = Exit(np.array([95.77, -52.74]), 0, lane_change.final_frame,
scenario_map, True)
trajectory = turn.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="blue")
plt.show()
def test_lane_change_test_map(self):
scenario_map = SCENARIOS["test_lane_change"]
frame = {
0:
AgentState(time=0,
position=np.array([89.9, 4.64]),
velocity=11.5,
acceleration=0.0,
heading=np.pi),
1:
AgentState(time=0,
position=np.array([79.7, 1.27]),
velocity=1.5,
acceleration=0.0,
heading=np.pi),
# 2: AgentState(time=0, # This agent will fail for now since the road splits
# position=np.array([71.7, 1.27]),
# velocity=4.5,
# acceleration=0.0,
# heading=np.pi),
3:
AgentState(time=0,
position=np.array([111.0, -1.34]),
velocity=9.5,
acceleration=0.0,
heading=np.pi / 8.5),
4:
AgentState(time=0,
position=np.array([128.7, -0.49]),
velocity=4.5,
acceleration=0.0,
heading=np.pi / 6),
5:
AgentState(time=0,
position=np.array([137.0, 8.5]),
velocity=10.0,
acceleration=0.0,
heading=np.pi / 4),
}
plot_map(scenario_map, markings=True, midline=False)
for agent_id, agent in frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o")
lane_change = ChangeLaneLeft(0, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="b")
lane_change = ChangeLaneRight(1, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="orange")
# lane_change = ChangeLaneRight(2, frame, scenario_map, True)
# trajectory = lane_change.get_trajectory().path
# plt.plot(trajectory[:, 0], trajectory[:, 1], color="green")
lane_change = ChangeLaneRight(3, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="red")
lane_change = ChangeLaneLeft(4, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="purple")
lane_change = ChangeLaneRight(5, frame, scenario_map, True)
trajectory = lane_change.get_trajectory().path
plt.plot(trajectory[:, 0], trajectory[:, 1], color="brown")
plt.show()
# Iterate over all agents and their full trajectories
for agent_id, agent in episode.agents.items():
goal_probabilities = GoalsProbabilities(goals)
start_t = agent.trajectory.states[0].time
end_t = start_t + len(agent.trajectory.states) // 8
initial_frame = episode.frames[start_t].agents
end_frame = episode.frames[end_t].agents
if agent_id not in initial_frame or agent_id not in end_frame:
continue
observed_trajectory = agent.trajectory.slice(0, end_t - start_t)
goal_recognition.update_goals_probabilities(
goal_probabilities,
observed_trajectory=observed_trajectory,
agent_id=agent_id,
frame_ini=initial_frame,
frame=end_frame,
maneuver=None)
if PLOT:
plot_map(scenario_map, markings=True)
plt.plot(*end_frame[agent_id].position, marker="x")
plt.plot(*list(zip(*observed_trajectory.path)), marker="o")
for goal_type in goal_probabilities.goals_and_types:
plt.text(
*goal_type[0].center,
f"p={goal_probabilities.goals_probabilities[goal_type]:.3f}"
)
plt.show()
def test_search_heckstrasse(self):
scenario_map = SCENARIOS["heckstrasse"]
frame = {
0: AgentState(time=0,
position=np.array([6.0, 0.7]),
velocity=1.5,
acceleration=0.0,
heading=-0.6),
1: AgentState(time=0,
position=np.array([19.7, -13.5]),
velocity=8.5,
acceleration=0.0,
heading=-0.6),
2: AgentState(time=0,
position=np.array([73.2, -47.1]),
velocity=11.5,
acceleration=0.0,
heading=np.pi - 0.6),
3: AgentState(time=0,
position=np.array([61.35, -13.9]),
velocity=5.5,
acceleration=0.0,
heading=-np.pi + 0.4),
4: AgentState(time=0,
position=np.array([45.262, -20.5]),
velocity=8.5,
acceleration=0.0,
heading=np.pi - 0.6),
5: AgentState(time=0,
position=np.array([41.33, -19.56]),
velocity=8.5,
acceleration=0.0,
heading=np.pi),
6: AgentState(time=0,
position=np.array([39.24, -25.7]),
velocity=8.5,
acceleration=0.0,
heading=-np.pi/3),
}
goals = {
0: PointGoal(np.array([90.12, -68.061]), 2),
1: PointGoal(np.array([61.17, -18.1]), 2),
2: PointGoal(np.array([61.17, -18.1]), 2),
3: PointGoal(np.array([90.12, -68.061]), 2),
4: PointGoal(np.array([21.09, -6.4]), 2),
5: PointGoal(np.array([21.09, -6.4]), 2),
6: PointGoal(np.array([90.12, -68.061]), 2),
}
colors = {0: "r", 1: "g", 2: "b", 3: "y", 4: "r", 5: "g", 6: "b"}
plot_map(scenario_map, markings=True, midline=False)
for agent_id, agent in frame.items():
plt.plot(agent.position[0], agent.position[1], marker="o", color=colors[agent_id])
plt.text(*agent.position, agent_id)
astar = AStar()
for agent_id in goals:
goal = goals[agent_id]
trajectories, actions = astar.search(agent_id, frame, goal, scenario_map)
for traj in trajectories:
plt.plot(*list(zip(*traj.path)), color=colors[agent_id])
plt.show()
