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_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_applicability(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),
}
applicables = {
0: [Exit, ChangeLaneLeft],
1: [Exit, ChangeLaneLeft],
2: [ChangeLaneRight, ContinueNextExit],
3: [ChangeLaneRight, Exit],
4: [Exit]
}
for agent_id, state in frame.items():
actions = MacroAction.get_applicable_actions(state, scenario_map)
assert all([a in actions for a in applicables[agent_id]])
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_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(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)
from igp2.recognition.goalprobabilities import GoalsProbabilities
from igp2.trajectory import VelocityTrajectory
from igp2.velocitysmoother import VelocitySmoother
from igp2.planning.simulator import Simulator
SCENARIOS = {
"heckstrasse": Map.parse_from_opendrive("scenarios/maps/heckstrasse.xodr"),
"round": Map.parse_from_opendrive("scenarios/maps/round.xodr"),
}
round_frame = {
0:
AgentState(time=0,
position=np.array([96.8, -0.2]),
velocity=4,
acceleration=0.0,
heading=-2 * np.pi / 3),
1:
AgentState(time=0,
position=np.array([25.0, -36.54]),
velocity=4,
acceleration=0.0,
heading=-0.3),
2:
AgentState(time=0,
position=np.array([133.75, -61.67]),
velocity=4,
acceleration=0.0,
heading=5 * np.pi / 6),
3:
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()
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()
'type': 'turn',
'junction_road_id': 5,
'junction_lane_id': -1,
'termination_point': (60.1, -18.5)
})
]
agent_id = 0
position = np.array((3.9, 1.3))
heading = 0.8
speed = 10
velocity = speed * np.array([np.cos(-heading), np.sin(-heading)])
acceleration = np.array([0., 0.])
state_0 = AgentState(time=0,
position=position,
velocity=velocity,
acceleration=acceleration,
heading=heading)
configs1 = [
ManeuverConfig({
'type': 'follow-lane',
'termination_point': (66.0, -42.0)
}),
ManeuverConfig({
'type': 'turn',
'junction_road_id': 8,
'junction_lane_id': -1,
'termination_point': (34.1, -17.2)
}),
ManeuverConfig({