Beispiel #1
0
def test_vehicle_move(beamng):
    with beamng as bng:
        bng.set_deterministic()

        scenario = Scenario('smallgrid', 'move_test')
        vehicle = Vehicle('test_car', model='etk800')
        scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
        scenario.make(bng)
        bng.load_scenario(scenario)
        bng.start_scenario()
        bng.pause()
        vehicle.control(throttle=1)
        bng.step(120)
        vehicle.update_vehicle()
        assert np.linalg.norm(vehicle.state['pos']) > 1

    scenario.delete(beamng)
Beispiel #2
0
def test_vehicle_spawn(beamng):
    scenario = Scenario('smallgrid', 'spawn_test')
    vehicle = Vehicle('irrelevant', model='pickup')
    scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
    scenario.make(beamng)

    with beamng as bng:
        bng.load_scenario(scenario)
        bng.start_scenario()

        other = Vehicle('relevant', model='etk800')
        scenario.add_vehicle(other, pos=(10, 10, 0), rot=(0, 0, 0))
        other.update_vehicle()
        assert 'pos' in other.state
        bng.step(120)
        scenario.remove_vehicle(other)
        bng.step(600)
        assert other.state is None
Beispiel #3
0
def collect_data(beamNG_path):

    bng = BeamNGpy('localhost', 64256, beamNG_path)
    #bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk')

    scenario = Scenario('GridMap', 'example')

    # Create an ETK800 with the licence plate 'PYTHON'
    vehicle1 = Vehicle('ego_vehicle',
                       model='etk800',
                       licence='PYTHON',
                       color='Red')
    #vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue')

    electrics = Electrics()
    vehicle1.attach_sensor('electrics', electrics)

    pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775)
    rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936)
    scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)

    # Place files defining our scenario for the simulator to read
    scenario.make(bng)

    # Launch BeamNG.research
    bng.open(launch=True)
    #SIZE = 50

    try:
        bng.load_scenario(scenario)
        bng.start_scenario()

        #vehicle1.ai_set_speed(10.452066507339481,mode = 'set')
        vehicle1.ai_set_mode('span')

        #collect data

        print("\n Position and rotation of car \n ")
        # for _ in range(200):
        for _ in range(200):
            time.sleep(0.1)
            vehicle1.update_vehicle(
            )  # Synchs the vehicle's "state" variable with the simulator
            sensors = bng.poll_sensors(
                vehicle1
            )  # Polls the data of all sensors attached to the vehicle
            positions.append(vehicle1.state['pos'])
            directions.append(vehicle1.state['dir'])
            print([vehicle1.state['pos'], vehicle1.state['dir']])

        #write data into file
        # print ("position :",positions)
        # print ("position :",directions)
        sys_output.print_title(
            "\n     The position and rotation of the car is saved in \"position.txt and \"roation.txt\" \""
        )
        write_data(FILE_POS, positions)
        write_data(FILE_ROT, directions)

        bng.stop_scenario()
        bng.close()
        time.sleep(2)

    finally:
        bng.close()
    return (positions, directions)
Beispiel #4
0
def run_spawn_for_parts_config(
    vehicle_model='etk800',
    loadfile='C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/front17x8rear17x9.json'
):
    global base_filename, default_color, default_scenario, setpoint
    global prev_error
    global fcam

    # setup DNN model + weights
    m = Model()
    model = m.define_model_BeamNG("BeamNGmodel-5.h5")

    random.seed(1703)
    setup_logging()

    beamng = BeamNGpy('localhost',
                      64256,
                      home='H:/BeamNG.research.v1.7.0.1clean')
    scenario = Scenario(default_scenario, 'research_test')
    vehicle = Vehicle('ego_vehicle',
                      model=vehicle_model,
                      licence='LOWPRESS',
                      color=default_color)
    vehicle = setup_sensors(vehicle)
    spawn = spawn_point(default_scenario, 'highway')
    # scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'])

    # Compile the scenario and place it in BeamNG's map folder
    scenario.make(beamng)

    # Start BeamNG and enter the main loop
    bng = beamng.open(launch=True)

    # bng.hide_hud()
    bng.set_deterministic()  # Set simulator to be deterministic
    bng.set_steps_per_second(100)  # With 60hz temporal resolution

    # Load and start the scenario
    bng.load_scenario(scenario)
    bng.start_scenario()

    # vehicle.load('C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/pristine.json')
    if loadfile:
        bng.spawn_vehicle(vehicle,
                          pos=spawn['pos'],
                          rot=None,
                          rot_quat=spawn['rot_quat'],
                          partConfig=loadfile)
    else:
        bng.spawn_vehicle(vehicle,
                          pos=spawn['pos'],
                          rot=None,
                          rot_quat=spawn['rot_quat'],
                          partConfig='vehicles/hopper/custom.pc')
    #bng.set_relative_camera()

    # Put simulator in pause awaiting further inputs
    bng.pause()
    assert vehicle.skt
    bng.resume()
    bng.set_steps_per_second(100)
    bng.set_deterministic()
    totalsecs = 0.0
    pitch_traj = []
    while totalsecs <= 30:
        vehicle.update_vehicle()
        camera_state = bng.poll_sensors(vehicle)['front_cam']
        camera_state['roll'] = vehicle.state['roll']
        camera_state['pitch'] = vehicle.state['pitch']
        camera_state['yaw'] = vehicle.state['yaw']
        pitch_traj.append(round(math.degrees(vehicle.state['pitch'][0]), 4))
        print("roll:{}, pitch:{}, yaw:{}".format(vehicle.state['roll'],
                                                 vehicle.state['pitch'],
                                                 vehicle.state['yaw']))
        bng.step(100)
        totalsecs += 1
    # print("Camera state:{}".format(camera_state))
    image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
    #get_camera_rot_and_pos([-0.3, 1, 1.0], [0, 0.75, 0], before_state['pos'], before_state['dir'], before_state['up'])
    img = m.process_image(image)
    before_prediction = model.predict(img)
    camera_state["prediction"] = before_prediction
    plt.imshow(image)
    plt.pause(0.01)
    bng.close()
    return camera_state, vehicle.state, pitch_traj
car_1 = Vehicle('car_1', model='etk800', licence='CAR 1', colour='Blue')
scenario.add_vehicle(car_1, pos=(-140, -121.233, 119.586), rot=(0, 0, 55))

scenario.make(beamng)
bng = beamng.open(launch=True)
bng.load_scenario(scenario)
bng.start_scenario()

vut.ai_set_mode('span')
vut.ai_drive_in_lane(True)

car_1.ai_set_line([{'pos': (-198.5, -164.189, 119.7), 'speed': 2000}])

for _ in range(240):
    sleep(0.1)
    vut.update_vehicle()
    sensors = bng.poll_sensors(vut)
    dmg = sensors['damage']

    # Below code snippet is generated form 'detect_obstacle_car' function for car_1
    scenario.update()
    dist_car_1 = np.linalg.norm(
        np.array(vut.state['pos']) - np.array(car_1.state['pos']))

    if dist_car_1 < 8:
        print('Car Detected')

        # Below code snippet is generated form 'ai_stopped' function for car_1
        scenario.update()
        if sensors['electrics']['values']['wheelspeed'] == 0 or dmg[
                'damage'] == 0:
Beispiel #6
0
import mmap
Beispiel #7
0
from beamngpy import BeamNGpy, Vehicle, Scenario, Road
from beamngpy.sensors import Camera
from getAIScript import getAIScript
beamng = BeamNGpy('localhost', 64256, getBeamngDirectory())

scenario = Scenario('smallgrid', 'vehicle_bbox_example')
road = Road('track_editor_A_center', rid='main_road')
orig = (0, -2, 0)
goal = (1150, -22, 0)
nodes = [(*orig, 7), (*goal, 7)]
road.nodes.extend(nodes)
scenario.add_road(road)

vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON')
overhead = Camera((0, -10, 5), (0, 1, -0.75), 60, (1024, 1024))
vehicle.attach_sensor('overhead', overhead)
scenario.add_vehicle(vehicle, pos=orig, rot=(0, 0, -90))

scenario.make(beamng)
bng = beamng.open(launch=True)
try:
    bng.load_scenario(scenario)
    bng.start_scenario()
    script = getAIScript()
    vehicle.ai_set_script(script)
    while (True):
        vehicle.update_vehicle()
        print(vehicle.state['pos'])
    input()
finally:
    bng.close()
Beispiel #8
0
def collection_run(speed=11, risk=0.6, num_samples=10000):
    global base_filename, training_dir, default_model, setpoint
    global spawnpoint, steps_per_second

    f = setup_dir(training_dir)
    spawn_pt = spawn_point(default_scenario, spawnpoint)
    random.seed(1703)
    setup_logging()

    home = 'H:/BeamNG.research.v1.7.0.1clean'  #'H:/BeamNG.tech.v0.21.3.0' #
    beamng = BeamNGpy('localhost', 64256, home=home, user='******')
    scenario = Scenario(default_scenario, 'research_test')
    # add barriers and cars to get the ego vehicle to avoid the barriers
    add_barriers(scenario)
    # add_barrier_cars(scenario)
    vehicle = Vehicle('ego_vehicle',
                      model=default_model,
                      licence='DATAMKR',
                      color='White')
    vehicle = setup_sensors(vehicle)
    scenario.add_vehicle(vehicle,
                         pos=spawn_pt['pos'],
                         rot=None,
                         rot_quat=spawn_pt['rot_quat'])
    # Compile the scenario and place it in BeamNG's map folder
    scenario.make(beamng)
    # Start BeamNG and enter the main loop
    bng = beamng.open(launch=True)
    # bng.hide_hud()
    bng.set_nondeterministic()  # Set simulator to be deterministic
    bng.set_steps_per_second(steps_per_second)  #
    # Load and start the scenario
    bng.load_scenario(scenario)
    bng.start_scenario()
    bng.switch_vehicle(vehicle)

    # ai_line, bng = create_ai_line_from_road(spawn_pt, bng)
    ai_line, bng = create_ai_line_from_road_with_interpolation(spawn_pt, bng)

    # Put simulator in pause awaiting further inputs
    bng.pause()
    assert vehicle.skt
    # bng.resume()
    start_time = time.time()
    # Send random inputs to vehicle and advance the simulation 20 steps
    imagecount = 0
    timer = 0
    traj = []
    steering_inputs = []
    timestamps = []
    kphs = []
    with open(f, 'w') as datafile:
        datafile.write(
            'filename,timestamp,steering_input,throttle_input,brake_input,driveshaft,engine_load,fog_lights,fuel,'
            'lowpressure,oil,oil_temperature,parkingbrake,rpm,water_temperature,wheelspeed\n'
        )
        return_str = ''
        while imagecount < num_samples:
            sensors = bng.poll_sensors(vehicle)
            image = sensors['front_cam']['colour'].convert('RGB')
            full_filename = "{}{}{}.jpg".format(training_dir, base_filename,
                                                imagecount)
            qualified_filename = "{}{}.jpg".format(base_filename, imagecount)
            steering = sensors['electrics']['steering_input']
            dt = sensors['timer']['time'] - timer
            if dt > 0:
                throttle = throttle_PID(
                    sensors['electrics']['wheelspeed'] * 3.6, dt)
            # print("current kph:{} dt:{} throttle:{}".format(sensors['electrics']['wheelspeed'] * 3.6, dt, throttle))
            vehicle.update_vehicle()
            # points=[vehicle.state['front']]
            # point_colors = [[0, 1, 0, 0.1]]
            # spheres=[[vehicle.state['front'][0],vehicle.state['front'][1],vehicle.state['front'][1],0.25]]
            # sphere_colors=[[1, 0, 0, 0.8]]
            # bng.add_debug_line(points, point_colors,
            #                    spheres=spheres, sphere_colors=sphere_colors,
            #                    cling=True, offset=0.1)
            try:
                steering_setpt = steering_setpoint(vehicle.state, traj)
                if dt > 0:
                    steering = steering_PID(steering, steering_setpt, dt)
                vehicle.control(steering=steering, throttle=throttle)
                timer = sensors['timer']['time']
                # traj.append(vehicle.state['pos'])
                # timestamps.append(timer)
                # steering_inputs.append(steering)
                # kphs.append(sensors['electrics']['wheelspeed'] * 3.6)
            except IndexError as e:
                print(e, "exiting...")
                print(e.with_traceback())
                plot_trajectory(traj, "Car Behavior using AI Script")
                plot_inputs(timestamps,
                            steering_inputs,
                            title="Steering Inputs by Time")
                plot_inputs(timestamps, kphs, title="KPHs by Time")
                exit(0)

            if timer > 10:  # and abs(steering) >= 0.1:
                # collect ancillary data
                datafile.write(
                    '{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format(
                        qualified_filename,
                        str(round(sensors['timer']['time'],
                                  2)), sensors['electrics']['steering_input'],
                        sensors['electrics']['throttle_input'],
                        sensors['electrics']['brake_input'],
                        sensors['electrics']['driveshaft'],
                        sensors['electrics']['engine_load'],
                        sensors['electrics']['fog_lights'],
                        sensors['electrics']['fuel'],
                        sensors['electrics']['lowpressure'],
                        sensors['electrics']['oil'],
                        sensors['electrics']['oil_temperature'],
                        sensors['electrics']['parkingbrake'],
                        sensors['electrics']['rpm'],
                        sensors['electrics']['water_temperature'],
                        sensors['electrics']['wheelspeed']))
                # save the image
                image.save(full_filename)
                imagecount += 1
                print(f"{imagecount=}")

            if sensors['damage']['damage'] > 0:
                return_str = "CRASHED at timestep {} speed {}; QUITTING".format(
                    round(sensors['timer']['time'], 2),
                    round(sensors['electrics']['wheelspeed'] * 3.6, 3))
                print(return_str)
                break
            # if timer > 300:
            #     break

            bng.step(1, wait=True)
    plot_trajectory(traj, "Car Behavior using AI Script")
    plot_inputs(timestamps, steering_inputs, title="Steering Inputs by Time")
    plot_inputs(timestamps, kphs, title="KPHs by Time")
    bng.close()
    plot_trajectory(traj, "Car Behavior using AI Script")
    return return_str
Beispiel #9
0
def main():
    global default_model, default_scenario
    beamng = BeamNGpy('localhost',
                      64256,
                      home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')

    #scenario = Scenario('smallgrid', 'spawn_objects_example')
    scenario = Scenario(default_scenario,
                        'research_test',
                        description='Random driving for research')

    vehicle = Vehicle('ego_vehicle', model=default_model, licence='PYTHON')
    vehicle = setup_sensors(vehicle)
    spawn = spawn_point(default_scenario)
    scenario.add_vehicle(vehicle,
                         pos=spawn['pos'],
                         rot=spawn['rot'],
                         rot_quat=spawn['rot_quat'])

    scenario.make(beamng)

    bng = beamng.open()
    bng.load_scenario(scenario)
    bng.start_scenario()

    vehicle.update_vehicle()

    d1 = bng.poll_sensors(vehicle)
    cum_list = []
    bound = 0.0
    for i in range(3):
        for _ in range(45):
            bound = bound + 0.0  # 0.1
            # vehicle.save()
            vehicle.update_vehicle()
            d2 = bng.poll_sensors(vehicle)
            throttle = 1.0
            #throttle = random.uniform(0.0, 1.0)
            steering = random.uniform(-1 * bound, bound)
            brake = 0.0  #random.choice([0, 0, 0, 1])
            vehicle.control(throttle=throttle, steering=steering, brake=brake)
            pointName = "{}_{}".format(i, _)
            cum_list.append(pointName)
            vehicle.saveRecoveryPoint(pointName)
            bng.step(20)
        print("SEGMENT #{}: COMPARE DAMAGE".format(i))
        damage_diff = compare_damage(d1, d2)
        d1 = d2
        # "Back up" 1 second -- load vehicle at that time in that position.
        backup_pointName = backup(cum_list, 0.001)
        print('recovering to {}'.format(pointName))
        loadfile = vehicle.loadRecoveryPoint(backup_pointName)
        print('loadfile is {}'.format(loadfile))
        bng.pause()
        vehicle.update_vehicle()
        vehicle.load(loadfile)
        #vehicle.load("vehicles/pickup/vehicle.save.json")
        bng.resume()
        #vehicle.startRecovering()
        #time.sleep(1.5)
        #vehicle.stopRecovering()

    vehicle.update_vehicle()
    bng.pause()
    time.sleep(2)
    # vehicle.load("vehicles/pickup/vehicle.save.json")
    bng.resume()
    bng.close()
def run_scenario(vehicle_model='etk800',
                 deflation_pattern=[0, 0, 0, 0],
                 parts_config='vehicles/hopper/custom.pc',
                 run_number=0):
    global base_filename, default_color, default_scenario, default_spawnpoint, steps_per_sec
    global integral, prev_error, setpoint
    integral = 0.0
    prev_error = 0.0

    # setup DNN model + weights
    sm = DAVE2Model()
    # steering_model = Model().define_model_BeamNG("BeamNGmodel-racetracksteering8.h5")
    # throttle_model = Model().define_model_BeamNG("BeamNGmodel-racetrackthrottle8.h5")
    dual_model = sm.define_dual_model_BeamNG()
    # dual_model = sm.load_weights("BeamNGmodel-racetrackdualcomparison10K.h5")
    # dual_model = sm.define_multi_input_model_BeamNG()
    # dual_model = sm.load_weights("BeamNGmodel-racetrackdual-comparison10K-PIDcontrolset-4.h5")
    # dual_model = sm.load_weights("BeamNGmodel-racetrackdual-comparison40K-PIDcontrolset-1.h5")
    # BeamNGmodel-racetrack-multiinput-dualoutput-comparison10K-PIDcontrolset-1.h5
    # BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-2
    dual_model = sm.load_weights(
        "BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-2.h5")
    # dual_model = sm.load_weights("BeamNGmodel-racetrack-multiinput-dualoutput-comparison103K-PIDcontrolset-1.h5")

    random.seed(1703)
    setup_logging()

    beamng = BeamNGpy('localhost',
                      64256,
                      home='H:/BeamNG.research.v1.7.0.1clean',
                      user='******')
    scenario = Scenario(default_scenario, 'research_test')
    vehicle = Vehicle('ego_vehicle',
                      model=vehicle_model,
                      licence='EGO',
                      color=default_color)
    vehicle = setup_sensors(vehicle)
    spawn = spawn_point(default_scenario, default_spawnpoint)
    scenario.add_vehicle(
        vehicle, pos=spawn['pos'], rot=None,
        rot_quat=spawn['rot_quat'])  #, partConfig=parts_config)
    add_barriers(scenario)
    # Compile the scenario and place it in BeamNG's map folder
    scenario.make(beamng)

    # Start BeamNG and enter the main loop
    bng = beamng.open(launch=True)
    #bng.hide_hud()
    bng.set_deterministic()  # Set simulator to be deterministic
    bng.set_steps_per_second(steps_per_sec)  # With 60hz temporal resolution
    bng.load_scenario(scenario)
    bng.start_scenario()
    # bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config)
    # Put simulator in pause awaiting further inputs
    bng.pause()
    assert vehicle.skt
    # bng.resume()

    # perturb vehicle
    print("vehicle position before deflation via beamstate:{}".format(
        vehicle.get_object_position()))
    print("vehicle position before deflation via vehicle state:{}".format(
        vehicle.state))
    image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
    # plt.imshow(image)
    # plt.pause(0.01)
    vehicle.deflate_tires(deflation_pattern)
    bng.step(steps_per_sec * 6)
    vehicle.update_vehicle()
    # print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position()))
    # print("vehicle position after deflation via vehicle state:{}".format(vehicle.state))
    pitch = vehicle.state['pitch'][0]
    roll = vehicle.state['roll'][0]
    z = vehicle.state['pos'][2]
    image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
    # plt.imshow(image)
    # plt.pause(0.01)

    # bng.resume()
    # vehicle.break_all_breakgroups()
    # vehicle.break_hinges()

    wheelspeed = 0.0
    throttle = 0.0
    prev_error = setpoint
    damage_prev = None
    runtime = 0.0
    kphs = []
    traj = []
    pitches = []
    rolls = []
    steering_inputs = []
    throttle_inputs = []
    timestamps = []
    damage = None
    final_img = None
    # Send random inputs to vehice and advance the simulation 20 steps
    overall_damage = 0.0
    total_loops = 0
    total_imgs = 0
    total_predictions = 0
    while overall_damage <= 0:
        # collect images
        sensors = bng.poll_sensors(vehicle)
        image = sensors['front_cam']['colour'].convert('RGB')
        # plt.imshow(image)
        # plt.pause(0.01)
        total_imgs += 1
        img = sm.process_image(np.asarray(image))
        wheelspeed = sensors['electrics']['wheelspeed']
        kph = ms_to_kph(wheelspeed)
        dual_prediction = dual_model.predict(x=[img, np.array([kph])])
        # steering_prediction = steering_model.predict(img)
        # throttle_prediction = throttle_model.predict(img)
        dt = sensors['timer']['time'] - runtime
        runtime = sensors['timer']['time']

        # control params

        brake = 0
        # steering = float(steering_prediction[0][0]) #random.uniform(-1.0, 1.0)
        # throttle = float(throttle_prediction[0][0])
        steering = float(dual_prediction[0][0])  #random.uniform(-1.0, 1.0)
        throttle = float(dual_prediction[0][1])
        total_predictions += 1
        if abs(steering) > 0.2:
            setpoint = 20
        else:
            setpoint = 40
        # if runtime < 10:
        throttle = throttle_PID(kph, dt)
        #     if throttle > 1:
        #         throttle = 1
        # if setpoint < kph:
        #     brake = 0.0 #throttle / 10000.0
        #     throttle = 0.0
        vehicle.control(throttle=throttle, steering=steering, brake=brake)

        steering_inputs.append(steering)
        throttle_inputs.append(throttle)
        timestamps.append(runtime)

        steering_state = sensors['electrics']['steering']
        steering_input = sensors['electrics']['steering_input']
        avg_wheel_av = sensors['electrics']['avg_wheel_av']

        damage = sensors['damage']
        overall_damage = damage["damage"]
        new_damage = diff_damage(damage, damage_prev)
        damage_prev = damage
        vehicle.update_vehicle()
        traj.append(vehicle.state['pos'])
        pitches.append(vehicle.state['pitch'][0])
        rolls.append(vehicle.state['roll'][0])

        kphs.append(ms_to_kph(wheelspeed))
        total_loops += 1

        if new_damage > 0.0:
            final_img = image
            break
        bng.step(1, wait=True)

        if runtime > 300:
            print("Exited after 5 minutes successful runtime")
            break

        if distance(
                spawn['pos'],
                vehicle.state['pos']) < 5 and sensors['timer']['time'] > 10:
            reached_start = True
            break

    #     print("runtime:{}".format(round(runtime, 2)))
    # print("time to crash:{}".format(round(runtime, 2)))
    bng.close()
    # avg_kph = float(sum(kphs)) / len(kphs)
    plt.imshow(final_img)
    plt.savefig("Run-{}-finalimg.png".format(run_number))
    plt.pause(0.01)
    plot_input(timestamps, steering_inputs, "Steering", run_number=run_number)
    plot_input(timestamps, throttle_inputs, "Throttle", run_number=run_number)
    plot_input(timestamps, kphs, "KPHs", run_number=run_number)
    print("Number of steering_inputs:", len(steering_inputs))
    print("Number of throttle inputs:", len(throttle_inputs))
    results = "Total loops: {} \ntotal images: {} \ntotal predictions: {} \nexpected predictions ={}*{}={}".format(
        total_loops, total_imgs, total_predictions, round(runtime, 3),
        steps_per_sec, round(runtime * steps_per_sec, 3))
    print(results)
    results = {
        'runtime': round(runtime, 3),
        'damage': damage,
        'kphs': kphs,
        'traj': traj,
        'pitch': round(pitch, 3),
        'roll': round(roll, 3),
        "z": round(z, 3),
        'final_img': final_img,
        'total_predictions': total_predictions,
        'expected_predictions': round(runtime * steps_per_sec, 3)
    }
    return results
Beispiel #11
0
def main():
    random.seed(1703)
    setup_logging()
    beamng = BeamNGpy('localhost',
                      64256,
                      home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')

    state_cfg = Config()
    parts_cfg = Config()
    # Create a scenario in west_coast_usa

    scenario = Scenario('west_coast_usa',
                        'research_test',
                        description='Random driving for research')

    # Set up first vehicle, with two cameras, gforces sensor, lidar, electrical
    # sensors, and damage sensors
    vehicle = Vehicle('ego_vehicle',
                      model='etk800',
                      licence='RED',
                      color='Red')
    vehicle = setup_sensors(vehicle)

    scenario.add_vehicle(vehicle,
                         pos=(-717.121, 101, 118.675),
                         rot=None,
                         rot_quat=(0, 0, 0.3826834, 0.9238795))

    # Compile the scenario and place it in BeamNG's map folder
    #scenario.make(beamng)

    # Start BeamNG and enter the main loop
    bng = beamng.open(launch=True)
    try:
        #bng.hide_hud()
        bng.set_deterministic()  # Set simulator to be deterministic
        bng.set_steps_per_second(60)  # With 60hz temporal resolution

        # Load and start the scenario
        bng.load_scenario(scenario)
        bng.start_scenario()
        # Put simulator in pause awaiting further inputs
        bng.pause()
        pcfg = vehicle.get_part_config()
        assert vehicle.skt

        # Send random inputs to vehicle and advance the simulation 20 steps
        #for _ in range(1024):
        for _ in range(30):
            throttle = random.uniform(0.0, 1.0)
            steering = random.uniform(-1.0, 1.0)
            brake = random.choice([0, 0, 0, 1])
            vehicle.control(throttle=throttle, steering=steering, brake=brake)

            # bng.step(20)
            bng.step(20)

            # Retrieve sensor data and show the camera data.
            sensors = bng.poll_sensors(vehicle)

            print('\n{} seconds passed.'.format(sensors['timer']['time']))
            print("step in loop {}".format(_))

    finally:
        sensors = bng.poll_sensors(vehicle)
        for s in sensors.keys():
            print("{} : {}".format(s, sensors[s]))
        damage_dict = sensors['damage']
        state_cfg.update(sensors['damage'])
        state_cfg.update(vehicle.state)
        state_cfg.save('{}/state_cfg.json'.format(beamng.home))
        vehicle.annotate_parts()
        vehicle.update_vehicle()
        #part_options = vehicle.get_part_options()
        parts_cfg_dict = vehicle.get_part_config()

        parts_cfg.load_values(vehicle.get_part_config())
        #parts_cfg.update(vehicle.get_part_config())
        parts_cfg.save('{}/parts_cfg.json'.format(beamng.home))
        vehicle.save()
        bng.close()

    # reload scenario with saved config
    random.seed(1703)
    setup_logging()
    beamng = BeamNGpy('localhost',
                      64256,
                      home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
    loaded_config = state_cfg.load("{}/state_cfg.json".format(beamng.home))
    scenario = Scenario('west_coast_usa',
                        'research_test',
                        description='Random driving for research')
    vehicle = Vehicle('ego_vehicle',
                      model='etk800',
                      licence='RED',
                      color='Red')
    vehicle = setup_sensors(vehicle)
    vehicle.set_part_config(parts_cfg)

    scenario.add_vehicle(vehicle,
                         pos=tuple(state_cfg.pos),
                         rot=None,
                         rot_quat=beamngpy.angle_to_quat(state_cfg.dir))
    scenario.make(beamng)
    bng = beamng.open(launch=True)
    bng.spawn_vehicle(vehicle,
                      pos=tuple(state_cfg.pos),
                      rot=None,
                      rot_quat=beamngpy.angle_to_quat(state_cfg.dir))
    # TODO: debug scenario restart
    # scenario.restart()

    try:
        bng.hide_hud()
        bng.set_deterministic()  # Set simulator to be deterministic
        bng.set_steps_per_second(60)  # With 60hz temporal resolution

        # Load and start the scenario
        bng.load_scenario(scenario)
        bng.start_scenario()
        # Put simulator in pause awaiting further inputs
        bng.pause()

        assert vehicle.skt
        vehicle.load()
        # Send random inputs to vehicle and advance the simulation 20 steps
        #for _ in range(1024):
        for _ in range(30):
            throttle = random.uniform(0.0, 1.0)
            steering = random.uniform(-1.0, 1.0)
            brake = random.choice([0, 0, 0, 1])
            vehicle.control(throttle=throttle, steering=steering, brake=brake)

            # bng.step(20)
            bng.step(20)

            # Retrieve sensor data and show the camera data.
            sensors = bng.poll_sensors(vehicle)

            print('\n{} seconds passed.'.format(sensors['timer']['time']))
            print("step in loop {}".format(_))

    finally:
        sensors = bng.poll_sensors(vehicle)
        for s in sensors.keys():
            print("{} : {}".format(s, sensors[s]))
        state_cfg.update(sensors['damage'])
        state_cfg.update(vehicle.state)
        state_cfg.save('{}/state_cfg.json'.format(beamng.home))
        parts_cfg.update(vehicle.get_part_config())
        parts_cfg.save('{}/parts_cfg.json'.format(beamng.home))
        bng.close()
def run_scenario_ai_version(vehicle_model='etk800',
                            deflation_pattern=[0, 0, 0, 0],
                            parts_config='vehicles/hopper/custom.pc'):
    global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec, prev_error

    random.seed(1703)
    setup_logging()

    home = 'H:/BeamNG.research.v1.7.0.1clean'  #'H:/BeamNG.research.v1.7.0.1untouched/BeamNG.research.v1.7.0.1' #'H:/BeamNG.tech.v0.21.3.0' #
    beamng = BeamNGpy('localhost', 64256,
                      home=home)  #, user='******')
    scenario = Scenario(default_scenario, 'research_test')
    vehicle = Vehicle('ego_vehicle',
                      model=vehicle_model,
                      licence='AI',
                      color=default_color)
    vehicle = setup_sensors(vehicle)
    spawn = spawn_point(default_scenario, default_spawnpoint)
    scenario.add_vehicle(vehicle,
                         pos=spawn['pos'],
                         rot=None,
                         rot_quat=spawn['rot_quat'])

    # Compile the scenario and place it in BeamNG's map folder
    scenario.make(beamng)
    # Start BeamNG and enter the main loop
    bng = beamng.open(launch=True)
    bng.set_deterministic()  # Set simulator to be deterministic
    bng.set_steps_per_second(steps_per_sec)  # With 100hz temporal resolution
    # Load and start the scenario
    bng.load_scenario(scenario)
    bng.start_scenario()

    # create vehicle to be chased
    # chase_vehicle = Vehicle('chase_vehicle', model='miramar', licence='CHASEE', color='Red')
    # bng.spawn_vehicle(chase_vehicle, pos=(469.784, 346.391, 144.982), rot=None,
    #                   rot_quat=(-0.0037852677050978, -0.0031219546217471, -0.78478640317917, 0.61974692344666))
    # bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config)

    # Put simulator in pause awaiting further inputs
    bng.pause()
    assert vehicle.skt
    bng.resume()
    ai_line, bng = create_ai_line_from_road(spawn, bng)
    # ai_line, bng = create_ai_line_from_centerline(bng)
    # ai_line, bng = create_ai_line(bng)
    vehicle.ai_set_script(ai_line, cling=True)

    pitch = vehicle.state['pitch'][0]
    roll = vehicle.state['roll'][0]
    z = vehicle.state['pos'][2]
    image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
    # bng.resume()

    # vehicle.ai_set_mode('chase')
    # vehicle.ai_set_target('chase_vehicle')
    # vehicle.ai_set_mode("traffic")
    # vehicle.ai_set_speed(12, mode='set')
    # vehicle.ai_drive_in_lane(True)

    damage_prev = None
    runtime = 0.0
    traj = []
    kphs = []
    # with open("ai_lap_data.txt", 'w') as f:
    for _ in range(1024):
        sensors = bng.poll_sensors(vehicle)
        image = sensors['front_cam']['colour'].convert('RGB')
        damage = sensors['damage']
        wheelspeed = sensors['electrics']['wheelspeed']
        new_damage = diff_damage(damage, damage_prev)
        damage_prev = damage
        runtime = sensors['timer']['time']
        vehicle.update_vehicle()
        traj.append(vehicle.state['pos'])
        # f.write("{}\n".format(vehicle.state['pos']))
        kphs.append(ms_to_kph(wheelspeed))
        if new_damage > 0.0:
            break
        # if distance(spawn['pos'], vehicle.state['pos']) < 3 and sensors['timer']['time'] > 90:
        #     reached_start = True
        #     plt.imshow(image)
        #     plt.show()
        #     break
        bng.step(1)
    bng.close()
    plot_trajectory(traj, "AI Lap")
    results = {
        'runtime': round(runtime, 3),
        'damage': damage,
        'kphs': kphs,
        'traj': traj,
        'pitch': round(pitch, 3),
        'roll': round(roll, 3),
        "z": round(z, 3),
        'final_img': image
    }
    return results
def run_scenario(vehicle_model='etk800',
                 deflation_pattern=[0, 0, 0, 0],
                 parts_config=None):
    global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec
    global integral, prev_error
    integral = 0.0
    prev_error = 0.0

    # setup DNN model + weights
    # m = Model()
    # model = m.define_model_BeamNG("BeamNGmodel-racetrack.h5")

    random.seed(1703)
    setup_logging()

    beamng = BeamNGpy('localhost',
                      64256,
                      home='H:/BeamNG.research.v1.7.0.1clean')
    scenario = Scenario(default_scenario, 'research_test')
    # unperturbed_vehicle = Vehicle('unperturbed_vehicle', model=vehicle_model,
    #                   licence='SAFE', color='Red')
    # unperturbed_vehicle = setup_sensors(unperturbed_vehicle)
    vehicle = Vehicle('ego_vehicle',
                      model=vehicle_model,
                      licence='EGO',
                      color=default_color)
    vehicle = setup_sensors(vehicle)
    spawn = get_spawn_point(default_scenario, default_spawnpoint)
    scenario.add_vehicle(vehicle,
                         pos=spawn['pos'],
                         rot=None,
                         rot_quat=spawn['rot_quat'])
    temp = copy.deepcopy(spawn['pos'])
    temp = [temp[0] + lanewidth, temp[1] + lanewidth, temp[2]]
    # scenario.add_vehicle(unperturbed_vehicle, pos=temp, rot=None, rot_quat=spawn['rot_quat'])

    # Compile the scenario and place it in BeamNG's map folder
    scenario.make(beamng)
    bng = beamng.open(launch=True)

    #bng.hide_hud()
    bng.set_deterministic()  # Set simulator to be deterministic
    bng.set_steps_per_second(steps_per_sec)

    # Load and start the scenario
    bng.load_scenario(scenario)
    bng.start_scenario()
    #bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config)

    # Put simulator in pause awaiting further inputs
    bng.pause()
    assert vehicle.skt
    bng.resume()

    # perturb vehicle
    vehicle.deflate_tires(deflation_pattern)
    bng.step(steps_per_sec * 6)
    vehicle.update_vehicle()
    damage = 0
    runtime = 0
    while damage <= 0:

        sensors = bng.poll_sensors(vehicle)
        headlight_img = sensors['headlight_cam']['colour'].convert('RGB')
        # sensors['']
        # prediction = model.predict()
        # steering = float(prediction[0][0]) #random.uniform(-1.0, 1.0)
        # vehicle.control(throttle=throttle, steering=steering, brake=brake)
        return_str = '\nPERTURBED headlight_cam INFO:'
        print('\nPERTURBED headlight_cam INFO:')
        temp = bng.poll_sensors(vehicle)['headlight_cam']
        for key in temp:
            if key == 'rotation':
                degs = euler_from_quaternion(temp[key][0], temp[key][1],
                                             temp[key][2], temp[key][3])
                return_str = "{}\nquaternions {}".format(return_str, temp[key])
                return_str = "{}\n{} {}".format(return_str, key,
                                                [round(i, 3) for i in degs])
                print(key, degs)
            elif key != "colour" and key != "annotation" and key != "depth":
                return_str = "{}\n{} {}".format(return_str, key, temp[key])
                print(key, temp[key])
        print('\nPERTURBED rearview_cam INFO:')
        return_str = "{}\nPERTURBED rearview_cam INFO:".format(return_str)
        # temp = bng.poll_sensors(vehicle)['rearview_cam']
        for key in temp:
            if key == 'rotation':
                degs = euler_from_quaternion(temp[key][0], temp[key][1],
                                             temp[key][2], temp[key][3])
                return_str = "{}\nquaternions {}".format(return_str, temp[key])
                return_str = "{}\n{} {}".format(return_str, key,
                                                [round(i, 3) for i in degs])
                print(key, degs)
            elif key != "colour" and key != "annotation" and key != "depth":
                return_str = "{}\n{} {}".format(return_str, key, temp[key])
                print(key, temp[key])
        # rearview_img = bng.poll_sensors(vehicle)['rearview_cam']['colour'].convert('RGB')
        headlight_img = sensors['headlight_cam']['colour'].convert('RGB')
        bng.step(1)
        damage = sensors['damage']['damage']
        runtime = sensors['timer']['time']

    #     print("runtime:{}".format(round(runtime, 2)))
    print("time to crash:{} damage:{}".format(round(runtime, 2), damage))
    bng.close()
    # avg_kph = float(sum(kphs)) / len(kphs)
    # plt.imshow(rearview_img)
    # plt.pause(0.01)
    plt.imshow(headlight_img)
    plt.pause(0.01)
    # results = {'pitch': round(pitch,3), 'roll':round(roll,3), "z":round(z,3), 'rearview_img':rearview_img, 'headlight_img':headlight_img}
    return return_str
def run_scenario_ai_version(vehicle_model='etk800',
                            deflation_pattern=[0, 0, 0, 0],
                            parts_config=None):
    global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec
    global prev_error

    random.seed(1703)
    setup_logging()

    beamng = BeamNGpy('localhost',
                      64256,
                      home='H:/BeamNG.research.v1.7.0.1clean',
                      user="******")
    scenario = Scenario(default_scenario, 'research_test')
    vehicle = Vehicle('ego_vehicle',
                      model=vehicle_model,
                      licence='AI',
                      color=default_color)
    vehicle = setup_sensors(vehicle)
    spawn = get_spawn_point(default_scenario, default_spawnpoint)
    # scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'])

    # Compile the scenario and place it in BeamNG's map folder
    scenario.make(beamng)

    # Start BeamNG and enter the main loop
    bng = beamng.open(launch=True)

    bng.set_deterministic()  # Set simulator to be deterministic
    bng.set_steps_per_second(steps_per_sec)  # With 100hz temporal resolution

    # Load and start the scenario
    bng.load_scenario(scenario)
    bng.start_scenario()

    # create vehicle to be chased
    chase_vehicle = Vehicle('chase_vehicle',
                            model='miramar',
                            licence='CHASEE',
                            color='Red')
    bng.spawn_vehicle(chase_vehicle,
                      pos=(469.784, 346.391, 144.982),
                      rot=None,
                      rot_quat=(-0.0037852677050978, -0.0031219546217471,
                                -0.78478640317917, 0.61974692344666))

    bng.spawn_vehicle(vehicle,
                      pos=spawn['pos'],
                      rot=None,
                      rot_quat=spawn['rot_quat'],
                      partConfig=parts_config)

    # Put simulator in pause awaiting further inputs
    bng.pause()
    assert vehicle.skt
    #bng.resume()

    # perturb vehicle
    print("vehicle position before deflation via beamstate:{}".format(
        vehicle.get_object_position()))
    print("vehicle position before deflation via vehicle state:{}".format(
        vehicle.state))
    image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
    plt.imshow(image)
    plt.pause(0.01)
    vehicle.deflate_tires(deflation_pattern)
    bng.step(steps_per_sec * 6)
    vehicle.update_vehicle()
    # print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position()))
    # print("vehicle position after deflation via vehicle state:{}".format(vehicle.state))
    pitch = vehicle.state['pitch'][0]
    roll = vehicle.state['roll'][0]
    z = vehicle.state['pos'][2]
    image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
    plt.imshow(image)
    plt.pause(0.01)
    bng.resume()

    vehicle.ai_set_mode('chase')
    vehicle.ai_set_target('chase_vehicle')
    vehicle.ai_drive_in_lane(True)
    damage_prev = None
    runtime = 0.0
    traj = []
    kphs = []
    for _ in range(650):
        image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
        damage = bng.poll_sensors(vehicle)['damage']
        wheelspeed = bng.poll_sensors(vehicle)['electrics']['wheelspeed']
        new_damage = diff_damage(damage, damage_prev)
        damage_prev = damage
        runtime = bng.poll_sensors(vehicle)['timer']['time']
        vehicle.update_vehicle()
        traj.append(vehicle.state['pos'])
        kphs.append(ms_to_kph(wheelspeed))
        if new_damage > 0.0:
            break
        bng.step(5)
    bng.close()
    results = {
        'runtime': round(runtime, 3),
        'damage': damage,
        'kphs': kphs,
        'traj': traj,
        'pitch': round(pitch, 3),
        'roll': round(roll, 3),
        "z": round(z, 3),
        'final_img': image
    }
    return results
Beispiel #15
0
def test_vehicle_ai(beamng):
    with beamng as bng:
        bng.set_deterministic()

        scenario = Scenario('west_coast_usa', 'ai_test')
        vehicle = Vehicle('test_car', model='etk800')
        other = Vehicle('other', model='etk800')
        pos = [-717.121, 101, 118.675]
        scenario.add_vehicle(vehicle, pos=pos, rot=(0, 0, 45))
        scenario.add_vehicle(other, pos=(-453, 700, 75), rot=(0, 0, 45))
        scenario.make(bng)

        bng.load_scenario(scenario)

        bng.start_scenario()
        bng.pause()

        vehicle.ai_set_mode('span')
        assert_continued_movement(bng, vehicle, pos)

        bng.restart_scenario()
        bng.pause()

        vehicle.ai_set_waypoint('Bridge4_B')
        assert_continued_movement(bng, vehicle, pos)

        bng.restart_scenario()
        bng.pause()

        vehicle.ai_set_target('other', mode='chase')
        assert_continued_movement(bng, vehicle, pos)

        bng.restart_scenario()
        bng.pause()

        vehicle.ai_set_target('other', mode='flee')
        assert_continued_movement(bng, vehicle, pos)

        bng.restart_scenario()
        bng.pause()

        script = [
            {
                'x': -735,
                'y': 86.7,
                'z': 119,
                't': 0
            },
            {
                'x': -752,
                'y': 70,
                'z': 119,
                't': 5
            },
            {
                'x': -762,
                'y': 60,
                'z': 119,
                't': 8
            },
        ]
        vehicle.ai_set_script(script)
        bng.step(600, wait=True)
        vehicle.update_vehicle()
        ref = [script[1]['x'], script[1]['y'], script[1]['z']]
        pos = vehicle.state['pos']
        ref, pos = np.array(ref), np.array(pos)
        assert np.linalg.norm(ref - pos) < 2.5

    scenario.delete(beamng)
Beispiel #16
0
def getOob(oobList, test, port):

    # Gains to port TORCS actuators to BeamNG
    # steering_gain = translate_steering()
    acc_gain = 0.5  # 0.4
    brake_gain = 1.0
    # BeamNG images are too bright for DeepDrive
    brightness = 0.4

    # Set up first vehicle
    # ! A vehicle with the name you specify here has to exist in the scenario
    vehicle = Vehicle('egovehicle')

    scenario = Scenario('asfault', 'asfault', True)

    # Set up sensors
    resolution = (280, 210)
    # Original Settings
    #pos = (-0.5, 1.8, 0.8)  # Left/Right, Front/Back, Above/Below
    # 0.4 is inside
    pos = (0, 2.0, 0.5)  # Left/Right, Front/Back, Above/Below
    direction = (0, 1, 0)
    # direction = (180, 0, 180)

    # FOV 60, MAX_SPEED 100, 20 (3) Hz fails
    # FOV 60, MAX_SPEED 80, 20 (3) Hz Ok
    # FOV 60, MAX_SPEED 80, 12 Hz Ok-ish Oscillations
    # FOV 60, MAX_SPEED 80, 10 Hz Ok-ish Oscillations
    # FOV 40, MAX_SPEED 50, 12 Hz Seems to be fine but drives slower
    # FOV 40, MAX_SPEED 80, 10 Hz Seems to be fine but drives slower
    fov = 60
    # MAX_SPEED = 70
    MAX_FPS = 60
    SIMULATION_STEP = 6
    # Running the controller at 20 hz makes experiments 3 to 4 times slower ! 5 minutes of simulations end up sucking 20 minutes !
    #

    # WORKING SETTINGS: 20 Freq, 90 FOV.
    front_camera = Camera(pos,
                          direction,
                          fov,
                          resolution,
                          colour=True,
                          depth=True,
                          annotation=True)
    electrics = Electrics()
    vehicle.attach_sensor('front_cam', front_camera)
    vehicle.attach_sensor('electrics', electrics)

    with open(pathToScenarioBeamng + '/asfault.json', 'r') as f:
        jsonScenario = json.load(f)
    with open(pathToScenarioBeamng + '/asfault.lua', 'r') as f:
        luaScenario = f.readlines()
    with open(pathToScenarioBeamng + '/asfault.prefab', 'r') as f:
        prefabScenario = f.readlines()
    with open(pathToScenarioDocuments + '/asfault.json', 'w') as outfile:
        json.dump(jsonScenario, outfile)
    with open(pathToScenarioDocuments + '/asfault.lua', 'w') as outfile:
        outfile.writelines(luaScenario)
    with open(pathToScenarioDocuments + '/asfault.prefab', 'w') as outfile:
        outfile.writelines(prefabScenario)
    deep_drive_engaged = True
    STATE = "NORMAL"
    notFinished = True
    portCount = 1
    multiplier = 2
    turtleMode = False
    turtleSpeed = 10
    offRoad = False
    oobSegKey = None
    oobSearch = False

    print(oobList)
    speedList = []
    currentSpeed = oobList[0][0][0]
    for speed in oobList[0][0]:
        speedList.append(speed)
        if speed > currentSpeed:
            currentSpeed = speed

    currentSpeed = currentSpeed * 3.6
    waypointList = []
    resultList = []
    oobWaypoint = []
    newOobWaypointList = []
    for key in oobList[0][1]:
        waypointList.append(key)
    print(waypointList)
    for waypoint in waypointList:
        for key in waypointList:
            print(key)
            for k in key:
                oobWaypoint.append(k)
                resultList.append(k)

    while notFinished:
        # Connect to running beamng
        setup_logging()
        beamng = BeamNGpy('localhost', port)
        bng = beamng.open(launch=True)
        scenario.make(bng)
        bng.load_scenario(scenario)
        try:
            bng.set_deterministic()  # Set simulator to be deterministic
            bng.set_steps_per_second(60)  # With 60hz temporal resolution
            # Connect to the existing vehicle (identified by the ID set in the vehicle instance)
            bng.connect_vehicle(vehicle, port + portCount)
            bng.start_scenario()
            # Put simulator in pause awaiting further inputs
            bng.pause()

            assert vehicle.skt

            testDic = RoadTest.to_dict(test)
            parentage = testDic['network']['parentage']

            for parent in parentage:
                roadKeys = []
                for keys in parentage:
                    for key in keys:
                        roadKeys.append(key)

            nodesDict = testDic['network']['nodes']
            nodes = []
            for node in nodesDict:
                nodes.append(node)
            vehicle.update_vehicle()

            oobCount = 0
            inOob = False
            vehicle.ai_set_speed(70)
            if oobSearch:
                if turtleMode:
                    currentSpeed = turtleSpeed
                else:
                    currentSpeed = currentSpeed - 10
            print(currentSpeed)
            running = True
            while running:
                # Resume the execution
                # 6 steps correspond to 10 FPS with a resolution of 60FPS
                # 5 steps 12 FPS
                # 3 steps correspond to 20 FPS
                #bng.step(SIMULATION_STEP)

                # # TODO: Is there a way to query for the speed directly ?
                #speed = math.sqrt(vehicle.state['vel'][0] * vehicle.state['vel'][0] + vehicle.state['vel'][1] * vehicle.state['vel'][1])
                # Speed is M/S ?
                # print("Speed from BeamNG is: ", speed, speed*3.6)

                #vehicle.ai_drive_in_lane(True)
                vehicle.update_vehicle()
                tup = get_segment(test, vehicle.state)
                currentSegmentDict = None
                currentSegKey = None

                if tup is not None:
                    currentSegmentDict = NetworkNode.to_dict(tup)
                    currentSegKey = currentSegmentDict['key']

                if offRoad:
                    if oobSegKey in resultList:
                        index = resultList.index(oobSegKey)
                        lostSeg = resultList.pop(index)
                        print('resList without seg', resultList)
                        print(lostSeg)
                    if oobSegKey not in newOobWaypointList:
                        newOobWaypointList.append(oobSegKey)
                        print('new OOB list', newOobWaypointList)
                    offRoad = False

                if inOob and off_track(test, vehicle.state):
                    print('off track')
                    offRoad = True
                    oobCount = oobCount + 1

                if currentSegKey in oobWaypoint:
                    vehicle.ai_drive_in_lane(True)
                    inOob = True
                    oobSegKey = currentSegKey
                    vehicle.ai_set_speed(currentSpeed)
                else:
                    inOob = False
                    vehicle.ai_set_speed(70)

                vehicle.ai_set_waypoint('waypoint_goal')
                vehicle.ai_drive_in_lane(True)

                if goal_reached(test, vehicle.state):
                    print(turtleMode)
                    for res in resultList:
                        segOobDict = testDic['execution']['seg_oob_count']
                        segOobDict[res] = currentSpeed / 3.6
                    if oobCount == 0:
                        testDic['execution']['oobs'] = 0
                        print('waypointlist', newOobWaypointList)
                        print('resultlist', resultList)
                        test = RoadTest.from_dict(testDic)
                        return test
                    else:
                        running = False
                        portCount = portCount + 1
                        multiplier = multiplier + 1
                        oobSearch = True
                        oobWaypoint = newOobWaypointList
                        newOobWaypointList = []
                        if currentSpeed < 20:
                            print(currentSpeed)
                            print('NewoobWay: ', newOobWaypointList)
                            print('resultlist: ', resultList)
                            if turtleMode:
                                for res in oobWaypoint:
                                    print(res)
                                    segOobDict = testDic['execution'][
                                        'seg_oob_count']
                                    segOobDict[res] = 12
                                testDic['execution']['oobs'] = 0
                                test = RoadTest.from_dict(testDic)
                                print(segOobDict)
                                print(resultList)
                                running = False
                                oobCount = 0
                                return test
                            else:
                                turtleMode = True
                                print('mode on')
                        bng.close()

        finally:
            if oobCount == 0:
                bng.close()
            else:
                print('restart')
class BeamNG_ACC_Test():
    def __init__(self, **kwargs):
        keys = kwargs.keys()
        if "test_controller" in keys:
            self.controller = kwargs.get("test_controller")
        else:
            self.controller = ACC_Test()
        config = BeamNG_Cruise_Control.Config(KP=CC_P,
                                              KI=CC_I,
                                              KD=CC_D,
                                              target=ACC_CAR_SPEED)
        config2 = BeamNG_Cruise_Control.Config(KP=CC_P,
                                               KI=CC_I,
                                               KD=CC_D,
                                               target=FRONT_CAR_SPEED)
        self.PID = BeamNG_Cruise_Control.PID_Controller(config=config)
        self.PID2 = BeamNG_Cruise_Control.PID_Controller(config=config2)

    def setup_bngServer(self):
        # Instantiate BeamNGpy instance running the simulator from the given path,
        if ('BNG_HOME' in os.environ):
            self.bng = BeamNGpy('localhost',
                                64256,
                                home=os.environ['BNG_HOME'])
        else:
            print(
                "WARNING no BNG_HOME is set! Make sure to set BeamNG.research path to research\trunk\ as environment variable (or write the path in the script by yourself)"
            )
            self.bng = BeamNGpy(
                'localhost',
                64256,
                home='C:\\Users\\Ingars\\Beamng-unlimited\\trunk')

    def setup_BeamNG(self):
        # Create a scenario in test map
        self.scenario = Scenario('cruise-control', 'example')

        # Create an ETK800 with the licence plate 'PID'
        self.vehicle = Vehicle('ACC', model='etk800', licence='ACC')

        electrics = Electrics()
        self.vehicle.attach_sensor('electrics',
                                   electrics)  # Cars electric system

        # Add it to our scenario at this position and rotation
        self.scenario.add_vehicle(self.vehicle, pos=ACC_CAR_POS, rot=(0, 0, 0))

        self.vehicle2 = Vehicle('FRONT', model='etk800', licence='FRONT')
        self.vehicle2.attach_sensor('electrics',
                                    electrics)  # Cars electric system
        self.scenario.add_vehicle(self.vehicle2,
                                  pos=FRONT_CAR_POS,
                                  rot=(0, 0, 180))
        # Place files defining our scenario for the simulator to read
        self.scenario.make(self.bng)

        # Launch BeamNG.research

        self.bng.open()

        # Load and start our scenario

        self.bng.load_scenario(self.scenario)
        self.bng.start_scenario()

    def runTest(self):
        self.bng.restart_scenario()
        self.bng.teleport_vehicle(self.vehicle,
                                  pos=ACC_CAR_POS,
                                  rot=(0, 0, 180))
        self.run()

    def run(self):
        self.setupTest()
        wanted_t = ACC_CAR_SPEED
        wanted_d = WANTED_DISTANCE

        soft_brake_d = wanted_d - (wanted_d / 2)

        wanted_d_buffer = wanted_d - (wanted_d / 5)
        # Wanted distance has a buffer of 25%, to compensate holding same constant speed at bigger distance

        deacc_d = wanted_d_buffer + 10
        # +10 meters is buffer for starting to decelerate towards front car speed and wanted distance.
        # the buffer should be calculated dynamically instead to compensate for different speeds
        while self.controller.ended == False:
            start_time = datetime.datetime.now()
            sensors = self.bng.poll_sensors(self.vehicle)
            position = self.vehicle.state['pos']
            position2 = self.vehicle2.state['pos']
            sensors2 = self.bng.poll_sensors(
                self.vehicle2
            )  # replace with self.vehicle.update_vehicle when not using pre-generated coordinates of vehicle to follow

            wheel_speed = sensors['electrics']['values']['wheelspeed']

            distance = self.controller.setDistance(position, position2)

            front_car_speed = self.controller.getFrontCarSpeed(wheel_speed)
            curr_target = self.PID.get_target()
            print("distance: " + str(distance))
            #FORMULA USED for deacceleration = front_car_speed^2 = wheel_speed^2 + (distance-20)*2*a
            #d = (front_car_speed - wheel_speed) / (-0.3 * 2) # 0.3 deacceleration? distance for reducing speed to front cars speed with -0.3 acceleration
            #print("d: " + str(d))
            if distance < 5:  # 5 is manually set as last allowed distance between both cars, will not work good if ACC car is driving too fast. Would be better to calculate it as braking distance.
                print("brake1")
                self.vehicle.control(brake=1)
                self.vehicle.control(throttle=0)
            elif distance < soft_brake_d:
                print("brake1")
                self.vehicle.control(
                    brake=0.1
                )  #Softness of brake could also be calculated dynamically to compensate different speeds
                self.vehicle.control(throttle=0)
            else:
                if distance <= wanted_d_buffer:
                    print("wanted")
                    if front_car_speed > curr_target + 3.5 or front_car_speed < curr_target - 2:  #or front_car_speed < curr_target - 1   // calibrate 3.5 and 2
                        self.PID.change_target(max(front_car_speed, 0))
                elif distance <= deacc_d:
                    a = (front_car_speed - wheel_speed) / (
                        (distance - wanted_d_buffer) * 2)
                    print("a:" + str(a))
                    self.PID.change_target(a + wheel_speed)
                elif curr_target != wanted_t:
                    self.PID.change_target(wanted_t)
                print("throttle1")
                value = self.PID.calculate_throttle(wheel_speed)
                value = min(1, value)
                self.vehicle.control(brake=0)
                self.vehicle.control(throttle=value)
            #PID for front car
            wheel_speed2 = sensors2['electrics']['values']['wheelspeed']
            # print("real front:" + str(wheel_speed2))
            value2 = self.PID2.calculate_throttle(wheel_speed2)
            value2 = min(1, value2)
            self.vehicle2.control(brake=0)
            self.vehicle2.control(throttle=value2)

            elapsed_time = datetime.datetime.now() - start_time
            while (elapsed_time.total_seconds() * 1000) < 100:
                elapsed_time = datetime.datetime.now() - start_time
            elapsed_total = self.controller.calculateElapsed()
            # Change front car speed after 10 seconds and after 20 seconds
            if elapsed_total.total_seconds() > float(10):
                self.PID2.change_target(20)
            if elapsed_total.total_seconds() > float(20):
                self.PID2.change_target(10)
        print("Ending Test")

    def close(self):
        self.bng.close()

    def setupTest(self):
        self.vehicle.update_vehicle()
        self.controller.last_position = self.vehicle.state['pos']
        self.controller.start()
Beispiel #18
0
def GenerateTrack(trackLength, sampleRate, show, startBeamNG):  
    populationSize = 100
    maxAcc = 1
   
    times = 20
    relNewSize = 0.6

    duplicatesThreshold = 0.05
    intersectionDelta = 0.01

    mutationProb = 0.1
    mutationDeviation = 0.01

    print("Generating Tracks")

    pop = initPop(populationSize, trackLength, maxAcc, 20)

    pop = evolve(pop, times, relNewSize, duplicatesThreshold,
           intersectionDelta, mutationProb, mutationDeviation, 10)

    print("eliminating intersecting tracks")    
    pop = elminateIntersecting(pop, intersectionDelta)

    if show:
        plotTrack(pop, 100)

    tracks = []

    nr = 0    

    first = True

    for track in pop:
        track = np.vstack((track, completeAcc(track)))
        poly = polysFromAccs(track)
        bez = bezFromPolys(poly)
        smpl = sampleBezSeries(bez, sampleRate).transpose()
        smpl = scaleTrack(smpl, 100, 100)
        smpl = np.array(smpl)
        smpl = np.vstack((smpl, [smpl[0]]))

        if(first):
            writeCriteria(smpl)
            first = False

        tracks.append(smpl)
        writeTrack(smpl, nr)
        nr += 1


    if startBeamNG:   
        nodes = []
        for p in tracks[0]: nodes.append((p[0], p[1], 0, 7))

        beamng = BeamNGpy('localhost', 64256)
        vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Green')
        scenario = Scenario('GridMap_v0422', 'track test')
        scenario.add_vehicle(vehicle, pos=(0, 0, -16.64), rot=(0, 0, 180))
        road = Road(material='track_editor_C_center', rid='main_road', texture_length=5, looped=True)

        road.nodes.extend(nodes)
        scenario.add_road(road)
        scenario.make(beamng)

        beamng.open()
        beamng.load_scenario(scenario)
        beamng.start_scenario()
        vehicle.ai_set_mode('span')

        while 1:
            vehicle.update_vehicle()
            print(vehicle.state['pos'])
            sleep(1)
Beispiel #19
0
def main():
    beamng = BeamNGpy('localhost',
                      64256,
                      home='F:\Softwares\BeamNG_Research_SVN')

    scenario = Scenario('GridMap', 'road_test')
    road_a = Road('custom_track_center', looped=False)
    nodes = [
        (0, 0, 0, 4),
        (0, 400, 0, 4),
    ]
    road_a.nodes.extend(nodes)
    scenario.add_road(road_a)

    vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Black')
    scenario.add_vehicle(vehicle, pos=(0, 0, 0.163609),
                         rot=(0, 0, 180))  # 0.163609 ,
    scenario.make(beamng)

    script = list()

    node0 = {
        'pos': (0, 0, 0.163609),
        'speed': 20,
    }

    node1 = {
        'pos': (0, 100, 0.163609),
        'speed': 30,
    }

    script.append(node0)
    script.append(node1)

    bng = beamng.open(launch=True)
    try:
        bng.load_scenario(scenario)
        bng.start_scenario()
        vehicle.ai_set_line(script)
        # update the state of vehicle at impact.

        for _ in range(100):
            time.sleep(0.1)
            vehicle.update_vehicle(
            )  # Synchs the vehicle's "state" variable with the simulator
            sensors = bng.poll_sensors(
                vehicle
            )  # Polls the data of all sensors attached to the vehicle
            print(vehicle.state['pos'])
            if vehicle.state['pos'][1] > 99:
                print('free state')
                vehicle.control(throttle=0,
                                steering=0,
                                brake=0,
                                parkingbrake=0)
                vehicle.update_vehicle()

        bng.stop_scenario()

        for _ in range(20):
            time.sleep(0.1)

        bng.load_scenario(scenario)
        bng.start_scenario()

        node0 = {
            'pos': (0, 50, 0.163609),
            'speed': 20,
        }

        node1 = {
            'pos': (0, 100, 0.163609),
            'speed': 30,
        }

        script.append(node0)
        script.append(node1)

        vehicle.ai_set_line(script)

        input('Press enter when done...')
    finally:
        bng.close()
def createCrashSimulation():
    print("Crash Simulation")

    beamng = BeamNGpy('localhost',
                      64256,
                      home='F:\Softwares\BeamNG_Research_SVN')
    scenario = Scenario('GridMap', 'road_crash_simulation')

    road_a = Road('track_editor_C_center', looped=False)
    road_b = Road('track_editor_C_center', looped=False)

    nodesa = [(30, 0, -4, 4), (0, 0, -4, 4)]

    nodesb = [(0, 30, -4, 4), (0, 0, -4, 4)]

    road_a.nodes.extend(nodesa)
    road_b.nodes.extend(nodesb)

    # Create an ETK800 with the licence plate 'PYTHON'
    vehicleA = Vehicle('ego_vehicleA', model='etk800', licence='PYTHON')
    # Add it to our scenario at this position and rotation

    damageS = Damage()
    vehicleA.attach_sensor('damagesS', damageS)

    scenario.add_vehicle(vehicleA, pos=(30, 0, 0), rot=(0, 0, 90))

    # Create an ETK800 with the licence plate 'PYTHON'
    vehicleB = Vehicle('ego_vehicleB', model='etk800', licence='PYTHON')
    # Add it to our scenario at this position and rotation

    damageV = Damage()
    vehicleB.attach_sensor('damagesV', damageV)

    scenario.add_vehicle(vehicleB, pos=(0, 30, 0), rot=(0, 0, 0))

    scenario.add_road(road_a)
    scenario.add_road(road_b)

    scenario.make(beamng)

    bng = beamng.open(launch=True)
    try:
        bng.load_scenario(scenario)
        bng.start_scenario()

        node0 = {
            'pos': (30, 0, 0),
            'speed': 0,
        }

        node1 = {
            'pos': (0, 0, 0),
            'speed': 20,
        }

        script = list()
        script.append(node0)
        script.append(node1)

        vehicleA.ai_set_line(script)

        node3 = {
            'pos': (0, 30, 0),
            'speed': 0,
        }

        node4 = {
            'pos': (0, 0, 0),
            'speed': 20,
        }

        script = list()
        script.append(node3)
        script.append(node4)

        vehicleB.ai_set_line(script)

        time.sleep(12)
        #       input('Press enter when done...')
        vehicleA.update_vehicle(
        )  # Synchs the vehicle's "state" variable with the simulator
        sensors = bng.poll_sensors(vehicleA)
        damage_striker = sensors['damagesS']
        print(sensors['damagesS'])
        print(vehicleA.state['pos'])

        vehicleB.update_vehicle(
        )  # Synchs the vehicle's "state" variable with the simulator
        sensors = bng.poll_sensors(vehicleB)
        damage_victim = sensors['damagesV']
        print(sensors['damagesV'])
        print(vehicleB.state['pos'])

        multiObjectiveFitnessFunction(123456789, damage_striker,
                                      vehicleA.state['pos'], (0, 0),
                                      damage_victim, vehicleB.state['pos'],
                                      (0, 0))
        # multiobjective fitness function.

        bng.stop_scenario()

        # bng.load_scenario(scenario)
        # bng.start_scenario()
        #
        # print("sleep")
        # time.sleep(10)
        # print("wake")
        #
        # node0 = {
        #     'pos': (30, 0, 0),
        #     'speed': 0,
        # }
        #
        # node1 = {
        #     'pos': (0, 0, 0),
        #     'speed': 30,
        # }
        #
        # script = list()
        # script.append(node0)
        # script.append(node1)
        #
        # vehicleA.ai_set_line(script)
        #
        # node0 = {
        #     'pos': (0, 30, 0),
        #     'speed': 0,
        # }
        #
        # node1 = {
        #     'pos': (0, 0, 0),
        #     'speed': 30,
        # }
        #
        # script = list()
        # script.append(node0)
        # script.append(node1)
        #
        # vehicleB.ai_set_line(script)
        #
        # input('Press enter when done...')

    finally:
        bng.close()
Beispiel #21
0
def run_spawn_for_deflation(vehicle_model='etk800',
                            deflation_pattern=[0, 0, 0, 0]):
    global base_filename, default_color, default_scenario, setpoint
    global prev_error
    global fcam

    vehicle_loadfile = 'vehicles/hopper/crawler.pc'
    # setup DNN model + weights
    m = Model()
    model = m.define_model_BeamNG("BeamNGmodel-5.h5")

    random.seed(1703)
    setup_logging()

    # beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
    beamng = BeamNGpy('localhost',
                      64256,
                      home='H:/BeamNG.research.v1.7.0.1clean')
    scenario = Scenario(default_scenario, 'research_test')
    vehicle = Vehicle('ego_vehicle',
                      model=vehicle_model,
                      licence='LOWPRESS',
                      color=default_color)
    vehicle = setup_sensors(vehicle)
    spawn = spawn_point(default_scenario, 'highway')
    # scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'])

    # Compile the scenario and place it in BeamNG's map folder
    scenario.make(beamng)

    # Start BeamNG and enter the main loop
    bng = beamng.open(launch=True)

    # bng.hide_hud()
    bng.set_deterministic()  # Set simulator to be deterministic

    # Load and start the scenario
    bng.load_scenario(scenario)
    bng.start_scenario()

    # load part config
    #pc = vehicle.get_part_config()
    # loadfile = 'C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/front17x8rear17x9.json'
    # vehicle.load(loadfile)

    bng.spawn_vehicle(vehicle,
                      pos=spawn['pos'],
                      rot=None,
                      rot_quat=spawn['rot_quat'],
                      partConfig='vehicles/hopper/custom.pc')
    #bng.set_relative_camera()

    # Put simulator in pause awaiting further inputs
    bng.pause()
    assert vehicle.skt
    # vehicle.control(throttle=0.0, steering=0.0, brake=1.0)

    # perturb vehicle
    # before_state = copy.deepcopy(vehicle.state)
    before_state = bng.poll_sensors(vehicle)['front_cam']
    before_state['vel'] = vehicle.state['vel']
    before_state['roll'] = vehicle.state['roll']
    before_state['pitch'] = vehicle.state['pitch']
    before_state['yaw'] = vehicle.state['yaw']
    # print("vehicle position before deflation via beamstate:{}".format(vehicle.get_object_position()))
    # print("vehicle position before deflation via vehicle state:{}".format(vehicle.state))
    print(
        "vehicle position before deflation via camera:{}".format(before_state))
    image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
    # print("camera sensor before deflation: {}".format(bng.poll_sensors(vehicle)['front_cam']))
    #get_camera_rot_and_pos([-0.3, 1, 1.0], [0, 0.75, 0], before_state['pos'], before_state['dir'], before_state['up'])
    img = m.process_image(image)
    before_prediction = model.predict(img)
    before_state["prediction"] = before_prediction
    plt.imshow(image)
    plt.pause(0.01)
    if deflation_pattern != [0, 0, 0, 0]:
        print("deflating according to pattern {}".format(deflation_pattern))
        vehicle.deflate_tires(deflation_pattern)
        time.sleep(1)
    bng.resume()
    bng.set_steps_per_second(100)
    bng.set_deterministic()
    totalsecs = 0.0
    deflation_traj = []
    while totalsecs <= 30:
        vehicle.update_vehicle()
        # vehicle.control(throttle=0.0, steering=0.0, brake=1.0)
        after_state = bng.poll_sensors(vehicle)['front_cam']
        after_state['vel'] = vehicle.state['vel']
        after_state['roll'] = vehicle.state['roll']
        after_state['pitch'] = vehicle.state['pitch']
        after_state['yaw'] = vehicle.state['yaw']
        print("roll:{}, pitch:{}, yaw:{}".format(vehicle.state['roll'],
                                                 vehicle.state['pitch'],
                                                 vehicle.state['yaw']))
        deflation_traj.append(round(math.degrees(vehicle.state['pitch'][0]),
                                    4))
        bng.step(100)
        totalsecs += 1
    # after_state = copy.deepcopy(vehicle.state)
    # print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position()))
    # print("vehicle position after deflation via vehicle state:{}".format(vehicle.state))
    image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
    print("vehicle position after deflation via camera:{}".format(after_state))
    #print("camera sensor output: {}".format(bng.poll_sensors(vehicle)['front_cam']['rot']))
    #print("camera pos output: {}".format(bng.poll_sensors(vehicle)['front_cam']['pos']))
    # print("camera rot output: {}".format(bng.poll_sensors(vehicle)['front_cam']['direction']))
    # print("fcam.encode_engine_request() = {}".format(fcam.encode_engine_request()))
    damages = bng.poll_sensors(vehicle)['damage']['deform_group_damage']
    d = ["{}={}".format(k, damages[k]['damage']) for k in damages.keys()]
    print("vehicle pressure after deflation: lowpressure={} damages:".format(
        bng.poll_sensors(vehicle)['damage']['lowpressure'], d))
    img = m.process_image(image)
    after_state["prediction"] = model.predict(img)
    plt.imshow(image)
    plt.pause(0.01)
    bng.close()
    return before_state, after_state, deflation_traj
def main():
    setup_logging()

    beamng = BeamNGpy('localhost', 64256, home='D:/Programs/BeamNG/trunk')
    #scenario = Scenario('west_cost_usa', 'ai_sine')
    scenario = Scenario('smallgrid', 'ai_sine')

    vehicle = Vehicle('ego_vehicle', model='etk800', license='AI')

    #orig = (-769.1, 400.8, 142.8)
    orig = (-10, -1309, 0.215133)  #smallgrid

    #scenario.add_vehicle(vehicle, pos=orig, rot=(0, 0, 180))
    scenario.add_vehicle(vehicle, pos=orig, rot=(1, 0, 0))  #smallgrid
    scenario.make(beamng)

    #Run simulation without emg_processing_time to obtain target trajectory.
    bng = beamng.open(launch=True)
    count = 0
    index = 0
    vehicle_dynamics = []

    try:
        print('Running simulation with target trajectory.')
        bng.load_scenario(scenario)
        bng.start_scenario()
        #Set throttle to maintain speed near 7 km/h during turn.
        vehicle.control(throttle=0.04)
        start_time = time.time()

        while True:
            bng.step(1)

            vehicle.update_vehicle()
            if vehicle.state['pos'][1] <= -1319 and count == 0:
                vehicle.control(steering=-1.0)
                count += 1
            if vehicle.state['pos'][1] >= -1319 and count == 1:
                break

            #Store vehicle dynamics.
            vehicle_dynamics.append(','.join(
                map(str, [
                    index,
                    time.time() - start_time,
                    math.sqrt(vehicle.state['vel'][0]**2 +
                              vehicle.state['vel'][1]**2),
                    vehicle.state['pos'][0], vehicle.state['pos'][1]
                ])))
            print('Time:' + str(time.time() - start_time) + 'Position: ' +
                  str(vehicle.state['pos'][1]) + ' ' + 'Velocity: ' + str(
                      math.sqrt(vehicle.state['vel'][0]**2 +
                                vehicle.state['vel'][1]**2)))
            index += 1
    finally:
        bng.close()

    #Save target trajectory results.
    heading = ['', 'time', 'speed', 'x_coordinate', 'y_coordinate']
    with open('sim_target_trajectory_left_uturn.txt', "w") as results:
        heading = map(str, heading)
        results.write(",".join(heading) + '\n' +
                      "\n".join(str(element) for element in vehicle_dynamics))
    results.close()

    #Read total EMG signal processing time (seconds/feature) per trial from txt files.
    f = [
        line.rstrip('\n') for line in open(
            'D:/Research Projects/sEMG_control_for_automobiles/putemg-downloader/putemg_examples/total_processing_time_wrist_flexion.txt'
        )
    ][1:]
    total_EMG_processing_time = []
    for line in f:
        total_EMG_processing_time.append(float(line))

    #Run simulations that delay steering initiation based based on EMG signal processing time from trials.
    for emg_processing_time in range(len(total_EMG_processing_time)):
        #for emg_processing_time in range(0,1):
        print(
            '\n' +
            'Running simulations with EMG signal processing time from trial ' +
            str(emg_processing_time) + '.')
        bng = beamng.open(launch=True)
        count = 0
        index = 0
        vehicle_dynamics = []

        try:
            bng.load_scenario(scenario)
            bng.start_scenario()
            #Set throttle to maintain speed near 7 km/h during turn.
            vehicle.control(throttle=0.04)
            start_time = time.time()

            while True:
                bng.step(1)

                vehicle.update_vehicle()
                if vehicle.state['pos'][1] <= -1319 and count == 0:
                    sleep(total_EMG_processing_time[emg_processing_time])
                    vehicle.control(steering=-1.0)
                    count += 1
                if vehicle.state['pos'][1] >= -1319 and count == 1:
                    break

                #Store vehicle dynamics.
                vehicle_dynamics.append(','.join(
                    map(str, [
                        index,
                        time.time() - start_time,
                        math.sqrt(vehicle.state['vel'][0]**2 +
                                  vehicle.state['vel'][1]**2),
                        vehicle.state['pos'][0], vehicle.state['pos'][1]
                    ])))
                print('Time:' + str(time.time() - start_time) + 'Position: ' +
                      str(vehicle.state['pos'][1]) + ' ' + 'Velocity: ' + str(
                          math.sqrt(vehicle.state['vel'][0]**2 +
                                    vehicle.state['vel'][1]**2)))
                index += 1
        finally:
            bng.close()

        heading = ['', 'time', 'speed', 'x_coordinate', 'y_coordinate']
        with open('sim_results_trial_' + str(emg_processing_time) + '.txt',
                  "w") as results:
            heading = map(str, heading)
            results.write(",".join(heading) + '\n' + "\n".join(
                str(element) for element in vehicle_dynamics))
        results.close()
        node3 = {
            'pos': (collision_points[0][0], collision_points[0][1], 0),
            'speed': victim_speeds[0],
        }

        script = list()
        script.append(node2)
        script.append(node3)

        vehicleVictim.ai_set_line(script)

        for number in range(60):
            time.sleep(0.20)

            vehicleStriker.update_vehicle(
            )  # Synchs the vehicle's "state" variable with the simulator
            sensors = bng.poll_sensors(
                vehicleStriker
            )  # Polls the data of all sensors attached to the vehicle
            #print(vehicleStriker.state['pos'])
            if vehicleStriker.state['pos'][0] > 306 and vehicleStriker.state[
                    'pos'][1] > 39:
                # print('free state')
                vehicleStriker.control(throttle=0,
                                       steering=0,
                                       brake=0,
                                       parkingbrake=0)
                vehicleStriker.update_vehicle()

            vehicleVictim.update_vehicle(
            )  # Synchs the vehicle's "state" variable with the simulator
def scenario(beamNG_path):
   
    if (beamNG_path == ""):
        beamNG_path = 'D:/BeamNGReasearch/Unlimited_Version/trunk'
    
    bng = BeamNGpy('localhost', 64256, beamNG_path)
    
    
  

    #scenario = Scenario('west_coast_usa', 'example')

    scenario = Scenario('GridMap', 'example')

    # Create an ETK800 with the licence plate 'PYTHON'
    vehicle1 = Vehicle('ego_vehicle', model='etk800', licence='PYTHON',  color='Red')
    vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue')

    electrics = Electrics()
    vehicle1.attach_sensor('electrics', electrics)


   

    pos2 = (-13.04469108581543, -107.0409164428711, 0.202297180891037)
    pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775)
    rot2 = (0.0009761620895005763, -0.9999936819076538, -0.0034209610894322395)
    rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936)
    scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)
    scenario.add_vehicle(vehicle2, pos=pos2, rot=rot2)

    scenario.make(bng)

    # Launch BeamNG.research
    bng.open(launch=True)
    SIZE = 500

    try:
        bng.load_scenario(scenario)
        bng.start_scenario()
        
        vehicle1.ai_set_speed(10.452066507339481,mode = 'set')
        vehicle1.ai_set_mode('span')
        vehicle2.ai_set_mode('chase')
         #collect data
        positions = list()
        directions = list()
        wheel_speeds = list()
 
        for _ in range(100):
            time.sleep(0.1)
            vehicle1.update_vehicle()  # Synchs the vehicle's "state" variable with the simulator
            sensors = bng.poll_sensors(vehicle1)  # Polls the data of all sensors attached to the vehicle
            positions.append(vehicle1.state['pos'])
            directions.append(vehicle1.state['dir'])
            wheel_speeds.append(sensors['electrics']['values']['wheelspeed'])
           

            print('The Final result - position  :')
            print(positions)
            print('The Final result - direction  :')
            print(directions)
            print('The Final result - speed  :')
            print(wheel_speeds)
        
        bng.stop_scenario()
        bng.close()
    finally:
            bng.close()
# display(sensors['electrics']['values']['brake'])

# print('The Damage is:')
# display(sensors['damages'])
# display(damage)

positions = list()
directions = list()
wheel_speeds = list()
throttles = list()
brakes = list()
damages = list()

for _ in range(150):
    time.sleep(0.1)
    vehicle.update_vehicle(
    )  # Synchs the vehicle's "state" variable with the simulator
    sensors = bng.poll_sensors(
        vehicle)  # Polls the data of all sensors attached to the vehicle
    positions.append(vehicle.state['pos'])
    directions.append(vehicle.state['dir'])
    wheel_speeds.append(sensors['electrics']['values']['wheelspeed'])
    throttles.append(sensors['electrics']['values']['throttle'])
    brakes.append(sensors['electrics']['values']['brake'])
    damages.append(sensors['damages'])

bng.close()

#print(wheel_speeds)
# print(directions)
# print(damages[-1])
Beispiel #26
0
def launch(beamNGPath, car1, car2, speed_car2):
    crash = False
    dist_2car = 20
    speed_car2 = int(speed_car2)
    bng = BeamNGpy('localhost', 64256, beamNG_path)
    #bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk')

    scenario = Scenario('GridMap', 'example')

    # Create an ETK800 with the licence plate 'PYTHON'
    vehicle1 = Vehicle('ego_vehicle',
                       model='etk800',
                       licence='PYTHON',
                       color='Red')
    vehicle2 = Vehicle('vehicle',
                       model='etk800',
                       licence='CRASH',
                       color='Blue')

    electrics1 = Electrics()
    electrics2 = Electrics()
    vehicle1.attach_sensor('electrics1', electrics1)
    vehicle2.attach_sensor('electrics2', electrics2)

    #position to try drive then teleport
    #-365.2436828613281, -214.7460479736328, 1.2118444442749023], [0.9762436747550964, 0.20668958127498627, 0.0650215595960617]]
    #[[-362.4477844238281, -214.16226196289062, 1.32931387424469], [0.9824153780937195, 0.1852567195892334, 0.023236412554979324]]

    pos2 = (25.75335693359375, -127.78406524658203, 0.2072899490594864)
    pos1 = (-88.8136978149414, -261.0204162597656, 0.20253072679042816)

    #pos_tel1 = (53.35311508178711, -139.84017944335938, 0.20729705691337585)           #change this
    #pos_tel2 = (75.94310760498047, -232.62135314941406, 0.20568031072616577)            #change this
    pos_tel1 = car1[0]
    pos_tel2 = car2[0]

    rot2 = (0.9298766851425171, -0.36776003241539, 0.009040255099534988)
    rot1 = (-0.9998571872711182, 0.016821512952446938, -0.0016229493776336312)
    # rot_tel1= (0.8766672611236572, -0.4810631573200226, 0.005705998744815588)         #change this
    # rot_tel2 = (-0.896364688873291, -0.4433068335056305, -0.0030648468527942896)       #change this
    rot_tel1 = car1[1]
    rot_tel2 = car2[1]

    #initial postion of two car.
    # run 2cars on particular road until they reach particular speed which satisfies the condisiton of the given testcase
    scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)
    scenario.add_vehicle(vehicle2, pos=pos2, rot=rot2)

    scenario.make(bng)

    # Launch BeamNG.research
    bng.open(launch=True)

    try:
        bng.load_scenario(scenario)
        bng.start_scenario()

        vehicle1.ai_set_speed(10, mode='set')
        vehicle1.ai_set_mode('span')

        vehicle2.ai_set_speed(speed_car2,
                              mode='set')  ##change this param of speed
        vehicle2.ai_set_mode('chase')
        sys_output.print_sub_tit("Initial Position and rotation of car")
        print("\nInitial  Position and rotation of car1  %s %s  " %
              (pos1, rot1))
        print("\nInitial  Position and rotation of car2  %s %s  " %
              (pos2, rot2))
        sys_output.print_sub_tit(
            "\n when speed of car 1 rearch 10 and speed car 2 reach %s. Two cars are teleport to new locations."
            % (speed_car2))
        for _ in range(450):
            time.sleep(0.1)
            vehicle1.update_vehicle(
            )  # Synchs the vehicle's "state" variable with the simulator
            vehicle2.update_vehicle()
            sensors1 = bng.poll_sensors(
                vehicle1
            )  # Polls the data of all sensors attached to the vehicle
            sensors2 = bng.poll_sensors(vehicle2)

            speed1 = sensors1['electrics1']['values']['wheelspeed']
            speed2 = sensors2['electrics2']['values']['wheelspeed']
            print("speed of car 1", speed1)
            print("speed of car 2", speed2)
            #print([vehicle1.state['pos'],vehicle1.state['dir']])
            if speed1 > 9 and speed2 > speed_car2 - 1:  #change speed here
                bng.teleport_vehicle(vehicle1, pos_tel1, rot_tel1)
                bng.teleport_vehicle(vehicle2, pos_tel2, rot_tel2)
                sys_output.print_sub_tit("Teleport 2 car to new location")
                print("\n  Car1 : " + str(pos_tel1) + ", " + str(rot_tel1))
                print("\n  Car2 : " + str(pos_tel2) + ", " + str(rot_tel2))
                print("\n  Distance between two cars: " +
                      str(distance.euclidean(pos_tel1, pos_tel2)))
                break

            # if speed > 19:
            # bng.teleport_vehicle(vehicle1,pos_tel,rot_tel )
            # break

        for _ in range(100):
            #pos1 = []
            time.sleep(0.1)
            vehicle1.update_vehicle(
            )  # Synchs the vehicle's "state" variable with the simulator
            vehicle2.update_vehicle()
            sensors1 = bng.poll_sensors(
                vehicle1
            )  # Polls the data of all sensors attached to the vehicle
            sensors2 = bng.poll_sensors(vehicle2)

            speed1 = sensors1['electrics1']['values']['wheelspeed']
            speed2 = sensors2['electrics2']['values']['wheelspeed']

            #pos1.append(vehicle1.state['pos'])
            #pos2.append(vehicle2.state['pos'])

            dist_2car = distance.euclidean(vehicle1.state['pos'],
                                           vehicle2.state['pos'])
            if dist_2car < 5:  #or int(speed2)== 0 :
                crash = True
                print(
                    "\n  Failed because distance of two cars are less than 5")
                break
            print("\n speed1 %s, speed2: %s, distance: %s" %
                  (speed1, speed2, dist_2car))
            if int(speed2) == 0:
                print("\n  Pass because car2 stoped")
                break

        bng.stop_scenario()
        bng.close()
    finally:
        bng.close()

    return crash