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_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_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_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 extract_goal_data(goals_data):
goals = []
for goal_data in goals_data:
point = Point(np.array(goal_data))
goals.append(PointGoal(point, 1.))
return goals
def goals():
goals_data = [[17.40, -4.97],
[75.18, -56.65],
[62.47, -17.54]]
goals = []
for goal_data in goals_data:
point = Point(np.array(goal_data))
goals.append(PointGoal(point, 1.))
return goals
# This script showcases how to run goal recognition on the InD Dataset
SCENARIO = "round"
PLOT = True
if __name__ == '__main__':
setup_logging()
logger = logging.getLogger(__name__)
colors = "brgyk"
scenario_map = Map.parse_from_opendrive(f"scenarios/maps/{SCENARIO}.xodr")
data_loader = InDDataLoader(f"scenarios/configs/{SCENARIO}.json",
["valid"])
data_loader.load()
goals = np.array(data_loader.scenario.config.goals)
goals = [PointGoal(center, 1.5) for center in goals]
cost_factors = {
"time": 0.001,
"velocity": 0.0,
"acceleration": 0.0,
"jerk": 0.,
"heading": 10,
"angular_velocity": 0.0,
"angular_acceleration": 0.,
"curvature": 0.0,
"safety": 0.
}
astar = AStar(next_lane_offset=0.25)
cost = Cost(factors=cost_factors)
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)
for traj in trajectories:
if PLOT:
plt.plot(*list(zip(*traj.path)),
color=colors[agent_id % 5])
if PLOT: plt.show()
# Iterate over all agents and their full trajectories
for agent_id, agent in episode.agents.items():
if agent.goal_reached:
if PLOT: plot_map(scenario_map, markings=True, midline=True)
if PLOT:
plt.plot(*list(zip(*agent.trajectory.path)), marker="o")
agent_trajectory = agent.trajectory
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()
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),
}
colors = "rgbyk"
scenario_map = SCENARIOS["heckstrasse"]
frame = heckstrasse_frame
goals = {
0: PointGoal(np.array([17.40, -4.97]), 2), #N
1: PointGoal(np.array([75.18, -56.65]), 2), #S
2: PointGoal(np.array([62.47, -17.54]), 2), #W
}
#plot_map(scenario_map, markings=True)
# for agent_id, state in frame.items():
# plt.plot(*state.position, marker="o")
# for _, goal in goals.items():
# plt.plot(*goal.center, marker="x")
# plt.show()
cost_factors = {
"time": 0.001,
"velocity": 0.0,
"acceleration": 0.0,