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 maneuver_follow_lane():
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_map)
return maneuver
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 get_maneuvers(self) -> List[Maneuver]:
state = self.start_frame[self.agent_id]
current_lane = self.scenario_map.best_lane_at(state.position, state.heading)
endpoint = self.termination_point
configs = []
if endpoint is not None:
config_dict = {
"type": "follow-lane",
"termination_point": endpoint
}
configs.append(config_dict)
else:
while current_lane is not None:
endpoint = current_lane.midline.interpolate(1, normalized=True)
config_dict = {
"type": "follow-lane",
"termination_point": np.array(endpoint.coords[0])
}
configs.append(config_dict)
if current_lane.link.successor is not None and len(current_lane.link.successor) == 1:
current_lane = current_lane.link.successor[0]
else:
current_lane = None
maneuvers = []
current_frame = self.start_frame
for config_dict in configs:
config = ManeuverConfig(config_dict)
if self.open_loop:
man = FollowLane(config, self.agent_id, current_frame, self.scenario_map)
else:
man = ip.CLManeuverFactory.create(config, self.agent_id, current_frame, self.scenario_map)
maneuvers.append(man)
current_frame = Maneuver.play_forward_maneuver(self.agent_id, self.scenario_map,
current_frame, maneuvers[-1], 0.1)
self.final_frame = current_frame
return maneuvers
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)
def create(cls, config: ManeuverConfig, agent_id: int,
frame: Dict[int, ip.AgentState], scenario_map: ip.Map):
config.config_dict["adjust_swerving"] = False
return cls.maneuver_types[config.type](config, agent_id, frame,
scenario_map)
import numpy as np
from igp2.agentstate import AgentState, AgentMetadata
from igp2.planlibrary.maneuver import ManeuverConfig
from igp2.agents.maneuver_agent import ManeuverAgent
from igp2.carla.carla_client import CarlaSim
carla_sim = CarlaSim(xodr='scenarios/maps/heckstrasse.xodr')
configs = [
ManeuverConfig({
'type': 'give-way',
'junction_road_id': 5,
'junction_lane_id': -1,
'termination_point': (31.8, -18.5)
}),
ManeuverConfig({
'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,