def prep_tests(self, pathlength, velocity, trajectory1):
trajectory1._pathlength = pathlength
trajectory1._velocity = velocity
trajectory = VelocitySmoother()
trajectory.load_trajectory(trajectory1)
return trajectory
def test_endpoint(self, trajectory1):
"""Checks that the smooth_velocity method will only complete after fully bounding the trajectory"""
trajectory1_smooth = VelocitySmoother()
trajectory1_smooth.load_trajectory(trajectory1)
sol, X, V = trajectory1_smooth.smooth_velocity(trajectory1.pathlength,
trajectory1.velocity)
assert X[-1] >= trajectory1.pathlength[-1]
def test_exception_handling(self):
logger = setup_logging(level=logging.DEBUG)
velocity = np.array([np.inf, np.inf, np.inf])
path = np.array([[0, 0], [0, 1], [0, 2]])
trajectory = VelocityTrajectory(path, velocity)
smoother = VelocitySmoother()
smoother.load_trajectory(trajectory)
sol, X, V = smoother.smooth_velocity(trajectory.pathlength,
trajectory.velocity)
assert all(trajectory.velocity == V)
def test_stop_locations(self, trajectory2):
stop_indent = 27
stop_length = 2
trajectory2_smooth = VelocitySmoother(dt_s=0.1, n=10, amax_m_s2=10)
trajectory2_smooth.load_trajectory(trajectory2)
smoothed_velocity = trajectory2_smooth.split_smooth(False)
stop_velocities = smoothed_velocity.tolist()[stop_indent:stop_indent +
stop_length]
expected_stop_velocities = trajectory2.velocity.tolist(
)[stop_indent:stop_indent + stop_length]
assert stop_velocities == expected_stop_velocities
def test_optimiser_constraints(self, trajectory1):
"""Checks that all constraints are satisfied"""
eps = 1e-5
trajectory1_smooth = VelocitySmoother()
trajectory1_smooth.load_trajectory(trajectory1)
sol, X, V = trajectory1_smooth.smooth_velocity(trajectory1.pathlength,
trajectory1.velocity)
vel_inter = trajectory1_smooth.lin_interpolant(trajectory1.pathlength,
trajectory1.velocity)
c1 = X[0] == trajectory1.pathlength[0]
c2 = V[0] == trajectory1.velocity[0]
c3 = V.max() - trajectory1_smooth.vmax < eps
c4 = V.min() >= 0.0 - eps
c5 = (np.diff(vel_inter(X) - V) >= 0.0).all()
c6 = (abs(np.diff(X) - V[0:-1] * trajectory1_smooth.dt) <= eps).all()
c7 = (np.abs(np.diff(V)) -
trajectory1_smooth.dt * trajectory1_smooth.amax <= eps).all()
errors = []
# replace assertions by conditions
if not c1:
errors.append("Initial pathlength is not matching.")
if not c2:
errors.append("Initial velocity is not matching.")
if not c3:
errors.append("Maximum velocity exceeded.")
if not c4:
errors.append("Negative velocity.")
if not c5:
errors.append("Velocity higher than unsmoothed interpolation.")
if not c6:
errors.append("Inconsistency between path travelled and velocity.")
if not c7:
errors.append("Maximum acceleration exceeded.")
# assert no error message has been registered, else print messages
assert not errors, "errors occured:\n{}".format("\n".join(errors))
"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)
smoother = VelocitySmoother(vmin_m_s=1,
vmax_m_s=10,
n=10,
amax_m_s2=5,
lambda_acc=10)
goal_recognition = GoalRecognition(astar=astar,
smoother=smoother,
scenario_map=scenario_map,
cost=cost,
reward_as_difference=True,
n_trajectories=2)
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 all agents and their full trajectories
for agent_id, agent in episode.agents.items():