class BeamNGBrewer: def __init__(self, beamng_home=None, road_nodes: List4DTuple = None): self.beamng = BeamNGpy('localhost', 64256, home=beamng_home) self.vehicle: Vehicle = None self.camera: BeamNGCamera = None if road_nodes: self.setup_road_nodes(road_nodes) steps = 5 self.params = SimulationParams(beamng_steps=steps, delay_msec=int(steps * 0.05 * 1000)) self.vehicle_start_pose = BeamNGPose() def setup_road_nodes(self, road_nodes): self.road_nodes = road_nodes self.decal_road: DecalRoad = DecalRoad('street_1').add_4d_points( road_nodes) self.road_points = RoadPoints().add_middle_nodes(road_nodes) def setup_vehicle(self) -> Vehicle: assert self.vehicle is None self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='TIG', color='Red') return self.vehicle def setup_scenario_camera(self, resolution=(1280, 1280), fov=120) -> BeamNGCamera: assert self.camera is None self.camera = BeamNGCamera(self.beamng, 'brewer_camera') return self.camera def bring_up(self): self.scenario = Scenario('tig', 'tigscenario') if self.vehicle: self.scenario.add_vehicle(self.vehicle, pos=self.vehicle_start_pose.pos, rot=self.vehicle_start_pose.rot) if self.camera: self.scenario.add_camera(self.camera.camera, self.camera.name) self.scenario.make(self.beamng) if not self.beamng.server: self.beamng.open() self.beamng.pause() self.beamng.set_deterministic() self.beamng.load_scenario(self.scenario) self.beamng.start_scenario() def __del__(self): if self.beamng: try: self.beamng.close() except: pass
def run_sim(street_1: DecalRoad, ai_aggression): waypoint_goal = BeamNGWaypoint('waypoint_goal', get_node_coords(street_1.nodes[-1])) maps.beamng_map.generated().write_items(street_1.to_json() + '\n' + waypoint_goal.to_json()) beamng = BeamNGpy('localhost', 64256) scenario = Scenario('tig', 'tigscenario') vehicle = Vehicle('ego_vehicle', model='etk800', licence='TIG', color='Red') sim_data_collector = TrainingDataCollectorAndWriter(vehicle, beamng, street_1) scenario.add_vehicle(vehicle, pos=get_node_coords(street_1.nodes[0]), rot=get_rotation(street_1)) scenario.make(beamng) beamng.open() beamng.set_deterministic() beamng.load_scenario(scenario) beamng.pause() beamng.start_scenario() vehicle.ai_set_aggression(ai_aggression) vehicle.ai_drive_in_lane(False) vehicle.ai_set_speed(25) vehicle.ai_set_waypoint(waypoint_goal.name) #vehicle.ai_set_mode("manual") steps = 5 def start(): for idx in range(1000): if (idx * 0.05 * steps) > 3.: sim_data_collector.collect_and_write_current_data() if sim_data_collector.oob_monitor.is_oob(wrt="center"): raise ValueError("OOB detected during training") dist = distance(sim_data_collector.last_state.pos, waypoint_goal.position) if dist < 15.0: beamng.resume() break # one step is 0.05 seconds (5/100) beamng.step(steps) try: start() finally: beamng.close()
class BeamNGBrewer: def __init__(self, beamng_home=None, beamng_user=None, reuse_beamng=True, road_nodes: List4DTuple = None): self.reuse_beamng = reuse_beamng if self.reuse_beamng: # This represents the running BeamNG simulator. Since we use launch=True this should automatically # shut down when the main python process exits or when we call self.beamng_process.stop() self.beamng_process = BeamNGpy('localhost', 64256, home=beamng_home, user=beamng_user) self.beamng_process = self.beamng_process.open(launch=True) # This is used to bring up each simulation without restarting the simulator self.beamng = BeamNGpy('localhost', 64256, home=beamng_home, user=beamng_user) # We need to wait until this point otherwise the BeamNG loggers's level will be (re)configured by BeamNGpy log.info("Disabling BEAMNG logs") for id in [ "beamngpy", "beamngpy.beamngpycommon", "beamngpy.BeamNGpy", "beamngpy.beamng", "beamngpy.Scenario", "beamngpy.Vehicle" ]: logger = log.getLogger(id) logger.setLevel(log.CRITICAL) logger.disabled = True self.vehicle: Vehicle = None self.camera: BeamNGCamera = None if road_nodes: self.setup_road_nodes(road_nodes) steps = 5 self.params = SimulationParams(beamng_steps=steps, delay_msec=int(steps * 0.05 * 1000)) self.vehicle_start_pose = BeamNGPose() def setup_road_nodes(self, road_nodes): self.road_nodes = road_nodes self.decal_road: DecalRoad = DecalRoad('street_1').add_4d_points( road_nodes) self.road_points = RoadPoints().add_middle_nodes(road_nodes) def setup_vehicle(self) -> Vehicle: assert self.vehicle is None self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='TIG', color='Red') return self.vehicle def setup_scenario_camera(self, resolution=(1280, 1280), fov=120) -> BeamNGCamera: assert self.camera is None self.camera = BeamNGCamera(self.beamng, 'brewer_camera') return self.camera # TODO COnsider to transform brewer into a ContextManager or get rid of it... def bring_up(self): if self.reuse_beamng: # This assumes BeamNG is already running self.beamng.open(launch=False) else: self.beamng.open(launch=True) # After 1.18 to make a scenario one needs a running instance of BeamNG self.scenario = Scenario('tig', 'tigscenario') if self.vehicle: self.scenario.add_vehicle(self.vehicle, pos=self.vehicle_start_pose.pos, rot=self.vehicle_start_pose.rot) if self.camera: self.scenario.add_camera(self.camera.camera, self.camera.name) self.scenario.make(self.beamng) self.beamng.set_deterministic() self.beamng.load_scenario(self.scenario) self.beamng.start_scenario() # Pause the simulator only after loading and starting the scenario self.beamng.pause() def __del__(self): if self.beamng: try: self.beamng.close() except: pass
bng = BeamNGpy('localhost', 64256) scenario = Scenario('smallgrid', 'small_test') vehicle = Vehicle('egovehicle', model='etk800', licence='313') scenario.add_vehicle(vehicle, pos=(-1.5, -1, 0), rot=(0, 0, 0)) nodes = [(0, 0, 0, 6), (0, -20, 0, 6), (0, -100, 0, 6), (-25, -200, 0, 6), (25, -300, 0, 6)] road = Road(material='a_asphalt_01_a', rid='main_road', looped=False, texture_length=10) road.nodes.extend(nodes) scenario.add_road(road) scenario.make(bng) for i, line in enumerate(fileinput.input(scenario.get_prefab_path(), inplace=1)): sys.stdout.write(line.replace('overObjects = "0";', 'overObjects = "1";')) bng.open() bng.load_scenario(scenario) bng.start_scenario() bng.set_deterministic() bng.pause()
def main() -> None: # Read CommonRoad scenario cr_scenario, cr_planning_problem_set = CommonRoadFileReader(cr_scenario_path) \ .open() if cr_planning_problem_set: for _, pp in cr_planning_problem_set.planning_problem_dict.items(): cr_planning_problem = pp break # Read only the first one else: raise Exception( "The given CommonRoad scenario does not define a planning problem." ) # Setup BeamNG bng = BeamNGpy('localhost', 64256, home=home_path, user=user_path) bng_scenario = Scenario( bng_scenario_environment, bng_scenario_name, authors='Stefan Huber', description='Simple visualization of the CommonRoad scenario ' + cr_scenario_name) # Add lane network lanes = cr_scenario.lanelet_network.lanelets for lane in lanes: lane_nodes = [] for center in lane.center_vertices: lane_nodes.append((center[0], center[1], 0, 4)) # FIXME Determine appropriate width road = Road(cr_road_material) road.nodes.extend(lane_nodes) bng_scenario.add_road(road) # Add ego vehicle ego_vehicle = Vehicle('ego_vehicle', model='etk800', licence='EGO', color='Cornflowerblue') ego_init_state = cr_planning_problem.initial_state ego_init_state.position[0] = 82.8235 ego_init_state.position[1] = 31.5786 add_vehicle_to_bng_scenario(bng_scenario, ego_vehicle, ego_init_state, etk800_z_offset) obstacles_to_move = dict() # Add truck semi = Vehicle('truck', model='semi', color='Red') semi_init_state = cr_scenario.obstacle_by_id(206).initial_state add_vehicle_to_bng_scenario(bng_scenario, semi, semi_init_state, semi_z_offset) obstacles_to_move[206] = semi # Add truck trailer tanker_init_state = copy(semi_init_state) tanker_init_state.position += [6, 3] add_vehicle_to_bng_scenario( bng_scenario, Vehicle('truck_trailer', model='tanker', color='Red'), tanker_init_state, tanker_z_offset) # Add other traffic participant opponent = Vehicle('opponent', model='etk800', licence='VS', color='Cornflowerblue') add_vehicle_to_bng_scenario(bng_scenario, opponent, cr_scenario.obstacle_by_id(207).initial_state, etk800_z_offset) obstacles_to_move[207] = opponent # Add static obstacle obstacle_shape: Polygon = cr_scenario.obstacle_by_id(399).obstacle_shape obstacle_pos = obstacle_shape.center obstacle_rot_deg = rad2deg(semi_init_state.orientation) + rot_offset obstacle_rot_rad = semi_init_state.orientation + deg2rad(rot_offset) for w in range(3): for h in range(3): for d in range(2): obstacle = Vehicle('obstacle' + str(w) + str(h) + str(d), model='haybale', color='Red') haybale_pos_diff = obstacle_pos \ + pol2cart(1.3 * d, obstacle_rot_rad + pi / 4) \ + pol2cart(2.2 * w, pi / 2 - obstacle_rot_rad) bng_scenario.add_vehicle(obstacle, pos=(haybale_pos_diff[0], haybale_pos_diff[1], h * 1), rot=(0, 0, obstacle_rot_deg)) bng_scenario.make(bng) # Manually set the overObjects attribute of all roads to True (Setting this option is currently not supported) prefab_path = os.path.join(user_path, 'levels', bng_scenario_environment, 'scenarios', bng_scenario_name + '.prefab') lines = open(prefab_path, 'r').readlines() for i in range(len(lines)): if 'overObjects' in lines[i]: lines[i] = lines[i].replace('0', '1') open(prefab_path, 'w').writelines(lines) # Start simulation bng.open(launch=True) try: bng.load_scenario(bng_scenario) bng.start_scenario() bng.pause() bng.display_gui_message( "The scenario is fully prepared and paused. You may like to position the camera first." ) delay_to_resume = 15 input("Press enter to continue the simulation... You have " + str(delay_to_resume) + " seconds to switch back to the BeamNG window.") sleep(delay_to_resume) bng.resume() for id, obstacle in obstacles_to_move.items(): obstacle.ai_drive_in_lane(False) obstacle.ai_set_line( generate_node_list(cr_scenario.obstacle_by_id(id))) ego_vehicle.ai_drive_in_lane(False) # ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='limit') speed = 65 / 3.6 ego_vehicle.ai_set_line([{ 'pos': ego_vehicle.state['pos'] }, { 'pos': (129.739, 56.907, etk800_z_offset), 'speed': speed }, { 'pos': (139.4, 62.3211, etk800_z_offset), 'speed': speed }, { 'pos': (149.442, 64.3655, etk800_z_offset), 'speed': speed }, { 'pos': (150.168, 63.3065, etk800_z_offset), 'speed': speed }, { 'pos': (188.495, 78.8517, etk800_z_offset), 'speed': speed }]) # FIXME Setting the speed does not work as expected # ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='set') input("Press enter to end simulation...") finally: bng.close()
class Simulation(object): def __init__(self) -> None: super().__init__() thread = Thread(target=self._intervene) thread.start() self.step = 0 self.dist_driven = 0 self.done = False self.last_action = (0.0, 0.0) self.bng = BeamNGpy('localhost', 64257, home=BEAMNG_HOME) self.scenario = Scenario('train', 'train', authors='Vsevolod Tymofyeyev', description='Reinforcement learning') self.road = TrainingRoad(ASFAULT_PREFAB) self.road.calculate_road_line() spawn_point = self.road.spawn_point() self.last_pos = spawn_point.pos() self.scenario.add_road(self.road.asphalt) self.scenario.add_road(self.road.mid_line) self.scenario.add_road(self.road.left_line) self.scenario.add_road(self.road.right_line) self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='RL FTW', color='Red') front_camera = Camera(pos=(0, 1.4, 1.8), direction=(0, 1, -0.23), fov=FOV, resolution=(CAMERA_WIDTH, CAMERA_HEIGHT), colour=True, depth=False, annotation=False) self.vehicle.attach_sensor("front_camera", front_camera) self.scenario.add_vehicle(self.vehicle, pos=spawn_point.pos(), rot=spawn_point.rot()) self.scenario.make(self.bng) prefab_path = self.scenario.get_prefab_path() update_prefab(prefab_path) self.bng.open() self.bng.set_deterministic() self.bng.set_steps_per_second(SPS) self.bng.load_scenario(self.scenario) self.bng.start_scenario() # self.bng.hide_hud() self.bng.pause() def _intervene(self): while True: a = input() self.done = not self.done def take_action(self, action): steering, throttle = action steering = steering.item() throttle = throttle.item() self.last_action = action self.step += 1 self.vehicle.control(steering=steering, throttle=throttle, brake=0.0) self.bng.step(STEPS_INTERVAL) def _reward(self, done, dist): steering = self.last_action[0] throttle = self.last_action[1] velocity = self.velocity() # km/h if not done: reward = REWARD_STEP + THROTTLE_REWARD_WEIGHT * throttle #- MID_DIST_PENALTY_WEIGHT * dist else: reward = REWARD_CRASH - CRASH_SPEED_WEIGHT * throttle return reward def observe(self): sensors = self.bng.poll_sensors(self.vehicle) image = sensors['front_camera']['colour'].convert("RGB") image = np.array(image) r = ROI # Cut to the relevant region image = image[int(r[1]):int(r[1] + r[3]), int(r[0]):int(r[0] + r[2])] # Convert to BGR state = image[:, :, ::-1] #done = self.done pos = self.vehicle.state['pos'] dir = self.vehicle.state['dir'] self.refresh_dist(pos) self.last_pos = pos dist = self.road.dist_to_center(self.last_pos) #velocity = self.velocity() done = dist > MAX_DIST #or velocity > MAX_VELOCITY reward = self._reward(done, dist) return state, reward, done, {} def velocity(self): state = self.vehicle.state velocity = np.linalg.norm(state["vel"]) return velocity * 3.6 def position(self): return self.vehicle.state["pos"] def refresh_dist(self, pos): pos = np.array(pos) last_pos = np.array(self.last_pos) dist = np.linalg.norm(pos - last_pos) self.dist_driven += dist def close_connection(self): self.bng.close() def reset(self): self.vehicle.control(throttle=0, brake=0, steering=0) self.bng.poll_sensors(self.vehicle) self.dist_driven = 0 self.step = 0 self.done = False current_pos = self.vehicle.state['pos'] closest_point = self.road.closest_waypoint(current_pos) #closest_point = self.road.random_waypoint() self.bng.teleport_vehicle(vehicle=self.vehicle, pos=closest_point.pos(), rot=closest_point.rot()) self.bng.pause() # TODO delete def wait(self): from client.aiExchangeMessages_pb2 import SimStateResponse return SimStateResponse.SimState.RUNNING
class WCARaceGeometry(gym.Env): sps = 50 rate = 5 front_dist = 800 front_step = 100 trail_dist = 104 trail_step = 13 starting_proj = 1710 max_damage = 100 def __init__(self, host='localhost', port=64256): self.steps = WCARaceGeometry.sps // WCARaceGeometry.rate self.host = host self.port = port self.action_space = self._action_space() self.observation_space = self._observation_space() self.episode_steps = 0 self.spine = None self.l_edge = None self.r_edge = None self.polygon = None front_factor = WCARaceGeometry.front_dist / WCARaceGeometry.front_step trail_factor = WCARaceGeometry.trail_dist / WCARaceGeometry.trail_step self.front = lambda step: +front_factor * step self.trail = lambda step: -trail_factor * step self.bng = BeamNGpy(self.host, self.port) self.vehicle = Vehicle('racecar', model='sunburst', licence='BEAMNG', colour='red', partConfig='vehicles/sunburst/hillclimb.pc') electrics = Electrics() damage = Damage() self.vehicle.attach_sensor('electrics', electrics) self.vehicle.attach_sensor('damage', damage) scenario = Scenario('west_coast_usa', 'wca_race_geometry_v0') scenario.add_vehicle(self.vehicle, pos=(394.5, -247.925, 145.25), rot=(0, 0, 90)) scenario.make(self.bng) self.bng.open(launch=True) self.bng.set_deterministic() self.bng.set_steps_per_second(WCARaceGeometry.sps) self.bng.load_scenario(scenario) self._build_racetrack() self.observation = None self.last_observation = None self.last_spine_proj = None self.bng.start_scenario() self.bng.pause() def __del__(self): self.bng.close() def _build_racetrack(self): roads = self.bng.get_roads() track = roads['race_ref'] l_vtx = [] s_vtx = [] r_vtx = [] for right, middle, left in track: r_vtx.append(right) s_vtx.append(middle) l_vtx.append(left) self.spine = LinearRing(s_vtx) self.r_edge = LinearRing(r_vtx) self.l_edge = LinearRing(l_vtx) r_vtx = [v[0:2] for v in r_vtx] l_vtx = [v[0:2] for v in l_vtx] self.polygon = Polygon(l_vtx, holes=[r_vtx]) def _action_space(self): action_lo = [-1., -1.] action_hi = [+1., +1.] return gym.spaces.Box(np.array(action_lo), np.array(action_hi), dtype=float) def _observation_space(self): # n vertices of left and right polylines ahead and behind, 3 floats per # vtx scope = WCARaceGeometry.trail_step + WCARaceGeometry.front_step obs_lo = [ -np.inf, ] * scope * 3 obs_hi = [ np.inf, ] * scope * 3 obs_lo.extend([ -np.inf, # Distance to left edge -np.inf, # Distance to right edge -2 * np.pi, # Inclination -2 * np.pi, # Angle -2 * np.pi, # Vertical angle -np.inf, # Spine speed 0, # RPM -1, # Gear 0, # Throttle 0, # Brake -1.0, # Steering 0, # Wheel speed -np.inf, # Altitude ]) obs_hi.extend([ np.inf, # Distance to left edge np.inf, # Distance to right edge 2 * np.pi, # Inclincation 2 * np.pi, # Angle 2 * np.pi, # Vertical angle np.inf, # Spine speed np.inf, # RPM 8, # Gear 1.0, # Throttle 1.0, # Brake 1.0, # Steering np.inf, # Wheel speed np.inf, # Altitude ]) return gym.spaces.Box(np.array(obs_lo), np.array(obs_hi), dtype=float) def _make_commands(self, action): brake = 0 throttle = action[1] steering = action[0] if throttle < 0: brake = -throttle throttle = 0 self.vehicle.control(steering=steering, throttle=throttle, brake=brake) def _project_vehicle(self, pos): r_proj = self.r_edge.project(pos) r_proj = self.r_edge.interpolate(r_proj) l_proj = self.l_edge.project(r_proj) l_proj = self.l_edge.interpolate(l_proj) s_proj = self.spine.project(r_proj) s_proj = self.spine.interpolate(s_proj) return l_proj, s_proj, r_proj def _get_vehicle_angles(self, vehicle_pos, spine_seg): spine_beg = spine_seg.coords[+0] spine_end = spine_seg.coords[-1] spine_angle = np.arctan2(spine_end[1] - spine_beg[1], spine_end[0] - spine_beg[0]) vehicle_angle = self.vehicle.state['dir'][0:2] vehicle_angle = np.arctan2(vehicle_angle[1], vehicle_angle[0]) vehicle_angle = normalise_angle(vehicle_angle - spine_angle) vehicle_angle -= np.pi elevation = np.arctan2(spine_beg[2] - spine_end[2], spine_seg.length) vehicle_elev = self.vehicle.state['dir'] vehicle_elev = np.arctan2(vehicle_elev[2], np.linalg.norm(vehicle_elev)) return vehicle_angle, vehicle_elev, elevation def _wrap_length(self, target): length = self.spine.length while target < 0: target += length while target > length: target -= length return target def _gen_track_scope_loop(self, it, fn, base, s_scope, s_width): for step in it: distance = base + fn(step) distance = self._wrap_length(distance) s_proj = self.spine.interpolate(distance) s_scope.append(s_proj) l_proj = self.l_edge.project(s_proj) l_proj = self.l_edge.interpolate(l_proj) r_proj = self.r_edge.project(s_proj) r_proj = self.r_edge.interpolate(r_proj) width = l_proj.distance(r_proj) s_width.append(width) def _gen_track_scope(self, pos, spine_seg): s_scope = [] s_width = [] base = self.spine.project(pos) it = range(WCARaceGeometry.trail_step, 0, -1) self._gen_track_scope_loop(it, self.trail, base, s_scope, s_width) it = range(1) self._gen_track_scope_loop(it, lambda x: x, base, s_scope, s_width) it = range(WCARaceGeometry.front_step + 1) self._gen_track_scope_loop(it, self.front, base, s_scope, s_width) s_proj = self.spine.interpolate(base) offset = (-s_proj.x, -s_proj.y, -s_proj.z) s_line = LineString(s_scope) s_line = affinity.translate(s_line, *offset) spine_beg = spine_seg.coords[+0] spine_end = spine_seg.coords[-1] direction = [spine_end[i] - spine_beg[i] for i in range(3)] angle = np.arctan2(direction[1], direction[0]) + np.pi / 2 s_line = affinity.rotate(s_line, -angle, origin=(0, 0), use_radians=True) ret = list() s_scope = s_line.coords for idx in range(1, len(s_scope) - 1): curvature = calculate_curvature(s_scope, idx) inclination = calculate_inclination(s_scope, idx) width = s_width[idx] ret.append(curvature) ret.append(inclination) ret.append(width) return ret def _spine_project_vehicle(self, vehicle_pos): proj = self.spine.project(vehicle_pos) - WCARaceGeometry.starting_proj if proj < 0: proj += self.spine.length proj = self.spine.length - proj return proj def _get_spine_speed(self, vehicle_pos, vehicle_dir, spine_seg): spine_beg = spine_seg.coords[0] future_pos = Point(vehicle_pos.x + vehicle_dir[0], vehicle_pos.y + vehicle_dir[1], vehicle_pos.z + vehicle_dir[2]) spine_end = self.spine.project(future_pos) spine_end = self.spine.interpolate(spine_end) return spine_end.distance(Point(*spine_beg)) def _make_observation(self, sensors): electrics = sensors['electrics']['values'] vehicle_dir = self.vehicle.state['dir'] vehicle_pos = self.vehicle.state['pos'] vehicle_pos = Point(*vehicle_pos) spine_beg = self.spine.project(vehicle_pos) spine_end = spine_beg spine_end += WCARaceGeometry.front_dist / WCARaceGeometry.front_step spine_beg = self.spine.interpolate(spine_beg) spine_end = self.spine.interpolate(spine_end) spine_seg = LineString([spine_beg, spine_end]) spine_speed = self._get_spine_speed(vehicle_pos, vehicle_dir, spine_seg) l_dist = self.l_edge.distance(vehicle_pos) r_dist = self.r_edge.distance(vehicle_pos) angle, vangle, elevation = self._get_vehicle_angles( vehicle_pos, spine_seg) l_proj, s_proj, r_proj = self._project_vehicle(vehicle_pos) s_scope = self._gen_track_scope(vehicle_pos, spine_seg) obs = list() obs.extend(s_scope) obs.append(l_dist) obs.append(r_dist) obs.append(elevation) obs.append(angle) obs.append(vangle) obs.append(spine_speed) obs.append(electrics['rpm']) obs.append(electrics['gearIndex']) obs.append(electrics['throttle']) obs.append(electrics['brake']) obs.append(electrics['steering']) obs.append(electrics['wheelspeed']) obs.append(electrics['altitude']) return np.array(obs) def _compute_reward(self, sensors): damage = sensors['damage'] vehicle_pos = self.vehicle.state['pos'] vehicle_pos = Point(*vehicle_pos) if damage['damage'] > WCARaceGeometry.max_damage: return -1, True if not self.polygon.contains(Point(vehicle_pos.x, vehicle_pos.y)): return -1, True score, done = -1, False spine_proj = self._spine_project_vehicle(vehicle_pos) if self.last_spine_proj is not None: diff = spine_proj - self.last_spine_proj if diff >= -0.10: if diff < 0.5: return -1, False else: score, done = diff / self.steps, False elif np.abs(diff) > self.spine.length * 0.95: score, done = 1, True else: score, done = -1, True self.last_spine_proj = spine_proj return score, done def reset(self): self.episode_steps = 0 self.vehicle.control(throttle=0, brake=0, steering=0) self.bng.restart_scenario() self.bng.step(30) self.bng.pause() self.vehicle.set_shift_mode(3) self.vehicle.control(gear=2) sensors = self.bng.poll_sensors(self.vehicle) self.observation = self._make_observation(sensors) vehicle_pos = self.vehicle.state['pos'] vehicle_pos = Point(*vehicle_pos) self.last_spine_proj = self._spine_project_vehicle(vehicle_pos) return self.observation def advance(self): self.bng.step(self.steps, wait=False) def observe(self): sensors = self.bng.poll_sensors(self.vehicle) new_observation = self._make_observation(sensors) return new_observation, sensors def step(self, action): action = [*np.clip(action, -1, 1), action[0], action[1]] action = [float(v) for v in action] self.episode_steps += 1 self._make_commands(action) self.advance() new_observation, sensors = self.observe() if self.observation is not None: self.last_observation = self.observation self.observation = new_observation score, done = self._compute_reward(sensors) print((' A: {:5.2f} B: {:5.2f} ' ' S: {:5.2f} T: {:5.2f} R: {:5.2f}').format( action[2], action[3], action[0], action[1], score)) # if done: # self.bng.step(WCARaceGeometry.sps * 1) return self.observation, score, done, {}
class ImpactGenerator: parts = [ 'etk800_bumper_F', 'etk800_bumperbar_F', 'etk800_bumper_R', 'etk800_fender_L', 'etk800_fender_R', 'etk800_hood', 'etk800_towhitch', 'etk800_steer', 'etk800_radiator', 'etk800_roof_wagon', 'wheel_F_5', ] emptyable = { 'etk800_bumperbar_F', 'etk800_towhitch', } wca = { 'level': 'west_coast_usa', 'a_spawn': (-270.75, 678, 74.9), 'b_spawn': (-260.25, 678, 74.9), 'pole_pos': (-677.15, 848, 75.1), 'linear_pos_a': (-630, 65, 103.4), 'linear_pos_b': (-619, 77, 102.65), 'linear_rot_b': (0, 0, 45.5), 't_pos_a': (-440, 688, 75.1), 't_pos_b': (-453, 700, 75.1), 't_rot_b': (0, 0, 315), 'ref_pos': (-18, 610, 75), } smallgrid = { 'level': 'smallgrid', 'a_spawn': (-270.75, 678, 0.1), 'b_spawn': (-260.25, 678, 0.1), 'pole_pos': (-677.15, 848, 0.1), 'pole': (-682, 842, 0), 'linear_pos_a': (-630, 65, 0.1), 'linear_pos_b': (-619, 77, 0.1), 'linear_rot_b': (0, 0, 45.5), 't_pos_a': (-440, 688, 0.1), 't_pos_b': (-453, 700, 0.1), 't_rot_b': (0, 0, 315), 'ref_pos': (321, 321, 0.1), } def __init__(self, bng_home, output, config, poolsize=2, smallgrid=False, sim_mtx=None, similarity=0.5, random_select=False, single=False): self.bng_home = bng_home self.output = output self.config = config self.smallgrid = smallgrid self.single = single self.impactgen_mats = None if smallgrid: mats = materialmngr.get_impactgen_materials(bng_home) self.impactgen_mats = sorted(list(mats)) self.poolsize = poolsize self.similarity = similarity self.sim_mtx = sim_mtx self.random_select = random_select self.pole_space = None self.t_bone_space = None self.linear_space = None self.nocrash_space = None self.post_space = None self.total_possibilities = 0 self.bng = BeamNGpy('localhost', 64256, home=bng_home) self.scenario = None scenario_props = ImpactGenerator.wca if smallgrid: scenario_props = ImpactGenerator.smallgrid self.vehicle_a = Vehicle('vehicle_a', model='etk800') self.vehicle_b = Vehicle('vehicle_b', model='etk800') self.scenario = Scenario(scenario_props['level'], 'impactgen') self.scenario.add_vehicle(self.vehicle_a, pos=scenario_props['a_spawn'], rot=(0, 0, 0)) self.scenario.add_vehicle(self.vehicle_b, pos=scenario_props['b_spawn'], rot=(0, 0, 0)) self.vehicle_a_parts = defaultdict(set) self.vehicle_a_config = None self.vehicle_b_config = None def generate_colors(self): return copy.deepcopy(self.config['colors']) def generate_nocrash_space(self, props): nocrash_options = [] for part in ImpactGenerator.parts: # Vary each configurable part nocrash_options.append(self.vehicle_a_parts[part]) self.nocrash_space = OptionSpace(nocrash_options) def generate_pole_space(self, props): pole_options = [(False, True)] # Vehicle facing forward/backward pole_options.append(np.linspace(-0.75, 0.75, 5)) # Position offset pole_options.append(np.linspace(0.15, 0.5, 4)) # Throttle intensity for part in ImpactGenerator.parts: # Vary each configurable part pole_options.append(self.vehicle_a_parts[part]) self.pole_space = OptionSpace(pole_options) def generate_t_bone_space(self, props): t_options = [(False, True)] # Vehicle hit left/right t_options.append(np.linspace(-30, 30, 11)) # A rotation offset t_options.append(np.linspace(-1.5, 1.5, 5)) # B pos. offset t_options.append(np.linspace(0.2, 0.5, 4)) # B throttle for part in ImpactGenerator.parts: t_options.append(self.vehicle_a_parts[part]) self.t_bone_space = OptionSpace(t_options) def generate_linear_space(self, props): linear_options = [(False, True)] # Vehicle hit front/back linear_options.append(np.linspace(-15, 15, 5)) # A rot. offset linear_options.append(np.linspace(-1.33, 1.33, 5)) # B pos. offset linear_options.append(np.linspace(0.25, 0.5, 4)) # B throttle for part in ImpactGenerator.parts: linear_options.append(self.vehicle_a_parts[part]) self.linear_space = OptionSpace(linear_options) def get_material_options(self): if not self.random_select: selected = materialmngr.pick_materials(self.impactgen_mats, self.sim_mtx, poolsize=self.poolsize, similarity=self.similarity) if selected is None: log.info('Could not find material pool through similarity. ' 'Falling back to random select.') else: selected = random.sample(self.impactgen_mats, self.poolsize) return selected def generate_post_space(self): colors = self.generate_colors() post_options = [] post_options.append(self.config['times']) if self.smallgrid: post_options.append([0]) post_options.append([0]) else: post_options.append(self.config['clouds']) post_options.append(self.config['fogs']) post_options.append(colors) if self.smallgrid: mats = self.get_material_options() if mats is not None: post_options.append(list(mats)) post_options.append(list(mats)) return OptionSpace(post_options) def generate_spaces(self): props = ImpactGenerator.wca if self.smallgrid: props = ImpactGenerator.smallgrid self.generate_nocrash_space(props) self.generate_t_bone_space(props) self.generate_linear_space(props) self.generate_pole_space(props) def scan_parts(self, parts, known=set()): with open('out.json', 'w') as outfile: outfile.write(json.dumps(parts, indent=4, sort_keys=True)) for part_type in ImpactGenerator.parts: options = parts[part_type] self.vehicle_a_parts[part_type].update(options) def init_parts(self): self.vehicle_a_config = self.vehicle_a.get_part_config() self.vehicle_b_config = self.vehicle_b.get_part_config() b_parts = self.vehicle_b_config['parts'] b_parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU' b_parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU' b_parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1' options = self.vehicle_a.get_part_options() self.scan_parts(options) for k in self.vehicle_a_parts.keys(): self.vehicle_a_parts[k] = list(self.vehicle_a_parts[k]) if k in ImpactGenerator.emptyable: self.vehicle_a_parts[k].append('') def init_settings(self): self.bng.set_particles_enabled(False) self.generate_spaces() log.info('%s pole crash possibilities.', self.pole_space.count) log.info('%s T-Bone crash possibilities.', self.t_bone_space.count) log.info('%s parallel crash possibilities.', self.linear_space.count) log.info('%s no crash possibilities.', self.nocrash_space.count) self.total_possibilities = \ self.pole_space.count + \ self.t_bone_space.count + \ self.linear_space.count + \ self.nocrash_space.count log.info('%s total incidents possible.', self.total_possibilities) def get_vehicle_config(self, setting): parts = dict() for idx, part in enumerate(ImpactGenerator.parts): parts[part] = setting[idx] refwheel = parts['wheel_F_5'] parts['wheel_R_5'] = refwheel.replace('_F', '_R') # Force licence plate to always be German parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU' parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU' parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1' config = copy.deepcopy(self.vehicle_a_config) config['parts'] = parts return config def set_annotation_paths(self): part_path = os.path.join(self.bng_home, PART_ANNOTATIONS) part_path = os.path.abspath(part_path) obj_path = os.path.join(self.bng_home, OBJ_ANNOTATIONS) obj_path = os.path.abspath(obj_path) req = dict(type='ImpactGenSetAnnotationPaths') req['partPath'] = part_path req['objPath'] = obj_path self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenAnnotationPathsSet' def set_image_properties(self): req = dict(type='ImpactGenSetImageProperties') req['imageWidth'] = self.config['imageWidth'] req['imageHeight'] = self.config['imageHeight'] req['colorFmt'] = self.config['colorFormat'] req['annotFmt'] = self.config['annotFormat'] req['radius'] = self.config['cameraRadius'] req['height'] = self.config['cameraHeight'] req['fov'] = self.config['fov'] self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenImagePropertiesSet' def setup(self): self.scenario.make(self.bng) log.debug('Loading scenario...') self.bng.load_scenario(self.scenario) log.debug('Setting steps per second...') self.bng.set_steps_per_second(50) log.debug('Enabling deterministic mode...') self.bng.set_deterministic() log.debug('Starting scenario...') self.bng.start_scenario() log.debug('Scenario started. Sleeping 20s.') time.sleep(20) self.init_parts() self.init_settings() log.debug('Setting annotation properties.') self.set_annotation_paths() self.set_image_properties() def settings_exhausted(self): return self.t_bone_space.exhausted() and \ self.linear_space.exhausted() and \ self.pole_space.exhausted() and \ self.nocrash_space.exhausted() def set_post_settings(self, vid, settings): req = dict(type='ImpactGenPostSettings') req['ego'] = vid req['time'] = settings[0] req['clouds'] = settings[1] req['fog'] = settings[2] req['color'] = settings[3] if len(settings) > 4: req['skybox'] = settings[4] if len(settings) > 5: req['ground'] = settings[5] self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenPostSet' def finished_producing(self): req = dict(type='ImpactGenOutputGenerated') self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenZipGenerated' return resp['state'] def produce_output(self, color_name, annot_name): while not self.finished_producing(): time.sleep(0.2) req = dict(type='ImpactGenGenerateOutput') req['colorName'] = color_name req['annotName'] = annot_name self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenZipStarted' 'ImpactGenZipStarted' def capture_post(self, crash_setting): log.info('Enumerating post-crash settings and capturing output.') self.bng.switch_vehicle(self.vehicle_a) ref_pos = ImpactGenerator.wca['ref_pos'] if self.smallgrid: ref_pos = ImpactGenerator.smallgrid['ref_pos'] self.bng.teleport_vehicle(self.vehicle_a, ref_pos) self.bng.teleport_vehicle(self.vehicle_b, (10000, 10000, 10000)) self.bng.step(50, wait=True) self.bng.pause() self.post_space = self.generate_post_space() while not self.post_space.exhausted(): post_setting = self.post_space.sample_new() scenario = [[str(s) for s in crash_setting]] scenario.append([str(s) for s in post_setting]) key = str(scenario).encode('ascii') key = hashlib.sha512(key).hexdigest()[:30] t = int(time.time()) color_name = '{}_{}_0_image.zip'.format(t, key) annot_name = '{}_{}_0_annotation.zip'.format(t, key) color_name = os.path.join(self.output, color_name) annot_name = os.path.join(self.output, annot_name) log.info('Setting post settings.') self.set_post_settings(self.vehicle_a.vid, post_setting) log.info('Producing output.') self.produce_output(color_name, annot_name) if self.single: break self.bng.resume() def run_t_bone_crash(self): log.info('Running t-bone crash setting.') if self.t_bone_space.exhausted(): log.debug('T-Bone crash setting exhausted.') return None props = ImpactGenerator.wca if self.smallgrid: props = ImpactGenerator.smallgrid setting = self.t_bone_space.sample_new() side, angle, offset, throttle = setting[:4] config = setting[4:] config = self.get_vehicle_config(config) if side: angle += 225 else: angle += 45 pos_a = props['t_pos_a'] rot_b = props['t_rot_b'] pos_b = list(props['t_pos_b']) pos_b[0] += offset req = dict(type='ImpactGenRunTBone') req['ego'] = self.vehicle_a.vid req['other'] = self.vehicle_b.vid req['config'] = config req['aPosition'] = pos_a req['angle'] = angle req['bPosition'] = pos_b req['bRotation'] = rot_b req['throttle'] = throttle log.debug('Sending t-bone crash config.') self.bng.send(req) log.debug('T-Bone crash response received.') resp = self.bng.recv() assert resp['type'] == 'ImpactGenTBoneRan' return setting def run_linear_crash(self): log.info('Running linear crash setting.') if self.linear_space.exhausted(): log.debug('Linear crash settings exhausted.') return None props = ImpactGenerator.wca if self.smallgrid: props = ImpactGenerator.smallgrid setting = self.linear_space.sample_new() back, angle, offset, throttle = setting[:4] config = setting[4:] config = self.get_vehicle_config(config) if back: angle += 225 else: offset += 1.3 angle += 45 pos_a = props['linear_pos_a'] rot_b = props['linear_rot_b'] pos_b = list(props['linear_pos_b']) pos_b[0] += offset req = dict(type='ImpactGenRunLinear') req['ego'] = self.vehicle_a.vid req['other'] = self.vehicle_b.vid req['config'] = config req['aPosition'] = pos_a req['angle'] = angle req['bPosition'] = pos_b req['bRotation'] = rot_b req['throttle'] = throttle log.debug('Sending linear crash config.') self.bng.send(req) log.debug('Linear crash response received.') resp = self.bng.recv() assert resp['type'] == 'ImpactGenLinearRan' return setting def run_pole_crash(self): log.info('Running pole crash setting.') if self.pole_space.exhausted(): log.debug('Pole crash settings exhausted.') return None props = ImpactGenerator.wca if self.smallgrid: props = ImpactGenerator.smallgrid setting = self.pole_space.sample_new() back, offset, throttle = setting[:3] config = setting[3:] config = self.get_vehicle_config(config) angle = 45 if back: angle = 225 offset += 0.85 throttle = -throttle pos = list(props['pole_pos']) pos[0] += offset req = dict(type='ImpactGenRunPole') req['ego'] = self.vehicle_a.vid req['config'] = config req['position'] = pos req['angle'] = angle req['throttle'] = throttle if self.smallgrid: req['polePosition'] = props['pole'] log.debug('Sending pole crash config.') self.bng.send(req) log.debug('Got pole crash response.') resp = self.bng.recv() assert resp['type'] == 'ImpactGenPoleRan' return setting def run_no_crash(self): log.info('Running non-crash scenario.') if self.nocrash_space.exhausted(): return None setting = self.nocrash_space.sample_new() log.info('Got new setting: %s', setting) vehicle_config = self.get_vehicle_config(setting) log.info('Got new vehicle config: %s', vehicle_config) req = dict(type='ImpactGenRunNonCrash') req['ego'] = self.vehicle_a.vid req['config'] = vehicle_config log.info('Sending Non-Crash request: %s', req) self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenNonCrashRan' log.info('Non-crash finished.') return setting def run_incident(self, incident): log.info('Setting up next incident.') self.bng.display_gui_message('Setting up next incident...') setting = incident() self.capture_post(setting) return setting def run_incidents(self): log.info('Enumerating possible incidents.') count = 1 incidents = [ self.run_t_bone_crash, self.run_linear_crash, self.run_pole_crash, self.run_no_crash, ] while not self.settings_exhausted(): log.info('Running incident %s of %s...', count, self.total_possibilities) self.bng.restart_scenario() log.info('Scenario restarted.') time.sleep(5.0) self.vehicle_b.set_part_config(self.vehicle_b_config) log.info('Vehicle B config set.') incident = incidents[count % len(incidents)] if self.run_incident(incident) is None: log.info('Ran out of options for: %s', incident) incidents.remove(incident) # Possibility space exhausted count += 1 def run(self): log.info('Starting up BeamNG instance.') self.bng.open(['impactgen/crashOutput']) self.bng.skt.settimeout(1000) try: log.info('Setting up BeamNG instance.') self.setup() self.run_incidents() finally: log.info('Closing BeamNG instance.') self.bng.close()
class Driver: vehicle = None bng = None labeler = None left_path = None right_path = None driving_path = None road_profiler = None car_model = None road_model = None def __init__(self, car_model: dict, road_model: dict, control_model: dict): self.car_model = car_model self.road_model = road_model self.control_model = control_model # Note: Speed limit must be m/s self.road_profiler = RoadProfiler( road_model['mu'], road_model['speed_limit'] / 3.6, control_model['discretization_factor']) def _compute_driving_path(self, car_state, road_name): road_geometry = self.bng.get_road_edges(road_name) left_edge_x = np.array([e['left'][0] for e in road_geometry]) left_edge_y = np.array([e['left'][1] for e in road_geometry]) right_edge_x = np.array([e['right'][0] for e in road_geometry]) right_edge_y = np.array([e['right'][1] for e in road_geometry]) road_edges = dict() road_edges['left_edge_x'] = left_edge_x road_edges['left_edge_y'] = left_edge_y road_edges['right_edge_x'] = right_edge_x road_edges['right_edge_y'] = right_edge_y self.right_edge = LineString( zip(road_edges['right_edge_x'][::-1], road_edges['right_edge_y'][::-1])) self.left_edge = LineString( zip(road_edges['left_edge_x'], road_edges['left_edge_y'])) current_position = Point(car_state['pos'][0], car_state['pos'][1]) from shapely.ops import nearest_points from shapely.affinity import rotate projection_point_on_right = nearest_points(self.right_edge, current_position)[0] projection_point_on_left = nearest_points(self.left_edge, current_position)[0] # If the car is closest to the left, then we need to switch the direction of the road... if current_position.distance( projection_point_on_right) > current_position.distance( projection_point_on_left): # Swap the axis and recompute the projection points l.debug("Reverse traffic direction") temp = self.right_edge self.right_edge = self.left_edge self.left_edge = temp del temp projection_point_on_right = nearest_points(self.right_edge, current_position)[0] projection_point_on_left = nearest_points(self.left_edge, current_position)[0] # Traffic direction is always 90-deg counter clockwise from right # Now rotate right point 90-deg counter clockwise from left and we obtain the traffic direction rotated_right = rotate(projection_point_on_right, 90.0, origin=projection_point_on_left) # Vector defining the direction of the road traffic_direction = np.array([ rotated_right.x - projection_point_on_left.x, rotated_right.y - projection_point_on_left.y ]) # Find the segment containing the projection of current location # Starting point on right edge start_point = None for pair in pairs(list(self.right_edge.coords[:])): segment = LineString([pair[0], pair[1]]) # xs, ys = segment.coords.xy # plt.plot(xs, ys, color='green') if segment.distance(projection_point_on_right) < 1.8e-5: road_direction = np.array( [pair[1][0] - pair[0][0], pair[1][1] - pair[0][1]]) if dot(traffic_direction, road_direction) < 0: l.debug("Reverse order !") self.right_edge = LineString([ Point(p[0], p[1]) for p in self.right_edge.coords[::-1] ]) start_point = Point(pair[0][0], pair[0][1]) break else: l.debug("Original order !") start_point = Point(pair[1][0], pair[1][1]) break assert start_point is not None # At this point compute the driving path of the car (x, y, t) self.driving_path = [current_position] # plt.plot(current_position.x, current_position.y, color='black', marker="x") # # This might not be robust we need to get somethign close by # plt.plot([pair[0][0], pair[1][0]], [pair[0][1], pair[1][1]], marker="o") # plt.plot(projection_point_on_right.x, projection_point_on_right.y, color='b', marker="*") # started = False for right_position in [ Point(p[0], p[1]) for p in list(self.right_edge.coords) ]: if right_position.distance(start_point) < 1.8e-5: # print("Start to log positions") # plt.plot(right_position.x, right_position.y, color='blue', marker="o") started = True if not started: # print("Skip point") # plt.plot(right_position.x, right_position.y, color='red', marker="*") continue else: # print("Consider point") # plt.plot(right_position.x, right_position.y, color='green', marker="o") pass # Project right_position to left_edge projected_point = self.left_edge.interpolate( self.left_edge.project(right_position)) # Translate the right_position 2m toward the center line = LineString([(right_position.x, right_position.y), (projected_point.x, projected_point.y)]) self.driving_path.append(line.interpolate(2.0)) def plot_all(self, car_state): current_position = Point(car_state['pos'][0], car_state['pos'][1]) plt.figure(1, figsize=(5, 5)) plt.clf() ax = plt.gca() x, y = self.left_edge.coords.xy ax.plot(x, y, 'r-') x, y = self.right_edge.coords.xy ax.plot(x, y, 'b-') driving_lane = LineString([p for p in self.driving_path]) x, y = driving_lane.coords.xy ax.plot(x, y, 'g-') # node = { # 'x': target_position.x, # 'y': target_position.y, # 'z': 0.3, # 't': target_time, # } xs = [node['x'] for node in self.script] ys = [node['y'] for node in self.script] # print("{:.2f}".format(3.1415926)); vs = ['{:.2f}'.format(node['avg_speed'] * 3.6) for node in self.script] # plt.plot(xs, ys, marker='.') ax = plt.gca() for i, txt in enumerate(vs): ax.annotate(txt, (xs[i], ys[i])) plt.plot(current_position.x, current_position.y, marker="o", color="green") # Center around current_positions ax.set_xlim([current_position.x - 50, current_position.x + 50]) ax.set_ylim([current_position.y - 50, current_position.y + 50]) plt.draw() plt.pause(0.01) def run(self, debug=False): try: self.vehicle = Vehicle(car_model['id']) electrics = Electrics() self.vehicle.attach_sensor('electrics', electrics) # Connect to running beamng self.bng = BeamNGpy( 'localhost', 64256 ) #, home='C://Users//Alessio//BeamNG.research_unlimited//trunk') self.bng = self.bng.open(launch=False) # Put simulator in pause awaiting while planning the driving self.bng.pause() # Connect to the existing vehicle (identified by the ID set in the vehicle instance) self.bng.set_deterministic() # Set simulator to be deterministic self.bng.connect_vehicle(self.vehicle) assert self.vehicle.skt # Get Initial state of the car. This assumes that the script is invoked after the scenario is started self.bng.poll_sensors(self.vehicle) # Compute the "optimal" driving path and program the ai_script self._compute_driving_path(self.vehicle.state, self.road_model['street']) self.script = self.road_profiler.compute_ai_script( LineString(self.driving_path), self.car_model) # Enforce initial car direction nad up start_dir = (self.vehicle.state['dir'][0], self.vehicle.state['dir'][1], self.vehicle.state['dir'][2]) up_dir = (0, 0, 1) # Configure the ego car self.vehicle.ai_set_mode('disabled') # Note that set script teleports the car by default self.vehicle.ai_set_script(self.script, start_dir=start_dir, up_dir=up_dir) # Resume the simulation self.bng.resume() # At this point the controller can stop ? or wait till it is killed while True: if debug: self.bng.pause() self.bng.poll_sensors(self.vehicle) self.plot_all(self.vehicle.state) self.bng.resume() # Progress the simulation for some time... # self.bng.step(50) sleep(2) except Exception: # When we brutally kill this process there's no need to log an exception l.error("Fatal Error", exc_info=True) finally: self.bng.close()
def run_sim(street_1: DecalRoad): waypoints = [] for node in street_1.nodes: waypoint = BeamNGWaypoint("waypoint_" + str(node), get_node_coords(node)) waypoints.append(waypoint) print(len(waypoints)) maps.beamng_map.generated().write_items( street_1.to_json() + '\n' + "\n".join([waypoint.to_json() for waypoint in waypoints])) beamng = BeamNGpy('localhost', 64256) scenario = Scenario('tig', 'tigscenario') vehicle = Vehicle('ego_vehicle', model='etk800', licence='TIG', color='Red') sim_data_collector = TrainingDataCollectorAndWriter( vehicle, beamng, street_1) scenario.add_vehicle(vehicle, pos=get_node_coords(street_1.nodes[0]), rot=get_rotation(street_1)) scenario.make(beamng) beamng.open() beamng.set_deterministic() beamng.load_scenario(scenario) beamng.pause() beamng.start_scenario() vehicle.ai_drive_in_lane(True) vehicle.ai_set_mode("disabled") vehicle.ai_set_speed(10 / 4) steps = 5 def start(): for waypoint in waypoints[10:-1:20]: vehicle.ai_set_waypoint(waypoint.name) for idx in range(1000): if (idx * 0.05 * steps) > 3.: sim_data_collector.collect_and_write_current_data() dist = distance(sim_data_collector.last_state.pos, waypoint.position) if dist < 15.0: beamng.resume() break # one step is 0.05 seconds (5/100) beamng.step(steps) try: start() finally: beamng.close()