diff --git a/.gitignore b/.gitignore
index c1b479cf0f3866b92a858a77189114554fbb49b7..c862ec277a8e310ff9b8c4b26eecfe81111ee68a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -7,3 +7,4 @@ venv/
**.egg-info/
.VSCodeCounter/
dist/dryvr_plus_plus-0.1-py3.8.egg
+tmp/
\ No newline at end of file
diff --git a/demo/ball_bounces.py b/demo/ball_bounces.py
index d952f629cdd58c989b4ab1b01ab8f84bcaa7bfbb..941302cfa62d44ac6abfe353f249a8a748b97c60 100644
--- a/demo/ball_bounces.py
+++ b/demo/ball_bounces.py
@@ -4,15 +4,15 @@ from typing import List
# from dryvr_plus_plus.scene_verifier.map.lane import Lane
class BallMode(Enum):
- # TODO: Any model should have at least one mode
+ # NOTE: Any model should have at least one mode
Normal = auto()
# TODO: The one mode of this automation is called "Normal" and auto assigns it an integer value.
# Ultimately for simple models we would like to write
# E.g., Mode = makeMode(Normal, bounce,...)
-class LaneMode(Enum):
- Lane0 = auto()
- # TODO: For now this is a dummy notion of Lane
+# class LaneMode(Enum):
+# Lane0 = auto()
+# #For now this is a dummy notion of Lane
class State:
'''Defines the state variables of the model
@@ -23,8 +23,7 @@ class State:
vx = 0.0
vy = 0.0
mode: BallMode
- lane_mode: LaneMode
- def __init__(self, x, y, vx, vy, ball_mode:BallMode, lane_mode:LaneMode):
+ def __init__(self, x, y, vx, vy, ball_mode:BallMode):
pass
def controller(ego:State, others:State):
@@ -59,42 +58,35 @@ def controller(ego:State, others:State):
from dryvr_plus_plus.example.example_agent.ball_agent import BallAgent
from dryvr_plus_plus.scene_verifier.scenario.scenario import Scenario
from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap3
-from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor4
from dryvr_plus_plus.plotter.plotter2D import *
import plotly.graph_objects as go
+from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor4
+from dryvr_plus_plus.scene_verifier.sensor.base_sensor import BaseSensor
if __name__ == "__main__":
- ball_controller = 'ball_bounces.py'
+ ball_controller = './ball_bounces.py'
bouncingBall = Scenario()
myball1 = BallAgent('red-ball', file_name=ball_controller)
myball2 = BallAgent('green-ball', file_name=ball_controller)
bouncingBall.add_agent(myball1)
bouncingBall.add_agent(myball2)
- #
- tmp_map = SimpleMap3()
- bouncingBall.set_map(tmp_map)
- # TODO: If there is no useful map, then we should not have to write this.
- # Default to some empty map
- bouncingBall.set_sensor(FakeSensor4())
- # TODO: There should be a way to default to some well-defined sensor
- # for any model, without having to write an explicit sensor
bouncingBall.set_init(
[
[[5, 10, 2, 2], [5, 10, 2, 2]],
[[15, 1, 1, -2], [15, 1, 1, -2]]
],
[
- (BallMode.Normal, LaneMode.Lane0),
- (BallMode.Normal, LaneMode.Lane0)
+ (BallMode.Normal,),
+ (BallMode.Normal,)
]
)
# TODO: WE should be able to initialize each of the balls separately
# this may be the cause for the VisibleDeprecationWarning
# TODO: Longer term: We should initialize by writing expressions like "-2 \leq myball1.x \leq 5"
# "-2 \leq myball1.x + myball2.x \leq 5"
- traces = bouncingBall.simulate(20)
+ traces = bouncingBall.simulate(40)
# TODO: There should be a print({traces}) function
fig = go.Figure()
- fig = plotly_simulation_anime(traces, tmp_map, fig)
+ fig = plotly_simulation_anime(traces, fig=fig)
fig.show()
diff --git a/demo/demo3.py b/demo/demo3.py
index 1ba4fc08711617729d18001c41ec933f58d52ccf..4500d5a003208005853b4d228217455a12374397 100644
--- a/demo/demo3.py
+++ b/demo/demo3.py
@@ -55,7 +55,6 @@ if __name__ == "__main__":
scenario.add_agent(car)
tmp_map = SimpleMap3()
scenario.set_map(tmp_map)
- scenario.set_sensor(FakeSensor3())
scenario.set_init(
[
[[0, -0.2, 0, 1.0],[0.01, 0.2, 0, 1.0]],
diff --git a/demo/demo4.py b/demo/demo4.py
index 86d69d5e7ca521262216dc42c3b5877d97729d6c..db61a07ce61085bf52b8be47483e8b5be84b0d08 100644
--- a/demo/demo4.py
+++ b/demo/demo4.py
@@ -4,6 +4,7 @@ from dryvr_plus_plus.scene_verifier.scenario.scenario import Scenario
from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap2, SimpleMap3, SimpleMap4, SimpleMap5, SimpleMap6
from dryvr_plus_plus.plotter.plotter2D import *
from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor3
+from dryvr_plus_plus.scene_verifier.sensor.base_sensor import BaseSensor
import matplotlib.pyplot as plt
import plotly.graph_objects as go
@@ -60,7 +61,6 @@ if __name__ == "__main__":
scenario.add_agent(car)
tmp_map = SimpleMap4()
scenario.set_map(tmp_map)
- scenario.set_sensor(FakeSensor3())
scenario.set_init(
[
[[0, -0.2, 0, 1.0],[0.05, 0.2, 0, 1.0]],
diff --git a/demo/demo5.py b/demo/demo5.py
index dd1323af43628def007ea2bc75929c46a9e817de..b4412151744fe597af87660b19a93227c10b7c7c 100644
--- a/demo/demo5.py
+++ b/demo/demo5.py
@@ -4,6 +4,7 @@ from dryvr_plus_plus.scene_verifier.scenario.scenario import Scenario
from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap2, SimpleMap3, SimpleMap5, SimpleMap6
from dryvr_plus_plus.plotter.plotter2D import *
from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor3
+from dryvr_plus_plus.scene_verifier.sensor.base_sensor import BaseSensor
import matplotlib.pyplot as plt
import plotly.graph_objects as go
@@ -57,7 +58,6 @@ if __name__ == "__main__":
# scenario.add_agent(car)
tmp_map = SimpleMap3()
scenario.set_map(tmp_map)
- scenario.set_sensor(FakeSensor3())
scenario.set_init(
[
[[0, -0.0, 0, 1.0],[0.0, 0.0, 0, 1.0]],
@@ -72,25 +72,19 @@ if __name__ == "__main__":
(VehicleMode.Normal, LaneMode.Lane0),
]
)
- traces = scenario.simulate(70)
- # traces = scenario.verify(60)
+ # traces = scenario.simulate(70)
+ traces = scenario.verify(60)
- # 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)
- # fig = plot_reachtube_tree(traces, 'car3', 1, [2], 'r', fig)
- # fig = plot_reachtube_tree(traces, 'car4', 1, [2], 'r', fig)
- # for traces in res_list:
- # # generate_simulation_anime(traces, tmp_map, fig)
- # fig = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig)
- # fig = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig)
- # fig = plot_simulation_tree(traces, 'car3', 1, [2], 'r', fig)
- # fig = plot_simulation_tree(traces, 'car4', 1, [2], 'r', fig)
- # plt.show()
+ 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)
+ fig = plot_reachtube_tree(traces, 'car3', 1, [2], 'r', fig)
+ fig = plot_reachtube_tree(traces, 'car4', 1, [2], 'r', fig)
+ plt.show()
- fig = go.Figure()
- fig = plotly_simulation_anime(traces, tmp_map, fig)
- fig.show()
+ # fig = go.Figure()
+ # fig = plotly_simulation_anime(traces, tmp_map, fig)
+ # fig.show()
diff --git a/demo/demo6.py b/demo/demo6.py
index 0d4e78211167d347bad883ce79dbd033a8d804c6..4030efdfb6a143907cda881758a824f4c210244a 100644
--- a/demo/demo6.py
+++ b/demo/demo6.py
@@ -4,6 +4,7 @@ from dryvr_plus_plus.scene_verifier.scenario.scenario import Scenario
from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap2, SimpleMap3, SimpleMap4, SimpleMap5, SimpleMap6
from dryvr_plus_plus.plotter.plotter2D import *
from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor3
+from dryvr_plus_plus.scene_verifier.sensor.base_sensor import BaseSensor
import matplotlib.pyplot as plt
import plotly.graph_objects as go
@@ -62,7 +63,6 @@ if __name__ == "__main__":
scenario.add_agent(car)
tmp_map = SimpleMap4()
scenario.set_map(tmp_map)
- scenario.set_sensor(FakeSensor3())
scenario.set_init(
[
[[0, -0.0, 0, 1.0],[0.0, 0.0, 0, 1.0]],
diff --git a/demo/demo7.py b/demo/demo7.py
index fd2b0b39a6877ae0a8540da85699bafc1ef6de3e..b3b5c0c43f928138d6260386fbd83b2c96d64642 100644
--- a/demo/demo7.py
+++ b/demo/demo7.py
@@ -6,6 +6,7 @@ from dryvr_plus_plus.scene_verifier.scenario.scenario import Scenario
from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap2, SimpleMap3, SimpleMap4, SimpleMap5, SimpleMap6
from dryvr_plus_plus.plotter.plotter2D import *
from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor3
+from dryvr_plus_plus.scene_verifier.sensor.base_sensor import BaseSensor
import matplotlib.pyplot as plt
import plotly.graph_objects as go
@@ -45,7 +46,7 @@ class State:
if __name__ == "__main__":
- input_code_name = './demo/example_controller8.py'
+ input_code_name = './example_controller8.py'
scenario = Scenario()
car = CarAgent('car1', file_name=input_code_name)
@@ -66,7 +67,6 @@ if __name__ == "__main__":
scenario.add_agent(car)
tmp_map = SimpleMap4()
scenario.set_map(tmp_map)
- scenario.set_sensor(FakeSensor3())
scenario.set_init(
[
[[0, -0.0, 0, 1.0],[0.0, 0.0, 0, 1.0]],
@@ -89,7 +89,7 @@ if __name__ == "__main__":
(VehicleMode.Normal, LaneMode.Lane3),
]
)
- traces = scenario.simulate(20)
+ traces = scenario.simulate(80)
# traces = scenario.verify(15)
# fig = plt.figure(2)
diff --git a/dryvr_plus_plus/plotter/plotter2D.py b/dryvr_plus_plus/plotter/plotter2D.py
index d31ad37ae57b694b2e8249a194d886d1b264b927..d72a2b35f60325e9ffbf856aba90ecd7aa980c5f 100644
--- a/dryvr_plus_plus/plotter/plotter2D.py
+++ b/dryvr_plus_plus/plotter/plotter2D.py
@@ -499,7 +499,8 @@ def plotly_simulation_anime(root, map=None, fig=None):
fig_dict["layout"]["sliders"] = [sliders_dict]
fig = go.Figure(fig_dict)
- fig = plotly_map(map, 'g', fig)
+ if map is not None:
+ fig = plotly_map(map, 'g', fig)
i = 0
queue = [root]
while queue != []:
diff --git a/dryvr_plus_plus/scene_verifier/automaton/guard.py b/dryvr_plus_plus/scene_verifier/automaton/guard.py
index 56fe2f1d83063df9c6615c4de2bf946367355f1e..cfb409a8cbc619789070b321a9796c622a9ee3e4 100644
--- a/dryvr_plus_plus/scene_verifier/automaton/guard.py
+++ b/dryvr_plus_plus/scene_verifier/automaton/guard.py
@@ -86,7 +86,7 @@ class GuardExpressionAst:
for guard in guard_list:
self.ast_list.append(copy.deepcopy(guard.ast))
self.cont_variables = {}
- self.varDict = {'t':Real('t')}
+ self.varDict = {}
def _build_guard(self, guard_str, agent):
"""
@@ -187,7 +187,6 @@ class GuardExpressionAst:
cur_solver.pop()
res = True
- # TODO: If the reachtube completely fall inside guard, break
tmp_solver = Solver()
tmp_solver.add(Not(cur_solver.assertions()[0]))
for symbol in symbols:
@@ -292,6 +291,11 @@ class GuardExpressionAst:
value = self._generate_z3_expression_node(node.operand)
if isinstance(node.op, ast.USub):
return -value
+ elif isinstance(node.op, ast.Not):
+ z3_str = 'Not('+value+')'
+ return z3_str
+ else:
+ raise NotImplementedError(f"UnaryOp {node.op} is not supported")
else:
# For other cases, we can return the expression directly
expr = astunparse.unparse(node)
@@ -621,6 +625,10 @@ class GuardExpressionAst:
else:
root = ast.parse('False').body[0].value
else:
+ for mode_name in agent.controller.modes:
+ if res in agent.controller.modes[mode_name]:
+ res = mode_name+'.'+res
+ break
root = ast.parse(str(res)).body[0].value
return res, root
else:
diff --git a/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrcore.py b/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrcore.py
index f3bd8861809044283c381e7665c12a1f150a685c..a52a0f324c7a2c0c9678ddba3f682f850e9d4205 100644
--- a/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrcore.py
+++ b/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrcore.py
@@ -294,11 +294,9 @@ def calc_bloated_tube(
# The major
if bloating_method == GLOBAL:
- # TODO: Replace this with ReachabilityEngine.get_reachtube_segment
cur_reach_tube: np.ndarray = get_reachtube_segment(np.array(traces), np.array(cur_delta), "PWGlobal")
# cur_reach_tube: np.ndarray = ReachabilityEngine.get_reachtube_segment_wrapper(np.array(traces), np.array(cur_delta))
elif bloating_method == PW:
- # TODO: Replace this with ReachabilityEngine.get_reachtube_segment
cur_reach_tube: np.ndarray = get_reachtube_segment(np.array(traces), np.array(cur_delta), "PW")
# cur_reach_tube: np.ndarray = ReachabilityEngine.get_reachtube_segment_wrapper(np.array(traces), np.array(cur_delta))
else:
diff --git a/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrmain.py b/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrmain.py
index 5e24eb989dc20b754e5bb38805b3590c658eaf9f..0b185c3c1f9018b9c687700594741c9bc072c46c 100644
--- a/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrmain.py
+++ b/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrmain.py
@@ -199,7 +199,6 @@ def verify(data, sim_function, param_config=None):
)
# Use the guard to calculate the next initial set
- # TODO: Made this function return multiple next_init
next_init, truncated_result, transit_time = guard.guard_reachtube(
cur_bloated_tube,
cur_guard_str,
@@ -209,7 +208,6 @@ def verify(data, sim_function, param_config=None):
continue
# Reset the next initial set
- # TODO: Made this function handle multiple next_init
next_init = reset.reset_set(cur_reset_str, next_init[0], next_init[1])
# Build next mode stack
@@ -220,7 +218,6 @@ def verify(data, sim_function, param_config=None):
start_time=transit_time+cur_mode_stack.start_time
)
next_mode_stack.parent = cur_mode_stack
- # TODO: Append all next_init into the next_mode_stack
next_mode_stack.stack.append(InitialSet(next_init[0], next_init[1]))
next_mode_stack.bloated_tube.append(
cur_mode_stack.bloated_tube[0] + '->' + buildModeStr(graph, cur_successor))
diff --git a/dryvr_plus_plus/scene_verifier/dryvr/core/guard.py b/dryvr_plus_plus/scene_verifier/dryvr/core/guard.py
index 2996f61bcc914234488f9957fe9c8c7549192856..79f1b70677881254fe6d2676f41485c3fdaef17a 100644
--- a/dryvr_plus_plus/scene_verifier/dryvr/core/guard.py
+++ b/dryvr_plus_plus/scene_verifier/dryvr/core/guard.py
@@ -171,7 +171,6 @@ class Guard:
guard_set_lower.append(lower_bound)
guard_set_upper.append(upper_bound)
- # TODO: If the reachtube completely fall inside guard, break
tmp_solver = Solver()
tmp_solver.add(Not(cur_solver.assertions()[0]))
for symbol in symbols:
@@ -204,7 +203,6 @@ class Guard:
for k in range(1, len(guard_set_lower[0])):
init_lower[k - 1] = min(init_lower[k - 1], guard_set_lower[j][k])
init_upper[k - 1] = max(init_upper[k - 1], guard_set_upper[j][k])
- # TODO: Treat tau as a special clock variable that don't have variation
# init_upper[0] = init_lower[0]
# Return next initial Set, the result tube, and the true transit time
diff --git a/dryvr_plus_plus/scene_verifier/scenario/scenario.py b/dryvr_plus_plus/scene_verifier/scenario/scenario.py
index 8a064619751afb34c28d241e6582e47b5c38abc8..1698eecc2fc2ee82341d935e38c98e101c8293f8 100644
--- a/dryvr_plus_plus/scene_verifier/scenario/scenario.py
+++ b/dryvr_plus_plus/scene_verifier/scenario/scenario.py
@@ -15,6 +15,8 @@ from dryvr_plus_plus.scene_verifier.analysis.verifier import Verifier
from dryvr_plus_plus.scene_verifier.map.lane_map import LaneMap
from dryvr_plus_plus.scene_verifier.utils.utils import *
from dryvr_plus_plus.scene_verifier.analysis.analysis_tree_node import AnalysisTreeNode
+from dryvr_plus_plus.scene_verifier.sensor.base_sensor import BaseSensor
+from dryvr_plus_plus.scene_verifier.map.lane_map import LaneMap
class Scenario:
def __init__(self):
@@ -23,8 +25,8 @@ class Scenario:
self.verifier = Verifier()
self.init_dict = {}
self.init_mode_dict = {}
- self.map = None
- self.sensor = None
+ self.map = LaneMap()
+ self.sensor = BaseSensor()
def set_sensor(self, sensor):
self.sensor = sensor
@@ -44,7 +46,7 @@ class Scenario:
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']:
+ if 'LaneMode' in agent.controller.modes and 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))
diff --git a/dryvr_plus_plus/scene_verifier/sensor/__init__.py b/dryvr_plus_plus/scene_verifier/sensor/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/dryvr_plus_plus/scene_verifier/sensor/base_sensor.py b/dryvr_plus_plus/scene_verifier/sensor/base_sensor.py
new file mode 100644
index 0000000000000000000000000000000000000000..642e18f05c6d916df8e3d5e8aead73c5a5077e36
--- /dev/null
+++ b/dryvr_plus_plus/scene_verifier/sensor/base_sensor.py
@@ -0,0 +1,68 @@
+import numpy as np
+
+def sets(d, thing, attrs, vals):
+ d.update({thing + "." + k: v for k, v in zip(attrs, vals)})
+
+def adds(d, thing, attrs, vals):
+ for k, v in zip(attrs, vals):
+ if thing + '.' + k not in d:
+ d[thing + '.' + k] = [v]
+ else:
+ d[thing + '.' + k].append(v)
+
+def set_states_2d(cnts, disc, thing, val, cont_var, disc_var):
+ state, mode = val
+ sets(cnts, thing, cont_var, state[1:5])
+ sets(disc, thing, disc_var, mode)
+
+def set_states_3d(cnts, disc, thing, val, cont_var, disc_var):
+ state, mode = val
+ transp = np.transpose(np.array(state)[:, 1:5])
+ assert len(transp) == 4
+ sets(cnts, thing, cont_var, transp)
+ sets(disc, thing, disc_var, mode)
+
+def add_states_2d(cont, disc, thing, val, cont_var, disc_var):
+ state, mode = val
+ adds(cont, thing, cont_var, state[1:5])
+ adds(disc, thing, disc_var, mode)
+
+def add_states_3d(cont, disc, thing, val, cont_var, disc_var):
+ state, mode = val
+ transp = np.transpose(np.array(state)[:, 1:5])
+ assert len(transp) == 4
+ adds(cont, thing, cont_var, transp)
+ adds(disc, thing, disc_var, mode)
+
+class BaseSensor():
+ # The baseline sensor is omniscient. Each agent can get the state of all other agents
+ def sense(self, scenario, agent, state_dict, lane_map):
+ cont = {}
+ disc = {}
+ len_dict = {'others':len(state_dict)-1}
+ tmp = np.array(list(state_dict.values())[0][0])
+ if tmp.ndim < 2:
+ for agent_id in state_dict:
+ if agent_id == agent.id:
+ if agent.controller.vars_dict:
+ cont_var = agent.controller.vars_dict['ego']['cont']
+ disc_var = agent.controller.vars_dict['ego']['disc']
+ set_states_2d(cont, disc, 'ego', state_dict[agent_id], cont_var, disc_var)
+ else:
+ if agent.controller.vars_dict:
+ cont_var = agent.controller.vars_dict['others']['cont']
+ disc_var = agent.controller.vars_dict['others']['disc']
+ add_states_2d(cont, disc, 'others', state_dict[agent_id], cont_var, disc_var)
+ else:
+ for agent_id in state_dict:
+ if agent_id == agent.id:
+ if agent.controller.vars_dict:
+ cont_var = agent.controller.vars_dict['ego']['cont']
+ disc_var = agent.controller.vars_dict['ego']['disc']
+ set_states_3d(cont, disc, "ego", state_dict[agent_id], cont_var, disc_var)
+ else:
+ if agent.controller.vars_dict:
+ cont_var = agent.controller.vars_dict['others']['cont']
+ disc_var = agent.controller.vars_dict['others']['disc']
+ add_states_3d(cont, disc, 'others', state_dict[agent_id], cont_var, disc_var)
+ return cont, disc, len_dict
\ No newline at end of file