def run(self):
        # Setup environment
        lgsvl_sim = self.simConnection.connect()
        control = lgsvl.NPCControl()
        ego_control = lgsvl.VehicleControl()

        # Placing the school_bus
        school_bus_state = spawn_state(lgsvl_sim)
        school_bus_state = CarControl.place_car_on_the_point(
            state=school_bus_state, sim=lgsvl_sim, point=self.npc_source)
        school_bus = load_npc(lgsvl_sim, "SchoolBus", school_bus_state)

        # Placing the ego on the starting point
        ego_state = spawn_state(lgsvl_sim)
        ego_state = CarControl.place_car_from_the_point(dimension="horizontal",
                                                        distance=-6,
                                                        state=ego_state)
        ego_state = CarControl.drive_ego_car(ego_state,
                                             [("vertical", self.ego_speed)])
        ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

        # Callback collision function
        ego.on_collision(self.on_collision)
        school_bus.on_collision(self.on_collision)

        # Set waypoints for School Bus
        waypoints = []
        for point in [self.npc_source, self.npc_target]:
            waypoints.append(
                lgsvl.DriveWaypoint(point, self.npc_speed,
                                    school_bus.state.transform.rotation))

        try:
            # Start the scenario
            # The School Bus is parked on the street
            control.headlights = 2
            control.e_stop = True
            school_bus.apply_control(control)
            # Let the ego running for 2 seconds
            self.simConnection.execute(timeout=2)

            # The school bus turns on signal to prepare for the turn
            control.headlights = 0  # turn off headlight
            control.turn_signal_left = True
            school_bus.apply_control(control)
            self.simConnection.execute(timeout=2)

            # Brake the ego
            CarControl.brake_ego(ego=ego,
                                 control=ego_control,
                                 brake_value=self.ego_brake,
                                 sticky=True)

            # The school bus starts to turn right
            school_bus.follow(waypoints)
            self.simConnection.execute(timeout=10)
        except Exception:
            print("Failed!")
示例#2
0
        def setup_cars(sim_connection: SimConnection, ego_speed: float,
                       npc_speed: float):
            lgsvl_sim = sim_connection.connect()
            # Placing the school_bus
            school_bus_state = spawn_state(lgsvl_sim)
            school_bus = load_npc(lgsvl_sim, "SchoolBus", school_bus_state)

            # Placing the ego on the starting point
            ego_state = spawn_state(lgsvl_sim)
            ego_state = CarControl.place_car_from_the_point(
                dimension="horizontal", distance=-6, state=ego_state)
            ego_state = CarControl.drive_ego_car(ego_state,
                                                 [("vertical", ego_speed)])
            ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
            return {
                "school_bus": school_bus,
                "ego": ego,
            }
示例#3
0
def init_configuration(lgsvl_sim: lgsvl.Simulator,
                       initial_state: lgsvl.AgentState, npc_speed: float):
    # Init ego state
    ego_state = copy.deepcopy(initial_state)
    # Place Ego Car - 5 meter ahead from the initial point
    ego_state = CarControl.place_car_from_the_point(dimension="vertical",
                                                    distance=5,
                                                    state=ego_state)
    # Place Ego Car - 3.5m meter on the left
    ego_state = CarControl.place_car_from_the_point(dimension="horizontal",
                                                    distance=-3.5,
                                                    state=ego_state)
    # Drive ego car with speed 5m/s
    ego_state = CarControl.drive_ego_car(state=ego_state,
                                         directions=[("vertical", 5)])
    # Add ego to simulator
    ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

    # Init sedan state
    sedan_state = copy.deepcopy(initial_state)
    # Place Sedan - 5 meter ahead from the initial point
    sedan_state = CarControl.place_car_from_the_point(dimension="vertical",
                                                      distance=5,
                                                      state=sedan_state)
    # Add Sedan to simulator
    sedan = load_npc(lgsvl_sim, "Sedan", sedan_state)
    # Drive sedan follow its lane with given speed in m/s
    sedan.follow_closest_lane(True, npc_speed)

    # Init suv state
    suv_state = copy.deepcopy(initial_state)
    # Place SUV - 15 meter ahead from the initial point
    suv_state = CarControl.place_car_from_the_point(dimension="vertical",
                                                    distance=20,
                                                    state=suv_state)
    # Add suv to simulator
    suv = load_npc(lgsvl_sim, "SUV", suv_state)
    # Drive suv follow its lane with given speed in m/s
    suv.follow_closest_lane(True, npc_speed)
    return {
        "sedan": sedan,
        "suv": suv,
        "ego": ego,
    }
 def generate_vehicles(self, sim: lgsvl.Simulator, vehicles: list):
     agents = []
     for vehicle in vehicles:
         state = spawn_state(sim)
         state = CarControl.place_car_from_the_point(
             dimension="vertical",
             distance=vehicle["distance"],
             state=state)
         agent = vehicle["load_vehicle"](sim, vehicle["type"], state)
         agent.on_collision(self.on_collision)
         agents.append(agent)
     return agents
示例#5
0
    def test_placing_EGO_and_NPC_on_different_lane(self):
        with SimConnection(scene="SingleLaneRoad") as sim:
            # Placing the sedan on the starting point
            state = spawn_state(sim)
            sedan = load_npc(sim, "Sedan", state)

            # Placing the ego on the left hand side of starting point - distance 3.5
            ego_state = CarControl.place_car_from_the_point(dimension="horizontal", distance=-3.5, state=state)
            ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

            # sedan and ego on different line
            self.assertNotEqual(ego.state.position.z, sedan.state.position.z, "sedan and ego on different lane")
    def test_EGO_following_NPC_without_crash(self):
        simConnection = SimConnection()
        sim = simConnection.connect()
        # Placing the suv - 10m ahead from the starting point
        state = spawn_state(sim)
        truck_state = CarControl.place_car_from_the_point(dimension="vertical",
                                                          distance=10,
                                                          state=state)
        truck = load_npc(sim, "BoxTruck", truck_state)
        # Driving the truck - speed 5m/s from the starting point
        truck.follow_closest_lane(True, 5)

        # Driving the ego - speed 1m/s from the starting point
        state = spawn_state(sim)
        ego_state = CarControl.drive_ego_car(state=state,
                                             directions=[("vertical", 4.5)])
        ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

        # Run the simulator for 10 seconds with debug mode
        simConnection.execute(timeout=10)
        self.assertEqual(True, True)
        simConnection.sim.close()
def setup_cars(sim_connection: SimConnection, ego_speed: float):
    lgsvl_sim = sim_connection.connect()
    # Placing the sedan
    sedan_state = spawn_state(lgsvl_sim)
    sedan_state = CarControl.place_car_from_the_point(dimension="vertical",
                                                      distance=15,
                                                      state=sedan_state)
    sedan_state = CarControl.place_car_from_the_point(dimension="horizontal",
                                                      distance=-8,
                                                      state=sedan_state)
    sedan_state = CarControl.rotate_car_by_degree(state=sedan_state,
                                                  degree=-45)
    sedan = load_npc(lgsvl_sim, "Sedan", sedan_state)

    # Placing the ego on the starting point
    ego_state = spawn_state(lgsvl_sim)
    ego_state = CarControl.drive_ego_car(ego_state, [("vertical", ego_speed)])
    ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
    return {
        "sedan": sedan,
        "ego": ego,
    }
示例#8
0
    def test_placing_EGO_between_TRUCK_and_SEDAN_on_same_lane(self):
        with SimConnection(scene="SingleLaneRoad") as sim:
            # Placing the sedan on the starting point
            state = spawn_state(sim)
            sedan = load_npc(sim, "Sedan", state)

            # Placing the ego - 5m ahead from the starting point
            state = spawn_state(sim)
            ego_state = CarControl.place_car_from_the_point(dimension="vertical", distance=5, state=state)
            ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

            # Placing the suv - 10m ahead from the starting point
            state = spawn_state(sim)
            suv_state = CarControl.place_car_from_the_point(dimension="vertical", distance=10, state=state)
            suv = load_npc(sim, "SUV", suv_state)

            # sedan and ego and suv on same line
            self.assertEqual(sedan.state.position.z, ego.state.position.z, "sedan and ego on same lane")
            self.assertEqual(ego.state.position.z, suv.state.position.z, "ego and suv on same lane")
            # ego is behind suv
            self.assertLess(suv.state.position.x, ego.state.position.x, "ego behind suv")
            self.assertLess(ego.state.position.x, sedan.state.position.x, "ego in front of sedan")
示例#9
0
        def setup_cars(simc: SimConnection, npc_speed: float):
            lgsvl_sim = simc.connect()
            sedan_state = spawn_state(lgsvl_sim)
            sedan_state = CarControl.place_car_from_the_point(dimension="vertical", distance=0, state=sedan_state)
            sedan = load_npc(lgsvl_sim, "Sedan", sedan_state)
            sedan.follow_closest_lane(True, npc_speed)

            suv_state = spawn_state(lgsvl_sim)
            suv_state = CarControl.place_car_from_the_point(dimension="vertical", distance=17, state=suv_state)
            suv = load_npc(lgsvl_sim, "Sedan", suv_state)
            suv.follow_closest_lane(True, npc_speed)

            ego_state = spawn_state(lgsvl_sim)
            ego_state = CarControl.place_car_from_the_point(dimension="horizontal", distance=-3.5, state=ego_state)
            ego_state = CarControl.place_car_from_the_point(dimension="vertical", distance=5, state=ego_state)
            ego_state = CarControl.drive_ego_car(state=ego_state, directions=[("vertical", 5)])
            ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
            return {
                "sedan": sedan,
                "suv": suv,
                "ego": ego,
            }
示例#10
0
    def test_placing_EGO_on_different_land_and_ego_between_SUV_and_SEDAN(self):
        with SimConnection(scene="SingleLaneRoad") as sim:
            # Placing the sedan on the starting point
            state = spawn_state(sim)
            sedan = load_npc(sim, "Sedan", state)

            # Placing the ego:
            # - 5m ahead from the starting point
            # - 3.5m on the left hand side
            state = spawn_state(sim)
            ego_state = CarControl.place_car_from_the_point(dimension="horizontal", distance=-3.5, state=state)
            ego_state = CarControl.place_car_from_the_point(dimension="vertical", distance=5, state=ego_state)
            ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

            # Placing the suv - 10m ahead from the starting point
            state = spawn_state(sim)
            suv_state = CarControl.place_car_from_the_point(dimension="vertical", distance=10, state=state)
            suv = load_npc(sim, "SUV", suv_state)

            self.assertNotEqual(ego.state.position.z, sedan.state.position.z, "ego and sedan on different lane")
            self.assertNotEqual(ego.state.position.z, suv.state.position.z, "ego and suv on different lane")
            self.assertEqual(suv.state.position.z, sedan.state.position.z, "suv and sedan on same lane")
            self.assertLess(suv.state.position.x, ego.state.position.x, "ego behind suv")
            self.assertLess(ego.state.position.x, sedan.state.position.x, "ego in front of sedan")
示例#11
0
    def test_placing_EGO_behind_NPC_on_same_lane(self):
        with SimConnection(scene="SingleLaneRoad") as sim:
            # Placing the sedan on the starting point
            state = spawn_state(sim)
            sedan = load_npc(sim, "Sedan", state)

            # Placing the sedan in front of the ego
            # Both vehicles are on the same lane
            ego_state = CarControl.place_car_from_the_point(dimension="vertical", distance=-3.5, state=state)
            ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

            # sedan and ego on same line
            self.assertEqual(ego.state.position.z, sedan.state.position.z, "sedan and ego on same lane")
            # ego is behind sedan
            self.assertLess(sedan.state.position.x, ego.state.position.x, "ego behind sedan")
示例#12
0
    def test_EGO_encroach_schoolbus_speed_6_with_apollo(self):
        LGSVL__APOLLO_HOST = config("LGSVL__APOLLO_HOST")
        LGSVL__APOLLO_PORT = int(config("LGSVL__APOLLO_PORT"))
        LGSVL__DREAMVIEW_HOST = config("LGSVL__DREAMVIEW_HOST")
        MODULES = [
            'Recorder', 'Localization', 'Perception', 'Transform', 'Routing',
            'Prediction', 'Planning', 'Traffic Light', 'Control'
        ]
        TARGET = Vector(3.03013730049133, -0.00637590885162354,
                        -16.5673313140869)
        COLLISIONS = []
        SEDAN_WPS = [
            Vector(-3.31942486763, -0.0809718370437622, 24.2049713134766),
            Vector(-3.84999561309814, -0.00320455431938171, 14.877103805542),
            Vector(3.84999561309814, -0.00320455431938171, -20.877103805542)
        ]
        NPC_SPEED = 6

        sim_connection = SimConnection(scene="CubeTown")
        lgsvl_sim = sim_connection.connect()
        # Placing the school_bus
        school_bus_state = spawn_state(lgsvl_sim)
        school_bus = load_npc(lgsvl_sim, "SchoolBus", school_bus_state)

        # Placing the ego on the starting point
        ego_state = spawn_state(lgsvl_sim)
        ego_state = CarControl.place_car_from_the_point(dimension="horizontal",
                                                        distance=-6,
                                                        state=ego_state)
        # ego_state = CarControl.drive_ego_car(ego_state, [("vertical", 6)])
        ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

        def on_collision(agent1, agent2, contact):
            COLLISIONS.append([agent1, agent2, contact])
            sim_connection.sim.close()
            print("Exception: {} collided with {}".format(agent1, agent2))

        ego.on_collision(on_collision)
        school_bus.on_collision(on_collision)

        try:
            ego.connect_bridge(LGSVL__APOLLO_HOST, LGSVL__APOLLO_PORT)
            dv = lgsvl.dreamview.Connection(lgsvl_sim, ego,
                                            LGSVL__DREAMVIEW_HOST)
            dv.set_hd_map("CubeTown")
            dv.set_vehicle("Lincoln2017MKZ (Apollo 5.0)")
            dv.setup_apollo(TARGET.x, TARGET.z, MODULES)
            # Start the scenario
            school_bus.follow_closest_lane(follow=True, max_speed=10)
            waypoints = []
            for point in SEDAN_WPS:
                waypoints.append(
                    lgsvl.DriveWaypoint(point, NPC_SPEED,
                                        school_bus.state.transform.rotation))

            school_bus.follow(waypoints)
            sim_connection.execute(timeout=15)
        except Exception:
            sim_connection.sim.close()
            self.fail("Failed!")

        sim_connection.sim.close()
        self.assertTrue(True, True)
示例#13
0
    def test_EGO_exit_with_apollo(self):
        LGSVL__APOLLO_HOST = config("LGSVL__APOLLO_HOST")
        LGSVL__APOLLO_PORT = int(config("LGSVL__APOLLO_PORT"))
        LGSVL__DREAMVIEW_HOST = config("LGSVL__DREAMVIEW_HOST")
        MODULES = [
            'Recorder', 'Localization', 'Perception', 'Transform', 'Routing',
            'Prediction', 'Planning', 'Traffic Light', 'Control'
        ]
        EGO_TARGET = Vector(114.219772338867, -0.003660649061203,
                            -1.39988207817078)
        COLLISIONS = []

        # Setup environment
        sim_connection = SimConnection()
        lgsvl_sim = sim_connection.connect()

        NPC_state = spawn_state(lgsvl_sim)
        NPC = load_npc(lgsvl_sim, "Sedan", NPC_state)

        sedan2_state = spawn_state(lgsvl_sim)
        sedan2_state = CarControl.place_car_from_the_point(
            dimension="vertical", distance=10, state=sedan2_state)
        sedan2 = load_npc(lgsvl_sim, "Sedan", sedan2_state)
        sedan3_state = spawn_state(lgsvl_sim)
        sedan3_state = CarControl.place_car_from_the_point(
            dimension="vertical", distance=15, state=sedan3_state)
        sedan3 = load_npc(lgsvl_sim, "Sedan", sedan3_state)

        # Placing the ego on the starting point
        ego_state = spawn_state(lgsvl_sim)
        ego_state = CarControl.place_car_from_the_point(dimension="vertical",
                                                        distance=5,
                                                        state=ego_state)
        ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

        sim_connection.execute(timeout=5, debug=True, vehicles=[ego])

        def on_collision(agent1, agent2, contact):
            COLLISIONS.append([agent1, agent2, contact])
            sim_connection.sim.close()
            # print("Exception: {} collided with {}".format(agent1, agent2))
            raise Exception()

        ego.on_collision(on_collision)
        NPC.on_collision(on_collision)
        sedan2.on_collision(on_collision)
        sedan3.on_collision(on_collision)

        try:
            ego.connect_bridge(LGSVL__APOLLO_HOST, LGSVL__APOLLO_PORT)
            dv = lgsvl.dreamview.Connection(lgsvl_sim, ego,
                                            LGSVL__DREAMVIEW_HOST)
            dv.set_hd_map("CubeTown")
            dv.set_vehicle("Lincoln2017MKZ (Apollo 5.0)")
            dv.setup_apollo(EGO_TARGET.x, EGO_TARGET.z, MODULES)
            # Start the scenario
            sim_connection.execute(timeout=15)
        except Exception:
            sim_connection.sim.close()
            self.fail("Failed!")

        sim_connection.sim.close()
        self.assertTrue(True, True)
示例#14
0
    def test_EGO_exit_park_with_incoming_NPC(self):
        NPC_START = lgsvl.geometry.Vector(120, -0.0120140314102173, -2)
        NPC_TARGET = lgsvl.geometry.Vector(60, -0.0121138095855713, -2)
        NPC_SPEED = 4
        VEHICLE_SET = [
            {
                "name": "Sedan",
                "load_vehicle": load_npc,
                "distance": 0,
                "type": "Sedan"
            },
            {
                "name": "Ego",
                "load_vehicle": load_ego,
                "distance": 10,
                "type": "Lincoln2017MKZ (Apollo 5.0)"
            },
            {
                "name": "SUV",
                "load_vehicle": load_npc,
                "distance": 5,
                "type": "SUV"
            },
            {
                "name": "Jeep",
                "load_vehicle": load_npc,
                "distance": 20,
                "type": "Jeep"
            },
        ]
        # Setup environment
        sim_connection = SimConnection()
        lgsvl_sim = sim_connection.connect()
        scenario = Scenario(sim_connection)
        # Setup vehicles position
        sedan, ego, suv, jeep = scenario.generate_vehicles(
            lgsvl_sim, VEHICLE_SET)
        # Setup a new NPC
        NPC_state = spawn_state(lgsvl_sim)
        NPC_state = CarControl.place_car_on_the_point(sim=lgsvl_sim,
                                                      point=NPC_START,
                                                      state=NPC_state)
        NPC_state = CarControl.rotate_car_by_degree(state=NPC_state,
                                                    degree=-90)
        NPC = load_npc(lgsvl_sim, "Sedan", NPC_state)
        NPC.on_collision(scenario.on_collision)
        waypointsCommand = [
            lgsvl.DriveWaypoint(NPC_START, NPC_SPEED,
                                NPC_state.transform.rotation),
            lgsvl.DriveWaypoint(NPC_TARGET, NPC_SPEED,
                                NPC_state.transform.rotation)
        ]

        # Delay the scenario for 2s
        sim_connection.execute(timeout=2)
        try:
            NPC.follow(waypointsCommand)
            scenario.drive_ego(sim_connection, ego)
        except Exception:
            sim_connection.sim.close()
            self.fail("Failed!")

        # Passed!
        sim_connection.sim.close()
        self.assertTrue(True, True)
    dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST)
    dv.set_hd_map('Cubetown')
    dv.set_vehicle('Lincoln2017MKZ LGSVL')

    modules = [
        'Localization', 'Transform', 'Routing', 'Prediction', 'Planning',
        'Control'
    ]
    destination = spawns[0].destinations[0]
    dv.setup_apollo(destination.position.x, destination.position.z, modules)
    endOfRoad = destination

    POVState = lgsvl.AgentState()
    sedan_state = POVState
    sedan_state = CarControl.place_car_from_the_point(dimension="vertical",
                                                      distance=25,
                                                      state=sedan_state)
    sedan_state = CarControl.place_car_from_the_point(dimension="horizontal",
                                                      distance=-3,
                                                      state=sedan_state)
    sedan_state = CarControl.rotate_car_by_degree(state=sedan_state,
                                                  degree=180)
    POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, sedan_state)

    def on_collision(agent1, agent2, contact):
        raise lgsvl.evaluator.TestException(
            "Ego collided with {}".format(agent2))

    ego.on_collision(on_collision)
    POV.on_collision(on_collision)
    def test_EGO_encroach_NPC_speed_20_with_apollo(self):
        LGSVL__APOLLO_HOST = config("LGSVL__APOLLO_HOST")
        LGSVL__APOLLO_PORT = int(config("LGSVL__APOLLO_PORT"))
        LGSVL__DREAMVIEW_HOST = config("LGSVL__DREAMVIEW_HOST")
        MODULES = [
            'Recorder', 'Localization', 'Perception', 'Transform', 'Routing',
            'Prediction', 'Planning', 'Traffic Light', 'Control'
        ]
        TARGET = Vector(4.85003185272217, -0.0120296478271484,
                        22.7699680328369)
        COLLISIONS = []
        SEDAN_WPS = [
            Vector(-3.15000438690186, 0, 37.7700042724609),
            Vector(4.85003137588501, -0.0120288729667664, 22.7699680328369)
        ]
        NPC_SPEED = 10

        sim_connection = SimConnection(scene="CubeTown")
        lgsvl_sim = sim_connection.connect()
        # Placing the sedan
        sedan_state = spawn_state(lgsvl_sim)
        sedan_state = CarControl.place_car_from_the_point(dimension="vertical",
                                                          distance=15,
                                                          state=sedan_state)
        sedan_state = CarControl.place_car_from_the_point(
            dimension="horizontal", distance=-8, state=sedan_state)
        sedan_state = CarControl.rotate_car_by_degree(state=sedan_state,
                                                      degree=-45)
        sedan = load_npc(lgsvl_sim, "Sedan", sedan_state)

        # Placing the ego on the starting point
        ego_state = spawn_state(lgsvl_sim)
        # ego_state = CarControl.drive_ego_car(ego_state, [("vertical", 3)])
        ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)

        def on_collision(agent1, agent2, contact):
            COLLISIONS.append([agent1, agent2, contact])
            sim_connection.sim.close()
            print("Exception: {} collided with {}".format(agent1, agent2))

        ego.on_collision(on_collision)
        sedan.on_collision(on_collision)

        try:
            ego.connect_bridge(LGSVL__APOLLO_HOST, LGSVL__APOLLO_PORT)
            dv = lgsvl.dreamview.Connection(lgsvl_sim, ego,
                                            LGSVL__DREAMVIEW_HOST)
            dv.set_hd_map("CubeTown")
            dv.set_vehicle("Lincoln2017MKZ (Apollo 5.0)")
            dv.setup_apollo(TARGET.x, TARGET.z, MODULES)
            # Start the scenario
            waypoints = []
            for point in SEDAN_WPS:
                waypoints.append(
                    lgsvl.DriveWaypoint(point, NPC_SPEED,
                                        sedan.state.transform.rotation))

            sedan.follow(waypoints)
            sim_connection.execute(timeout=15)
        except Exception:
            sim_connection.sim.close()
            self.fail("Failed!")

        sim_connection.sim.close()
        self.assertTrue(True, True)