experiment: adding (stop) sign to controller

example_two_car_sign_lane_switch.py

+from enum import Enum, auto
+import copy
+
+class LaneObjectMode(Enum):
+    Vehicle = auto()
+    Ped = auto()        # Pedestrians
+    Sign = auto()       # Signs, stop signs, merge, yield etc.
+    Signal = auto()     # Traffic lights
+    Obstacle = auto()   # Static (to road/lane) obstacles
+
+class VehicleMode(Enum):
+    Normal = auto()
+    SwitchLeft = auto()
+    SwitchRight = auto()
+    Brake = auto()
+
+
+class LaneMode(Enum):
+    Lane0 = auto()
+    Lane1 = auto()
+    Lane2 = auto()
+
+class State:
+    x: float
+    y: float
+    theta: float
+    v: float
+    vehicle_mode: VehicleMode
+    lane_mode: LaneMode
+    obj_mode: LaneObjectMode
+
+    def __init__(self, x: float = 0, y: float = 0, theta: float = 0, v: float = 0, vehicle_mode: VehicleMode = VehicleMode.Normal, lane_mode: LaneMode = LaneMode.Lane0, obj_mode: LaneObjectMode = LaneObjectMode.Vehicle):
+        self.data = []
+        self.x = x
+        self.y = y
+        self.theta = theta
+        self.v = v
+        self.vehicle_mode = vehicle_mode
+        self.lane_mode = lane_mode
+        self.obj_mode = obj_mode
+
+def controller(ego: State, other: State, sign: State, lane_map):
+    output = copy.deepcopy(ego)
+    if sign.y - ego.y < 3 and sign.lane_mode == ego.lane_mode:
+        output.vehicle_mode = VehicleMode.SwitchLeft
+        return output
+    if ego.vehicle_mode == VehicleMode.Normal:
+        # A simple example to demonstrate how our tool can handle change in controller
+        # if ego.x > 30 and ego.lane_mode == LaneMode.Lane0:
+        #     output.vehicle_mode = VehicleMode.SwitchRight
+        
+        if other.x - ego.x > 3 and other.x - ego.x < 5 and ego.lane_mode == other.lane_mode:
+            if lane_map.has_left(ego.lane_mode):
+                output.vehicle_mode = VehicleMode.SwitchLeft
+        if other.x - ego.x > 3 and other.x - ego.x < 5 and ego.lane_mode == other.lane_mode:
+            if lane_map.has_right(ego.lane_mode):
+                output.vehicle_mode = VehicleMode.SwitchRight
+    if ego.vehicle_mode == VehicleMode.SwitchLeft:
+        if  lane_map.lane_geometry(ego.lane_mode) - ego.y <= -2.5:
+            output.vehicle_mode = VehicleMode.Normal
+            output.lane_mode = lane_map.left_lane(ego.lane_mode)
+    if ego.vehicle_mode == VehicleMode.SwitchRight:
+        if lane_map.lane_geometry(ego.lane_mode)-ego.y >= 2.5:
+            output.vehicle_mode = VehicleMode.Normal
+            output.lane_mode = lane_map.right_lane(ego.lane_mode)
+
+    return output
+
+
+from src.example.example_agent.car_agent import CarAgent
+from src.example.example_agent.sign_agent import SignAgent
+from src.scene_verifier.scenario.scenario import Scenario
+from src.example.example_map.simple_map import SimpleMap2
+from src.plotter.plotter2D import plot_tree
+from src.example.example_sensor.fake_sensor import FakeSensor2
+
+import matplotlib.pyplot as plt
+
+if __name__ == "__main__":
+    input_code_name = 'example_two_car_lane_switch.py'
+    scenario = Scenario()
+
+    car = CarAgent('car1', file_name=input_code_name)
+    scenario.add_agent(car)
+    car = CarAgent('car2', file_name=input_code_name)
+    scenario.add_agent(car)
+    scenario.add_agent(SignAgent("sign", file_name=input_code_name))
+    scenario.add_map(SimpleMap2())
+    scenario.set_sensor(FakeSensor2())
+    scenario.set_init(
+        [
+            [[10, 0, 0, 0.5],[10, 0, 0, 0.5]], 
+            [[-0.2, -0.2, 0, 1.0],[0.2, 0.2, 0, 1.0]],
+            [[20, 3, 0, 0], [20, 3, 0, 0]],
+        ],
+        [
+            (VehicleMode.Normal, LaneMode.Lane1),
+            (VehicleMode.Normal, LaneMode.Lane1),
+            (VehicleMode.Normal, LaneMode.Lane2),
+        ]
+    )
+    # simulator = Simulator()
+    # traces = scenario.simulate(40)
+    traces = scenario.verify(40)
+
+    fig = plt.figure()
+    fig = plot_tree(traces, 'car1', 1, [2], 'b', fig)
+    fig = plot_tree(traces, 'car2', 1, [2], 'r', fig)
+
+    plt.show()
+
+    # plt.plot([0, 40], [3, 3], 'g')
+    # plt.plot([0, 40], [0, 0], 'g')
+    # plt.plot([0, 40], [-3, -3], 'g')
+
+    # queue = [traces]
+    # while queue != []:
+    #     node = queue.pop(0)
+    #     traces = node.trace
+    #     # for agent_id in traces:
+    #     agent_id = 'car2'
+    #     trace = np.array(traces[agent_id])
+    #     plt.plot(trace[:, 1], trace[:, 2], 'r')
+
+    #     agent_id = 'car1'
+    #     trace = np.array(traces[agent_id])
+    #     plt.plot(trace[:, 1], trace[:, 2], 'b')
+
+    #     # if node.child != []:
+    #     queue += node.child
+    # plt.show()

src/example/example_agent/sign_agent.py

+from src.scene_verifier.agents.base_agent import BaseAgent
+import numpy as np
+
+class SignAgent(BaseAgent):
+    def __init__(self, id, code = None, file_name = None):
+        super().__init__(id, code, file_name)
+
+    def TC_simulate(self, mode, init, time_horizon, map=None):
+        time_step = 0.01
+        number_points = int(np.ceil(float(time_horizon)/time_step))
+        t = [i*time_step for i in range(0,number_points)]
+        trace = [[0] + init] + [[i + time_step] + init for i in t]
+        return trace

src/example/example_sensor/fake_sensor.py

@@ -14,6 +14,20 @@ class FakeSensor1:
         disc['ego.lane_mode'] = mode[1]
         return cnts, disc
 
+def sets(d, thing, attrs, vals):
+    d.update({thing + "." + k: v for k, v in zip(attrs, vals)})
+
+def set_states_2d(cnts, disc, thing, val):
+    state, mode = val
+    sets(cnts, thing, ["x", "y", "theta", "v"], state[1:5])
+    sets(disc, thing, ["vehicle_mode", "lane_mode"], mode.split(","))
+
+def set_states_3d(cnts, disc, thing, val):
+    state, mode = val
+    transp = np.transpose(np.array(state)[:, 1:5])
+    assert len(transp) == 4
+    sets(cnts, thing, ["x", "y", "theta", "v"], transp)
+    sets(disc, thing, ["vehicle_mode", "lane_mode"], mode.split(","))
 
 class FakeSensor2:
     def sense(self, scenario, agent, state_dict, lane_map):
@@ -22,77 +36,29 @@ class FakeSensor2:
         tmp = np.array(state_dict['car1'][0])
         if tmp.ndim < 2:
             if agent.id == 'car1':
-                state = state_dict['car1'][0]
-                mode = state_dict['car1'][1].split(',')
-                cnts['ego.x'] = state[1]
-                cnts['ego.y'] = state[2]
-                cnts['ego.theta'] = state[3]
-                cnts['ego.v'] = state[4]
-                disc['ego.vehicle_mode'] = mode[0]
-                disc['ego.lane_mode'] = mode[1]
-
-                state = state_dict['car2'][0]
-                mode = state_dict['car2'][1].split(',')
-                cnts['other.x'] = state[1]
-                cnts['other.y'] = state[2]
-                cnts['other.theta'] = state[3]
-                cnts['other.v'] = state[4]
-                disc['other.vehicle_mode'] = mode[0]
-                disc['other.lane_mode'] = mode[1]
+                set_states_2d(cnts, disc, "ego", state_dict["car1"])
+                set_states_2d(cnts, disc, "other", state_dict["car2"])
+                set_states_2d(cnts, disc, "sign", state_dict["sign"])
             elif agent.id == 'car2':
-                state = state_dict['car2'][0]
-                mode = state_dict['car2'][1].split(',')
-                cnts['ego.x'] = state[1]
-                cnts['ego.y'] = state[2]
-                cnts['ego.theta'] = state[3]
-                cnts['ego.v'] = state[4]
-                disc['ego.vehicle_mode'] = mode[0]
-                disc['ego.lane_mode'] = mode[1]
-
-                state = state_dict['car1'][0]
-                mode = state_dict['car1'][1].split(',')
-                cnts['other.x'] = state[1]
-                cnts['other.y'] = state[2]
-                cnts['other.theta'] = state[3]
-                cnts['other.v'] = state[4]
-                disc['other.vehicle_mode'] = mode[0]
-                disc['other.lane_mode'] = mode[1]
+                set_states_2d(cnts, disc, "other", state_dict["car1"])
+                set_states_2d(cnts, disc, "ego", state_dict["car2"])
+                set_states_2d(cnts, disc, "sign", state_dict["sign"])
+            elif agent.id == 'sign':
+                set_states_2d(cnts, disc, "ego", state_dict["sign"])
+                set_states_2d(cnts, disc, "other", state_dict["car2"])
+                set_states_2d(cnts, disc, "sign", state_dict["sign"])
             return cnts, disc
         else:
             if agent.id == 'car1':
-                state = state_dict['car1'][0]
-                mode = state_dict['car1'][1].split(',')
-                cnts['ego.x'] = [state[0][1], state[1][1]]
-                cnts['ego.y'] = [state[0][2], state[1][2]]
-                cnts['ego.theta'] = [state[0][3], state[1][3]]
-                cnts['ego.v'] = [state[0][4], state[1][4]]
-                disc['ego.vehicle_mode'] = mode[0]
-                disc['ego.lane_mode'] = mode[1]
-
-                state = state_dict['car2'][0]
-                mode = state_dict['car2'][1].split(',')
-                cnts['other.x'] = [state[0][1], state[1][1]]
-                cnts['other.y'] = [state[0][2], state[1][2]]
-                cnts['other.theta'] = [state[0][3], state[1][3]]
-                cnts['other.v'] = [state[0][4], state[1][4]]
-                disc['other.vehicle_mode'] = mode[0]
-                disc['other.lane_mode'] = mode[1]
+                set_states_3d(cnts, disc, "ego", state_dict["car1"])
+                set_states_3d(cnts, disc, "other", state_dict["car2"])
+                set_states_3d(cnts, disc, "sign", state_dict["sign"])
             elif agent.id == 'car2':
-                state = state_dict['car2'][0]
-                mode = state_dict['car2'][1].split(',')
-                cnts['ego.x'] = [state[0][1], state[1][1]]
-                cnts['ego.y'] = [state[0][2], state[1][2]]
-                cnts['ego.theta'] = [state[0][3], state[1][3]]
-                cnts['ego.v'] = [state[0][4], state[1][4]]
-                disc['ego.vehicle_mode'] = mode[0]
-                disc['ego.lane_mode'] = mode[1]
-
-                state = state_dict['car1'][0]
-                mode = state_dict['car1'][1].split(',')
-                cnts['other.x'] = [state[0][1], state[1][1]]
-                cnts['other.y'] = [state[0][2], state[1][2]]
-                cnts['other.theta'] = [state[0][3], state[1][3]]
-                cnts['other.v'] = [state[0][4], state[1][4]]
-                disc['other.vehicle_mode'] = mode[0]
-                disc['other.lane_mode'] = mode[1]
+                set_states_3d(cnts, disc, "other", state_dict["car1"])
+                set_states_3d(cnts, disc, "ego", state_dict["car2"])
+                set_states_3d(cnts, disc, "sign", state_dict["sign"])
+            elif agent.id == 'sign':
+                set_states_3d(cnts, disc, "ego", state_dict["sign"])
+                set_states_3d(cnts, disc, "other", state_dict["car2"])
+                set_states_3d(cnts, disc, "sign", state_dict["sign"])
             return cnts, disc