diff --git a/demo.py b/demo.py
new file mode 100644
index 0000000000000000000000000000000000000000..3b250382bbcc8937228e6b89aecf1ec5e3aa1bd6
--- /dev/null
+++ b/demo.py
@@ -0,0 +1,66 @@
+from src.example.example_agent.car_agent import CarAgent
+from src.scene_verifier.scenario.scenario import Scenario
+from src.example.example_map.simple_map2 import SimpleMap2, SimpleMap3, SimpleMap5, SimpleMap6
+from src.plotter.plotter2D import *
+from src.example.example_sensor.fake_sensor import FakeSensor2
+
+import matplotlib.pyplot as plt
+import numpy as np
+from enum import Enum, auto
+
+class VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ Lane1 = auto()
+ Lane2 = auto()
+
+class State:
+ x = 0.0
+ y = 0.0
+ theta = 0.0
+ v = 0.0
+ vehicle_mode: VehicleMode = VehicleMode.Normal
+ lane_mode: LaneMode = LaneMode.Lane0
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode):
+ self.data = []
+
+if __name__ == "__main__":
+ input_code_name = 'example_controller2.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)
+ tmp_map = SimpleMap2()
+ scenario.set_map(tmp_map)
+ scenario.set_sensor(FakeSensor2())
+ scenario.set_init(
+ [
+ [[0, -0.2, 0, 1.0],[0.1, 0.2, 0, 1.0]],
+ [[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
+ ],
+ [
+ (VehicleMode.Normal, LaneMode.Lane0),
+ (VehicleMode.Normal, LaneMode.Lane0),
+ ]
+ )
+ res_list = scenario.simulate_multi(40,1)
+ # traces = scenario.verify(40)
+
+ fig = plt.figure(2)
+ # fig = plot_map(tmp_map, 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
+ # fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
+ for traces in res_list:
+ fig = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig)
+ fig = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig)
+
+
+ plt.show()
diff --git a/demo2.py b/demo2.py
new file mode 100644
index 0000000000000000000000000000000000000000..bc5f264e04a0903c399e7eb479738b5a2ac32ee7
--- /dev/null
+++ b/demo2.py
@@ -0,0 +1,66 @@
+from src.example.example_agent.car_agent import CarAgent, NPCAgent
+from src.scene_verifier.scenario.scenario import Scenario
+from src.example.example_map.simple_map2 import SimpleMap2, SimpleMap3, SimpleMap5, SimpleMap6
+from src.plotter.plotter2D import *
+from src.example.example_sensor.fake_sensor import FakeSensor2
+
+import matplotlib.pyplot as plt
+import numpy as np
+from enum import Enum, auto
+
+class VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ Lane1 = auto()
+ Lane2 = auto()
+
+class State:
+ x = 0.0
+ y = 0.0
+ theta = 0.0
+ v = 0.0
+ vehicle_mode: VehicleMode = VehicleMode.Normal
+ lane_mode: LaneMode = LaneMode.Lane0
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode):
+ self.data = []
+
+if __name__ == "__main__":
+ input_code_name = 'example_controller1.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)
+ tmp_map = SimpleMap3()
+ scenario.set_map(tmp_map)
+ scenario.set_sensor(FakeSensor2())
+ scenario.set_init(
+ [
+ [[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
+ [[0, -0.2, 0, 1.0],[0.1, 0.2, 0, 1.0]],
+ ],
+ [
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ ]
+ )
+ # res_list = scenario.simulate_multi(40,1)
+ traces = scenario.verify(40)
+
+ fig = plt.figure(2)
+ # fig = plot_map(tmp_map, 'g', fig)
+ fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
+ fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
+ # for traces in res_list:
+ # fig = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig)
+ # fig = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig)
+
+
+ plt.show()
diff --git a/example_controller1.py b/example_controller1.py
new file mode 100644
index 0000000000000000000000000000000000000000..788d4b77980e7a691bd79cb0653f72122cc80931
--- /dev/null
+++ b/example_controller1.py
@@ -0,0 +1,50 @@
+from enum import Enum, auto
+import copy
+from src.scene_verifier.map.lane_map import LaneMap
+
+class VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ Lane1 = auto()
+ Lane2 = auto()
+
+class State:
+ x = 0.0
+ y = 0.0
+ theta = 0.0
+ v = 0.0
+ vehicle_mode: VehicleMode = VehicleMode.Normal
+ lane_mode: LaneMode = LaneMode.Lane0
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode):
+ self.data = []
+
+def controller(ego:State, other:State, lane_map:LaneMap):
+ output = copy.deepcopy(ego)
+ if ego.vehicle_mode == VehicleMode.Normal:
+ if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 3 \
+ and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 5 \
+ and ego.lane_mode == other.lane_mode:
+ if lane_map.has_left(ego.lane_mode):
+ output.vehicle_mode = VehicleMode.SwitchLeft
+ if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 3 \
+ and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 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.get_lateral_distance(ego.lane_mode, [ego.x, 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.get_lateral_distance(ego.lane_mode, [ego.x, ego.y]) <= -2.5:
+ output.vehicle_mode = VehicleMode.Normal
+ output.lane_mode = lane_map.right_lane(ego.lane_mode)
+
+ return output
+
diff --git a/example_controller2.py b/example_controller2.py
new file mode 100644
index 0000000000000000000000000000000000000000..28b416f9915cc5ab0e8487be4df39193c3c84675
--- /dev/null
+++ b/example_controller2.py
@@ -0,0 +1,36 @@
+from enum import Enum, auto
+import copy
+from src.scene_verifier.map.lane_map import LaneMap
+
+class VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ Lane1 = auto()
+ Lane2 = auto()
+
+class State:
+ x = 0.0
+ y = 0.0
+ theta = 0.0
+ v = 0.0
+ vehicle_mode: VehicleMode = VehicleMode.Normal
+ lane_mode: LaneMode = LaneMode.Lane0
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode):
+ self.data = []
+
+def controller(ego:State, other:State, lane_map:LaneMap):
+ output = copy.deepcopy(ego)
+ if ego.vehicle_mode == VehicleMode.Normal:
+ if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 0 \
+ and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 3 \
+ and ego.lane_mode == other.lane_mode:
+ output.vehicle_mode = VehicleMode.Brake
+
+ return output
+
diff --git a/example_two_car_lane_switch2.py b/example_two_car_lane_switch2.py
deleted file mode 100644
index eb264b47d5d43a07d8904712a0f703fecc172a9d..0000000000000000000000000000000000000000
--- a/example_two_car_lane_switch2.py
+++ /dev/null
@@ -1,86 +0,0 @@
-from enum import Enum, auto
-from src.scene_verifier.map.lane_map import LaneMap
-
-class VehicleMode(Enum):
- Normal = auto()
- SwitchLeft = auto()
- SwitchRight = auto()
- Brake = auto()
-
-
-class LaneMode(Enum):
- Lane0 = auto()
- Lane1 = auto()
- Lane2 = auto()
-
-
-class State:
- x = 0.0
- y = 0.0
- theta = 0.0
- v = 0.0
- vehicle_mode: VehicleMode = VehicleMode.Normal
- lane_mode: LaneMode = LaneMode.Lane0
-
- def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode):
- self.data = []
-
-
-def controller(ego: State, other: State, lane_map:LaneMap):
- output = ego
- if ego.vehicle_mode == VehicleMode.Normal:
- 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.get_lateral_distance(ego.lane_mode, [ego.x, 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.get_lateral_distance(ego.lane_mode, [ego.x, 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.scene_verifier.scenario.scenario import Scenario
-from src.example.example_map.simple_map2 import SimpleMap3
-from src.plotter.plotter2D import *
-from src.example.example_sensor.fake_sensor import FakeSensor2
-
-import matplotlib.pyplot as plt
-import numpy as np
-
-if __name__ == "__main__":
- input_code_name = 'example_two_car_lane_switch2.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_map(SimpleMap3())
- scenario.set_sensor(FakeSensor2())
- scenario.set_init(
- [
- [[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
- [[0, -0.2, 0, 1.0],[0.1, 0.2, 0, 1.0]],
- ],
- [
- (VehicleMode.Normal, LaneMode.Lane1),
- (VehicleMode.Normal, LaneMode.Lane1)
- ]
- )
- # traces = scenario.simulate_once(40)
- traces = scenario.verify(33)
-
- fig = plt.figure(1)
- fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
- fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
-
- plt.show()
diff --git a/example_two_car_lane_switch3.py b/example_two_car_lane_switch3.py
index 4b5072739a61d856b37c0e8db2e5e9000fa51dca..abda32ce5617938a77082234cf787b639d5308fc 100644
--- a/example_two_car_lane_switch3.py
+++ b/example_two_car_lane_switch3.py
@@ -9,6 +9,7 @@ class VehicleMode(Enum):
Brake = auto()
class LaneMode(Enum):
+ pass
Lane0 = auto()
Lane1 = auto()
Lane2 = auto()
@@ -61,7 +62,7 @@ if __name__ == "__main__":
scenario.add_agent(car)
car = CarAgent('car2', file_name=input_code_name)
scenario.add_agent(car)
- scenario.add_map(SimpleMap6())
+ scenario.set_map(SimpleMap6())
scenario.set_sensor(FakeSensor2())
scenario.set_init(
[
diff --git a/example_two_car_lane_switch4.py b/example_two_car_lane_switch4.py
index 8776cb62bc961d54f49eb9bdf7f506a811e7e9d9..788d4b77980e7a691bd79cb0653f72122cc80931 100644
--- a/example_two_car_lane_switch4.py
+++ b/example_two_car_lane_switch4.py
@@ -48,46 +48,3 @@ def controller(ego:State, other:State, lane_map:LaneMap):
return output
-from src.example.example_agent.car_agent import CarAgent
-from src.scene_verifier.scenario.scenario import Scenario
-from src.example.example_map.simple_map2 import SimpleMap5, SimpleMap6
-from src.plotter.plotter2D import *
-from src.example.example_sensor.fake_sensor import FakeSensor2
-
-import matplotlib.pyplot as plt
-import numpy as np
-
-if __name__ == "__main__":
- input_code_name = 'example_two_car_lane_switch4.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)
- tmp_map = SimpleMap6()
- scenario.add_map(tmp_map)
- scenario.set_sensor(FakeSensor2())
- scenario.set_init(
- [
- [[14, 0, 0, 0.5],[14, 0, 0, 0.5]],
- [[0, -0.2, 0, 1.0],[0.1, 0.2, 0, 1.0]],
- ],
- [
- (VehicleMode.Normal, LaneMode.Lane1),
- (VehicleMode.Normal, LaneMode.Lane1)
- ]
- )
- # res_list = scenario.simulate_multi(40,10)
- traces = scenario.verify(40)
-
- fig = plt.figure(2)
- fig = plot_map(tmp_map, 'g', fig)
- fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
- fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
- # for traces in res_list:
- # fig = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig)
- # fig = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig)
-
-
- plt.show()
diff --git a/requirements.txt b/requirements.txt
index 4ee97379ead57793e215254068cd17e4c1134adc..2c26c89eb2c9ad080d6502168f48a6f5fbc42e7c 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -1,12 +1,12 @@
-numpy~=1.22.3
+numpy~=1.22.1
scipy~=1.8.0
-matplotlib~=3.4.3
+matplotlib~=3.4.2
polytope~=0.2.3
pyvista~=0.32.1
-networkx~=2.8
+networkx~=2.2
sympy~=1.6.2
six~=1.14.0
astunparse~=1.6.3
treelib~=1.6.1
z3-solver~=4.8.17.0
-igraph~=0.9.10
\ No newline at end of file
+igraph~=0.9.10
diff --git a/src/example/example_agent/car_agent.py b/src/example/example_agent/car_agent.py
index 0382553fc4c7fef01ef610441f035b2417f3aa67..aef59a0c740c649d66207cc63d9a88345f566d9c 100644
--- a/src/example/example_agent/car_agent.py
+++ b/src/example/example_agent/car_agent.py
@@ -6,6 +6,75 @@ from scipy.integrate import ode
from src.scene_verifier.agents.base_agent import BaseAgent
from src.scene_verifier.map.lane_map import LaneMap
+class NPCAgent(BaseAgent):
+ def __init__(self, id, code = None, file_name = None):
+ npc_code_str = "\
+class VehicleMode(Enum):\n\
+ Normal = auto()\n\
+\n\
+class LaneMode(Enum):\n\
+ Lane0 = auto()\n\
+ Lane1 = auto()\n\
+ Lane2 = auto()\n\
+\n\
+class State:\n\
+ x = 0.0\n\
+ y = 0.0\n\
+ theta = 0.0\n\
+ v = 0.0\n\
+ vehicle_mode: VehicleMode = VehicleMode.Normal\n\
+ lane_mode: LaneMode = LaneMode.Lane0\n\
+\n\
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode):\n\
+ self.data = []\n\
+\n\
+def controller(ego:State, other:State, lane_map:LaneMap):\n\
+ output = copy.deepcopy(ego)\n\
+ return output\n\
+ "
+ super().__init__(id, npc_code_str, None)
+
+ @staticmethod
+ def dynamic(t, state, u):
+ x, y, theta, v = state
+ delta, a = u
+ x_dot = v*np.cos(theta+delta)
+ y_dot = v*np.sin(theta+delta)
+ theta_dot = v/1.75*np.sin(delta)
+ v_dot = a
+ return [x_dot, y_dot, theta_dot, v_dot]
+
+ def action_handler(self, mode, state, lane_map:LaneMap)->Tuple[float, float]:
+ x,y,theta,v = state
+ vehicle_mode = mode[0]
+ vehicle_lane = mode[1]
+ vehicle_pos = np.array([x,y])
+ d = -lane_map.get_lateral_distance(vehicle_lane, vehicle_pos)
+ psi = lane_map.get_lane_heading(vehicle_lane, vehicle_pos)-theta
+ steering = psi + np.arctan2(0.45*d, v)
+ steering = np.clip(steering, -0.61, 0.61)
+ a = 0
+ return steering, a
+
+ def TC_simulate(self, mode, initialCondition, time_bound, lane_map:LaneMap=None)->np.ndarray:
+ mode = mode.split(',')
+ time_step = 0.05
+ time_bound = float(time_bound)
+ number_points = int(np.ceil(time_bound/time_step))
+ t = [i*time_step for i in range(0,number_points)]
+
+ init = initialCondition
+ trace = [[0]+init]
+ for i in range(len(t)):
+ steering, a = self.action_handler(mode, init, lane_map)
+ r = ode(self.dynamic)
+ r.set_initial_value(init).set_f_params([steering, a])
+ res:np.ndarray = r.integrate(r.t + time_step)
+ init = res.flatten().tolist()
+ trace.append([t[i] + time_step] + init)
+
+ return np.array(trace)
+
class CarAgent(BaseAgent):
def __init__(self, id, code = None, file_name = None):
super().__init__(id, code, file_name)
@@ -39,7 +108,7 @@ class CarAgent(BaseAgent):
def TC_simulate(self, mode, initialCondition, time_bound, lane_map:LaneMap=None)->np.ndarray:
mode = mode.split(',')
- time_step = 0.01
+ time_step = 0.05
time_bound = float(time_bound)
number_points = int(np.ceil(time_bound/time_step))
t = [i*time_step for i in range(0,number_points)]
diff --git a/src/example/example_map/simple_map2.py b/src/example/example_map/simple_map2.py
index 023fd6da128bb2e618beda69c49edd333b4ac5d3..b23d73ed2089b0cbbf7e0326e439610cb51bd60e 100644
--- a/src/example/example_map/simple_map2.py
+++ b/src/example/example_map/simple_map2.py
@@ -4,6 +4,18 @@ from src.scene_verifier.map.lane import Lane
import numpy as np
+class SimpleMap2(LaneMap):
+ def __init__(self):
+ super().__init__()
+ segment0 = StraightLane(
+ 'seg0',
+ [0,0],
+ [100,0],
+ 3
+ )
+ lane0 = Lane('Lane0', [segment0])
+ self.add_lanes([lane0])
+
class SimpleMap3(LaneMap):
def __init__(self):
super().__init__()
@@ -36,54 +48,6 @@ class SimpleMap3(LaneMap):
self.right_lane_dict[lane0.id].append(lane1.id)
self.right_lane_dict[lane1.id].append(lane2.id)
-class SimpleMap4(LaneMap):
- def __init__(self):
- super().__init__()
- segment0 = StraightLane(
- 'Seg0',
- [0,3],
- [30,3],
- 3
- )
- segment1 = StraightLane(
- 'Seg1',
- [30,3],
- [50,23],
- 3
- )
- lane0 = Lane('Lane0', [segment0, segment1])
- segment0 = StraightLane(
- 'seg0',
- [0,0],
- [33,0],
- 3
- )
- segment1 = StraightLane(
- 'seg0',
- [33,0],
- [53,20],
- 3
- )
- lane1 = Lane('Lane1', [segment0, segment1])
- segment0 = StraightLane(
- 'seg0',
- [0,-3],
- [36,-3],
- 3
- )
- segment1 = StraightLane(
- 'seg0',
- [36,-3],
- [56,17],
- 3
- )
- lane2 = Lane('Lane2', [segment0, segment1])
- self.add_lanes([lane0, lane1, lane2])
- self.left_lane_dict[lane1.id].append(lane0.id)
- self.left_lane_dict[lane2.id].append(lane1.id)
- self.right_lane_dict[lane0.id].append(lane1.id)
- self.right_lane_dict[lane1.id].append(lane2.id)
-
class SimpleMap5(LaneMap):
def __init__(self):
super().__init__()
diff --git a/src/scene_verifier/analysis/simulator.py b/src/scene_verifier/analysis/simulator.py
index a7f6bcf2abe9c577d2c97c50e2cf918ab8f75fdd..95532b12292077b9f5878d98bd5cc4bf86ea8aee 100644
--- a/src/scene_verifier/analysis/simulator.py
+++ b/src/scene_verifier/analysis/simulator.py
@@ -75,6 +75,8 @@ class Simulator:
# copy the traces that are not under transition
for transition in transitions:
transit_agent_idx, src_mode, dest_mode, next_init, idx = transition
+ if dest_mode is None:
+ continue
# next_node = AnalysisTreeNode(trace = {},init={},mode={},agent={}, child = [], start_time = 0)
next_node_mode = copy.deepcopy(node.mode)
next_node_mode[transit_agent_idx] = dest_mode
diff --git a/src/scene_verifier/analysis/verifier.py b/src/scene_verifier/analysis/verifier.py
index 600d9ee47885b3dd4bbb27a55be1882db2d63601..d78e2ee9e12bd9f5821890432a808a348465df73 100644
--- a/src/scene_verifier/analysis/verifier.py
+++ b/src/scene_verifier/analysis/verifier.py
@@ -73,12 +73,16 @@ class Verifier:
for transition in all_possible_transitions:
transit_agent_idx, src_mode, dest_mode, next_init, idx = transition
start_idx, end_idx = idx
-
+
truncated_trace = {}
for agent_idx in node.agent:
truncated_trace[agent_idx] = node.trace[agent_idx][start_idx*2:]
if end_idx > max_end_idx:
max_end_idx = end_idx
+
+ if dest_mode is None:
+ continue
+
next_node_mode = copy.deepcopy(node.mode)
next_node_mode[transit_agent_idx] = dest_mode
next_node_agent = node.agent
diff --git a/src/scene_verifier/scenario/scenario.py b/src/scene_verifier/scenario/scenario.py
index b351c752412e8556c0d46bb8ff0f391b365e65a3..0c061a5779ccab54802960aead1a8004619ff315 100644
--- a/src/scene_verifier/scenario/scenario.py
+++ b/src/scene_verifier/scenario/scenario.py
@@ -1,6 +1,7 @@
from typing import Tuple
import copy
import itertools
+import warnings
import numpy as np
@@ -26,12 +27,27 @@ class Scenario:
def set_sensor(self, sensor):
self.sensor = sensor
- def add_map(self, lane_map:LaneMap):
+ def set_map(self, lane_map:LaneMap):
self.map = lane_map
+ # Update the lane mode field in the agent
+ for agent_id in self.agent_dict:
+ agent = self.agent_dict[agent_id]
+ self.update_agent_lane_mode(agent, lane_map)
def add_agent(self, agent:BaseAgent):
+ if self.map is not None:
+ # Update the lane mode field in the agent
+ self.update_agent_lane_mode(agent, self.map)
self.agent_dict[agent.id] = agent
+ def update_agent_lane_mode(self, agent: BaseAgent, lane_map: LaneMap):
+ for lane_id in lane_map.lane_dict:
+ if lane_id not in agent.controller.modes['LaneMode']:
+ agent.controller.modes['LaneMode'].append(lane_id)
+ mode_vals = list(agent.controller.modes.values())
+ agent.controller.vertices = list(itertools.product(*mode_vals))
+ agent.controller.vertexStrings = [','.join(elem) for elem in agent.controller.vertices]
+
def set_init(self, init_list, init_mode_list):
assert len(init_list) == len(self.agent_dict)
assert len(init_mode_list) == len(self.agent_dict)
@@ -147,6 +163,9 @@ class Scenario:
if agent_id not in reset_dict:
reset_dict[agent_id] = {}
reset_idx_dict[agent_id] = {}
+ if not dest_list:
+ warnings.warn(f"Guard hit for mode {node.mode[agent_id]} for agent {agent_id} without available next mode")
+ dest_list.append(None)
for dest in dest_list:
if dest not in reset_dict[agent_id]:
reset_dict[agent_id][dest] = []
@@ -239,9 +258,13 @@ class Scenario:
for cts_variable in continuous_variable_dict:
tmp = tmp.replace(cts_variable, str(continuous_variable_dict[cts_variable]))
next_init[i] = eval(tmp)
- all_dest = itertools.product(*possible_dest)
+ all_dest = list(itertools.product(*possible_dest))
+ if not all_dest:
+ warnings.warn(f"Guard hit for mode {agent_mode} for agent {agent_id} without available next mode")
+ all_dest.append(None)
for dest in all_dest:
- dest = ','.join(dest)
+ if dest is not None:
+ dest = ','.join(dest)
next_transition = (
agent_id, agent_mode, dest, next_init,
)