def for_timing(env: Env):
cc = CollisionChecker(env, path_length=20)
for path in get_paths(env.state, n_paths=32, n_pts=100):
vel_profile = generate_velocity_profile(env, path)
if np.any(np.isnan(vel_profile)):
continue
cc.check_collisions(path, vel_profile)
def main():
env = Env()
plt.clf()
plt.gca().set_aspect('equal', adjustable='box')
plt.xlim((env.state[0] - 10, env.state[0] + 75))
plt.ylim((env.state[1] - 30, env.state[1] + 75))
path_length = 100
n_paths = 16
cc = CollisionChecker(env, path_length=path_length)
for path in get_paths(env.state, n_paths=n_paths, n_pts=path_length):
vel_profile = generate_velocity_profile(env, path)
if np.any(np.isnan(vel_profile)):
print("ERROR: NAN in velocity_profile")
continue
is_valid = cc.check_collisions(path, vel_profile)
plt.plot(*path.T,
linewidth=(2 if is_valid else 0.5),
c=('green' if is_valid else 'red'),
zorder=0)
# Ego
# Overall Length 192 inches/4876 mm
# Overall Width 76 inches/1930 mm
def draw_rect(center, theta=0, L=4.876, W=1.930, **kwargs):
c, s = np.cos(theta), np.sin(theta)
plt.gca().add_artist(
plt.Rectangle(
center +
np.array([-c * L / 2 + s * W / 2, -s * L / 2 - c * W / 2]),
width=L,
height=W,
angle=theta * 180 / np.pi,
**kwargs))
def draw_vech(xy, theta, params=env.collision_params, **kwargs):
ego_circle_locations = np.repeat(xy.reshape(1, -1),
len(params.circle_offsets), 0)
ego_circle_locations[:, 0] += params.circle_offsets * np.cos(theta)
ego_circle_locations[:, 1] += params.circle_offsets * np.sin(theta)
for loc in ego_circle_locations:
plt.gca().add_artist(plt.Circle(loc, params.circle_radii,
**kwargs))
# Ego Vehicle
# plt.arrow(env.state[0], env.state[1],
# 10 * np.cos(env.state[2]), 10 * np.sin(env.state[2]), head_width=2, label='vehicle', zorder=100)
# draw_rect(env.state[:2], env.state[2], color="blue")
draw_vech(env.state[:2], env.state[2], color="blue")
# Lane Boundary
if len(lane_boundry_pts := env.lane_to_points()) != 0:
plt.scatter(*zip(*lane_boundry_pts), label='Lane Boundaries', s=5)
def score_paths(env: Env, paths: Iterable[path_t], max_path_len: Optional[int] = 50
) -> Tuple[Optional[Tuple[path_t, np.ndarray]], float]:
best_trajectory = None
best_cost = +inf
collision_checker = CollisionChecker(env, max_path_len)
for index, path in enumerate(paths):
path = path[:]
cost = 0.0
costs = np.zeros(len(path)) # store individual costs for visualization
# ~ 300 ms total
vel_profile = generate_velocity_profile(env, path)
if np.any(np.isnan(vel_profile)):
print("ERROR: NAN in velocity_profile")
continue
if not collision_checker.check_collisions(path, vel_profile):
continue
# Assume the path is a set of line segments
global_path_index = 0 # The closes segment to the current point on the candidate path
distance_func = get_distance_func(env.path, global_path_index) # To get distance of point from nearest segment
# ~ 50 ms total
for j in range(len(path)):
pt = path[j]
# Shift to the 'closest' segment of the path
# `get_normal(env.path, global_path_index)(*pt) > 0`
# checks if `pt` is in the forward direction relative to the normal
while global_path_index != len(env.path) - 1 and get_normal(env.path, global_path_index)(*pt) > 0:
global_path_index += 1
distance_func = get_distance_func(env.path, global_path_index)
# Cross Track Error
costs[j] += env.weights.cte * np.abs(distance_func(*pt))
# Velocity Term
if j != len(path) - 1:
costs[j] += env.weights.vel * 1.0 / (vel_profile[j] + 1.0)
# Slope term:
if j != len(path) - 1:
costs[j] += env.weights.slope * np.abs(slope(path, j) - slope(env.path, global_path_index))
cost += costs[j]
if cost < best_cost:
best_trajectory = path, vel_profile
best_cost = cost
return best_trajectory, best_cost
def main(args: Optional[List[str]] = None):
env: Env = Env()
info: Callable[[str], None] = _logger.info
env.info = info
EGO: int = 1
if args is not None and len(args) >= 2:
EGO = int(args[1])
print(f"Using ego {EGO}")
if args is not None and len(args) >= 3 and args[2].strip() == '--no-plot':
env.plot_paths = False
info("Starting up...")
env.global_path_handler.load_from_csv(GLOBAL_PATH_CSV_FILE)
np_path = env.global_path_handler.global_path.to_numpy()[:, :2] # Take only (x, y) from path.
env.path = np.vstack([np_path] * 6 + [np_path[:20, :]]) # Repeat for 6 laps
info(f"Loaded path from {GLOBAL_PATH_CSV_FILE} of length {len(env.path)}")
try:
with rti.open_connector(config_name="SCADE_DS_Controller::Controller",
url=get_xml_url(EGO)) as connector:
info('Opened RTI Connector')
# Readers
class Inputs:
sim_wait: Input = connector.get_input("simWaitSub::simWaitReader")
vehicle_state: Input = connector.get_input("vehicleStateOutSub::vehicleStateOutReader")
track_polys: Input = connector.get_input("camRoadLinesF1Sub::camRoadLinesF1Reader")
other_vehicle_states: Input = connector.get_input("radarFSub::radarFReader")
def list(self) -> Iterable[Input]:
return [self.sim_wait, self.vehicle_state, self.track_polys, self.other_vehicle_states]
inputs = Inputs()
# Writers
vehicle_correct = connector.getOutput("toVehicleModelCorrectivePub::toVehicleModelCorrectiveWriter")
vehicle_steer = connector.getOutput("toVehicleSteeringPub::toVehicleSteeringWriter")
sim_done = connector.getOutput("toSimDonePub::toSimDoneWriter")
sim_done.write()
controller = Controller()
while True:
load_rti(env, inputs)
# info('Got RTI inputs')
t_start = time.time()
# Remove passed points
# Note fails if robot and path have very different orientations, check for that
update_global_path(env)
use_global = False # for plotting only
# ~ 3300 ms
trajectory = run(env)
if trajectory is None:
info("Warning: Could not get trajectory, falling back to global path.")
trajectory = env.path[:31, :], generate_velocity_profile(env, env.path[:32, :])
use_global = True
# ~ 30 ms
throttle, steer = controller.run_controller_timestep(env, trajectory)
if use_global: # Emergency braking
throttle = 0.1
t_logic = time.time()
print(f"time: {(t_logic - t_start):.3f}")
if env.plot_paths: # ~ 200ms
import matplotlib.pyplot as plt
plt.clf()
plt.title(f"EGO {EGO}")
plt.gca().set_aspect('equal', adjustable='box')
plt.xlim((env.state[0] - 75, env.state[0] + 75))
plt.ylim((env.state[1] - 75, env.state[1] + 75))
from iac_planner.collision_check import CollisionChecker
cc = CollisionChecker(env, 20, time_step=0.5)
cc.init_other_paths(trajectory[0])
if len(cc.obstacles) != 0:
plt.scatter(*zip(*cc.obstacles), label='obstacles', s=5)
plt.scatter(*env.path[:40].T, label='global path', s=5)
if trajectory is not None:
plt.scatter(*trajectory[0].T, label=('local path' if not use_global else 'fake local'), s=5)
plt.arrow(env.state[0], env.state[1], 20 * np.cos(env.state[2]), 20 * np.sin(env.state[2]),
head_width=5, label='vehicle')
for i, state in enumerate(env.other_vehicle_states):
plt.arrow(state[0], state[1],
20 * np.cos(state[2]), 20 * np.sin(state[2]),
head_width=5,
label=f"other {i + 1}", color='red')
for i, path in enumerate(cc._other_vehicle_paths):
plt.scatter(*path[:2], s=5, label=f"path {i + 1}", color='red')
plt.legend()
plt.pause(0.005)
vehicle_steer.instance.setNumber("AdditiveSteeringWheelAngle", steer)
vehicle_correct.instance.setNumber("AcceleratorAdditive", throttle)
vehicle_correct.write()
vehicle_steer.write()
sim_done.write()
info("=" * 20)
except KeyboardInterrupt:
info("Keyboard interrupt")