diff --git a/demo/tacas2023/exp11/decision_logic/inc-expr-switch-2.py b/demo/tacas2023/exp11/decision_logic/inc-expr-switch-2.py
new file mode 100644
index 0000000000000000000000000000000000000000..138e0442e306efa30b18436506affac1e2ae20f7
--- /dev/null
+++ b/demo/tacas2023/exp11/decision_logic/inc-expr-switch-2.py
@@ -0,0 +1,71 @@
+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
diff --git a/demo/tacas2023/exp11/inc-expr-switch-2.py b/demo/tacas2023/exp11/inc-expr-switch-2.py
new file mode 100644
index 0000000000000000000000000000000000000000..dc38405f155e0c2bad4260cc4d364de4089d7790
--- /dev/null
+++ b/demo/tacas2023/exp11/inc-expr-switch-2.py
@@ -0,0 +1,127 @@
+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)
diff --git a/verse/agents/example_agent/car_agent.py b/verse/agents/example_agent/car_agent.py
index d6a1d04f8aaeb3b4c01aa98b5cdcaf2db4d8544f..619739fcc0d11e92a8fcd4e668ef385ed503c79c 100644
--- a/verse/agents/example_agent/car_agent.py
+++ b/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
diff --git a/verse/map/example_map/simple_map2.py b/verse/map/example_map/simple_map2.py
index f7293dbb6a45350478eb7fb9fb0619ba6199d45d..7d7996797e65ad959ba32f3caeeed4f1c23be0dc 100644
--- a/verse/map/example_map/simple_map2.py
+++ b/verse/map/example_map/simple_map2.py
@@ -1,3 +1,4 @@
+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__()