add(example): 5 lane highway with 2-lane switches

demo/tacas2023/exp11/decision_logic/inc-expr-switch-2.py

+from enum import Enum, auto
+import copy
+from typing import List
+
+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 AgentMode(Enum):
+    Normal = auto()
+    SwitchLeft = auto()
+    SwitchLeft2 = auto()
+    SwitchRight = auto()
+    SwitchRight2 = auto()
+    Brake = auto()
+    Stop = auto()
+
+class TrackMode(Enum):
+    T0 = auto()
+    T1 = auto()
+    T2 = auto()
+    T3 = auto()
+    T4 = auto()
+
+class State:
+    x = 0.0
+    y = 0.0
+    theta = 0.0
+    v = 0.0
+    agent_mode: AgentMode = AgentMode.Normal
+    track_mode: TrackMode = TrackMode.T0
+    type: LaneObjectMode = LaneObjectMode.Vehicle
+
+def car_ahead(ego, others, track, track_map, thresh_far, thresh_close):
+    return any((thresh_far > track_map.get_longitudinal_position(other.track_mode, [other.x,other.y]) - track_map.get_longitudinal_position(ego.track_mode, [ego.x,ego.y]) > thresh_close \
+            and other.track_mode == track) for other in others)
+
+def decisionLogic(ego:State, others:List[State], track_map):
+    output = copy.deepcopy(ego)
+    if ego.agent_mode == AgentMode.Normal:
+        left_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft)
+        left2_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft2)
+        right_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight)
+        right2_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight2)
+        if car_ahead(ego, others, ego.track_mode, track_map, 5, 3):
+            # Switch left if left lane is empty
+            if left_lane != None and not car_ahead(ego, others, left_lane, track_map, 8, -3):
+                output.agent_mode = AgentMode.SwitchLeft
+                output.track_mode = left_lane
+            if left2_lane != None and not car_ahead(ego, others, left2_lane, track_map, 8, -3):
+                output.agent_mode = AgentMode.SwitchLeft2
+                output.track_mode = left2_lane
+            if right_lane != None and not car_ahead(ego, others, right_lane, track_map, 8, -3):
+                output.agent_mode = AgentMode.SwitchRight
+                output.track_mode = right_lane
+            if right2_lane != None and not car_ahead(ego, others, right2_lane, track_map, 8, -3):
+                output.agent_mode = AgentMode.SwitchRight2
+                output.track_mode = right2_lane
+    else:   # If switched enough, return to normal mode
+        lat = track_map.get_lateral_distance(ego.track_mode, [ego.x, ego.y])
+        lane_width = track_map.get_lane_width(ego.track_mode)
+        if ego.agent_mode == AgentMode.SwitchLeft and lat >= (lane_width-0.2) \
+                or ego.agent_mode == AgentMode.SwitchLeft2 and lat >= (lane_width * 2-0.2) \
+                or ego.agent_mode == AgentMode.SwitchRight and lat <= -(lane_width -0.2) \
+                or ego.agent_mode == AgentMode.SwitchRight2 and lat <= -(lane_width * 2-0.2):
+            output.agent_mode = AgentMode.Normal
+            output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.Normal)
+    return output

demo/tacas2023/exp11/inc-expr-switch-2.py

+from verse.agents.example_agent import CarAgentSwitch2, NPCAgent
+from verse.map.example_map import SimpleMap4Switch2
+
+from enum import Enum, auto
+from verse.plotter.plotter2D import reachtube_tree
+from verse.scenario.scenario import Benchmark
+import functools, pprint
+pp = functools.partial(pprint.pprint, compact=True, width=130)
+from typing import List
+
+class AgentMode(Enum):
+    Normal = auto()
+    SwitchLeft = auto()
+    SwitchLeft2 = auto()
+    SwitchRight = auto()
+    SwitchRight2 = auto()
+    Brake = auto()
+    Stop = auto()
+
+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 TrackMode(Enum):
+    T0 = auto()
+    T1 = auto()
+    T2 = auto()
+    T3 = auto()
+    T4 = auto()
+
+def jerk(l: List[List[float]], x=0, y=0):
+    return [[l[0][0] - x, l[0][1] - y, *l[0][2:]], [l[1][0] + x, l[1][1] + y, *l[1][2:]]]
+
+def jerks(ls: List[List[List[float]]], js: List[List[float]]):
+    return [jerk(l, *j) for l, j in zip(ls, js)]
+
+def dupi(l: List[List[float]]):
+    return [[i, i] for i in l]
+
+def run(meas=False):
+    if bench.config.sim:
+        bench.scenario.simulator.cache_hits = (0, 0)
+    else:
+        bench.scenario.verifier.tube_cache_hits = (0,0)
+        bench.scenario.verifier.trans_cache_hits = (0,0)
+    traces = bench.run(60, 0.1)
+
+    if bench.config.dump:
+        traces.dump_tree()
+        traces.dump("main.json") 
+        traces.dump("tree2.json" if meas else "tree1.json") 
+
+    if bench.config.plot and meas:
+        fig = go.Figure()
+        if bench.config.sim:
+            fig = simulation_tree(traces, bench.scenario.map, fig, 1, 2, print_dim_list=[1, 2])
+        else:
+            fig = reachtube_tree(traces, bench.scenario.map, fig, 1, 2, [1, 2], 'lines',combine_rect=5)
+        fig.show()
+
+    if meas:
+        bench.report()
+
+if __name__ == "__main__":
+    import sys
+    bench = Benchmark(sys.argv)
+    input_code_name = './demo/tacas2023/exp11/decision_logic/inc-expr-switch-2.py'
+
+    if bench.config.plot:
+        import plotly.graph_objects as go
+        from verse.plotter.plotter2D import simulation_tree
+
+    smarts = [1, 3, 8]
+    for i in range(8):
+        id = i + 1
+        if id in smarts:
+            bench.scenario.add_agent(CarAgentSwitch2(f"car{id}", file_name=input_code_name))
+        else:
+            bench.scenario.add_agent(NPCAgent(f"car{id}"))
+    tmp_map = SimpleMap4Switch2()
+    bench.scenario.set_map(tmp_map)
+    mode_inits = ([
+            (AgentMode.Normal, TrackMode.T1), (AgentMode.Normal, TrackMode.T1),
+            (AgentMode.Normal, TrackMode.T0), (AgentMode.Normal, TrackMode.T0),
+            (AgentMode.Normal, TrackMode.T1), (AgentMode.Normal, TrackMode.T2),
+            (AgentMode.Normal, TrackMode.T2), (AgentMode.Normal, TrackMode.T2),
+        ],
+        [(LaneObjectMode.Vehicle,) for _ in range(8)])
+
+    poses = [
+        [0, 0, 0, 1.0], [10, 0, 0, 0.5],
+        [14, 3, 0, 0.6], [20, 3, 0, 0.5],
+        [30, 0, 0, 0.5], [28.5, -3, 0, 0.5],
+        [39.5, -3, 0, 0.5], [30, -3, 0, 0.6],
+    ]
+    _jerks = [[0, 0.05] if i + 1 in smarts else [] for i in range(8)]
+    cont_inits = dupi(poses)
+    if not bench.config.sim:
+        cont_inits = jerks(cont_inits, _jerks)
+    bench.scenario.set_init(cont_inits, *mode_inits)
+
+    args = bench.config.args
+
+    if 'b' in args:
+        run(True)
+    elif 'r' in args:
+        run()
+        run(True)
+    elif 'n' in args:
+        run()
+        poses[6][0] = 50
+        cont_inits = dupi(poses)
+        if not bench.config.sim:
+            cont_inits = jerks(cont_inits, _jerks)
+        bench.scenario.set_init(cont_inits, *mode_inits)
+        run(True)
+    elif '3' in args:
+        run()
+        bench.scenario.agent_dict["car3"] = CarAgentSwitch2('car3', file_name=input_code_name.replace(".py", "-fsw7.py"))
+        run(True)
+    elif '8' in args:
+        run()
+        bench.scenario.agent_dict["car8"] = CarAgentSwitch2('car8', file_name=input_code_name.replace(".py", "-fsw4.py"))
+        run(True)

verse/agents/example_agent/car_agent.py

@@ -145,3 +145,37 @@ class WeirdCarAgent(CarAgent):
             return -self.gain, 0
         else:
             return 0, 0
+
+class CarAgentSwitch2(CarAgent):
+    def __init__(self, id, code = None, file_name = None):
+        super().__init__(id, code, file_name)
+
+    def action_handler(self, mode: List[str], state, lane_map:LaneMap)->Tuple[float, float]:
+        x,y,theta,v = state
+        vehicle_mode, vehicle_lane = mode
+        vehicle_pos = np.array([x,y])
+        a = 0
+        lane_width = lane_map.get_lane_width(vehicle_lane) 
+        d = -lane_map.get_lateral_distance(vehicle_lane, vehicle_pos)
+        if vehicle_mode == "Normal" or vehicle_mode == 'Stop':
+            pass
+        elif vehicle_mode == "SwitchLeft":
+            d += lane_width
+        elif vehicle_mode == "SwitchRight":
+            d -= lane_width
+        elif vehicle_mode == "SwitchLeft2":
+            d += lane_width * 2
+        elif vehicle_mode == "SwitchRight2":
+            d -= lane_width * 2
+        elif vehicle_mode == "Brake":
+            a = max(-self.accel, -v)
+        elif vehicle_mode == "Accel":
+            a = min(self.accel, self.speed - v)
+        else:
+            raise ValueError(f'Invalid mode: {vehicle_mode}')
+
+        heading = lane_map.get_lane_heading(vehicle_lane, vehicle_pos)
+        psi = wrap_to_pi(heading - theta)
+        steering = psi + np.arctan2(0.45*d, v)
+        steering = np.clip(steering, -0.61, 0.61)
+        return steering, a  

verse/map/example_map/simple_map2.py

+from typing import Optional
 from verse.map import LaneMap, LaneSegment, StraightLane, CircularLane, Lane
 
 import numpy as np
@@ -132,6 +133,36 @@ class SimpleMap4(LaneMap):
     def right_lane(self,lane_mode):
         return self.right_dict[lane_mode]
 
+class SimpleMap4Switch2(LaneMap):
+    def __init__(self):
+        super().__init__()
+        self.add_lanes([Lane(f"T{i}", [StraightLane(f"seg{i}", [0, 6 - 3 * i], [100, 6 - 3 * i], 3)]) for i in range(5)])
+
+    def h(self, lane_idx: str, mode_src: str, mode_dest: str) -> Optional[str]:
+        src_sw, dst_sw = mode_src.startswith("Switch"), mode_dest.startswith("Switch")
+        if src_sw:
+            if dst_sw:
+                return None
+            else:
+                return "T" + lane_idx[2]
+        else:
+            if dst_sw:
+                typ = mode_dest[6:]
+                ind = int(lane_idx[1])
+                if typ == "Left" and ind > 0:
+                    new_ind = ind - 1
+                if typ == "Left2" and ind > 1:
+                    new_ind = ind - 2
+                elif typ == "Right" and ind < 4:
+                    new_ind = ind + 1
+                elif typ == "Right2" and ind < 3:
+                    new_ind = ind + 2
+                else:
+                    return None
+                return f"M{ind}{new_ind}"
+            else:
+                return lane_idx
+
 class SimpleMap5(LaneMap):
     def __init__(self):
         super().__init__()