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")