def update_global_path_by_dist(env: Env):
def is_close(x: float, y: float) -> bool:
xe, ye = env.state[:2]
return (x - xe) ** 2 + (y - ye) ** 2 <= 6 ** 2
i = 0
while not is_close(*env.path[i]):
# print('Passed a point in global path.')
i += 1
if i + 10 > len(env.path):
print("ERROR: No point in global path is close enough")
return
print(f"Skipped {i} points in global path, remaining {len(env.path) - i}")
env.path = env.path[i:, :]
def update_global_path(env: Env):
def line_behind_vehicle(x: float, y: float) -> float:
p: state_t = env.state
# yaw = p[2]
p0, p1 = env.path[0:2]
yaw = np.arctan2(p1[1] - p0[1], p1[0] - p0[0])
return (x - p[0]) * np.cos(yaw) + (y - p[1]) * np.sin(yaw)
def is_behind(x: float, y: float) -> bool:
return line_behind_vehicle(x, y) < 0
while is_behind(*env.path[0]):
print('Passed a point in global path.')
env.path = env.path[1:, :]
def __init__(self, env: Env, path_length: int):
# # For Timing
# global times
# times.clear()
# self.print_times = True
self._env = env
self._params: CollisionParams = env.collision_params
self._path_length = path_length
self.other_vehicle_states = np.zeros(
(len(env.other_vehicle_states), 4))
if len(env.other_vehicle_states) != 0:
stacked = np.stack(env.other_vehicle_states, axis=0)
self.other_vehicle_states[:, :2] = env.shift_to_global(
stacked[:, :2])
self.other_vehicle_states[:, 2] = stacked[:, 2] + env.state[2]
self.other_vehicle_states[:, 3] = stacked[:, 3]
self._other_vehicle_paths = np.zeros(
(len(self.other_vehicle_states), 3, path_length), dtype=float)
self._time_steps = np.zeros((path_length, ))
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")