diff --git a/.gitignore b/.gitignore
index e0ecce94ebcc235743ed2bf3c674ad3210e66733..c862ec277a8e310ff9b8c4b26eecfe81111ee68a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,4 +5,6 @@ __pycache__/
venv/
.DS_Store
**.egg-info/
-.VSCodeCounter/
\ No newline at end of file
+.VSCodeCounter/
+dist/dryvr_plus_plus-0.1-py3.8.egg
+tmp/
\ No newline at end of file
diff --git a/README.md b/README.md
index 5797d2972c28affb8279bdb788c1a3e1c1e90949..34bed013c4507fc2ca8b9d303e03fa765e2b9fcf 100644
--- a/README.md
+++ b/README.md
@@ -5,6 +5,15 @@ The package requires python 3.8+. The package can be installed using pip
```
python3 -m pip install -e .
```
+To update the dependencies, setup.py or requirement.txt can be used.
+
+```
+python3 setup.py install
+```
+or
+```
+pip install -r requirements.txt
+```
## Examples
The package comes with two controller examples inside folder ```demo/```
diff --git a/demo/ball_bounces.py b/demo/ball_bounces.py
new file mode 100644
index 0000000000000000000000000000000000000000..455314ca29df2bf2b2551a80c0859e93a70352cb
--- /dev/null
+++ b/demo/ball_bounces.py
@@ -0,0 +1,100 @@
+from enum import Enum, auto
+import copy
+from typing import List
+# from dryvr_plus_plus.scene_verifier.map.lane import Lane
+
+class BallMode(Enum):
+ # 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()
+# #For now this is a dummy notion of Lane
+
+class State:
+ '''Defines the state variables of the model
+ Both discrete and continuous variables
+ '''
+ x:float
+ y = 0.0
+ vx = 0.0
+ vy = 0.0
+ mode: BallMode
+ def __init__(self, x, y, vx, vy, ball_mode:BallMode):
+ pass
+
+def controller(ego:State, others:State):
+ '''Computes the possible mode transitions'''
+ output = copy.deepcopy(ego)
+ '''TODO: Ego and output variable names should be flexible but
+ currently these are somehow harcoded with the sensor'''
+ # Stores the prestate first
+ if ego.x<0:
+ output.vx = -ego.vx
+ output.x=0
+ if ego.y<0:
+ output.vy = -ego.vy
+ output.y=0
+ if ego.x>20:
+ # TODO: Q. If I change this to ego.x >= 20 then the model does not work.
+ # I suspect this is because the same transition can be take many, many times.
+ # We need to figure out a clean solution
+ output.vx = -ego.vx
+ output.x=20
+ if ego.y>20:
+ output.vy = -ego.vy
+ output.y=20
+ ''' if ego.x - others[1].x < 1 and ego.y - others[1].y < 1:
+ output.vy = -ego.vy
+ output.vx = -ego.vx'''
+ # TODO: We would like to be able to write something like this, but currently not allowed.
+ return output
+
+
+
+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.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__":
+ ''' Defining and using a scenario involves the following 5 easy steps:
+ 1. creating a basic scenario object with Scenario()
+ 2. defining the agents that will populate the object, here we have two ball agents
+ 3. adding the agents to the scenario using .add_agent()
+ 4. initializing the agents for this scenario.
+ Note that agents are only initialized *in* a scenario, not individually outside a scenario
+ 5. genetating the simulation traces or computing the reachable states
+ '''
+ bouncingBall = Scenario()
+ ball_controller = './ball_bounces.py'
+ myball1 = BallAgent('red-ball', file_name=ball_controller)
+ myball2 = BallAgent('green-ball', file_name=ball_controller)
+ bouncingBall.add_agent(myball1)
+ bouncingBall.add_agent(myball2)
+ bouncingBall.set_init(
+ [
+ [[5, 10, 2, 2], [5, 10, 2, 2]],
+ [[15, 1, 1, -2], [15, 1, 1, -2]]
+ ],
+ [
+ (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(40)
+ # TODO: There should be a print({traces}) function
+ fig = go.Figure()
+ fig = plotly_simulation_anime(traces, fig=fig)
+ fig.show()
+
diff --git a/demo/demo1.py b/demo/demo1.py
index 53c582dc41ee951cf1ac4047d5bf9cb03dd14e1c..44b98844ff3bae7c5c5b80e087f242ee420cd85a 100644
--- a/demo/demo1.py
+++ b/demo/demo1.py
@@ -3,8 +3,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_new import *
from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor2
-
-import matplotlib.pyplot as plt
+import plotly.graph_objects as go
import numpy as np
from enum import Enum, auto
@@ -55,26 +54,8 @@ if __name__ == "__main__":
(VehicleMode.Normal, LaneMode.Lane1),
]
)
- res_list = scenario.simulate_multi(10, 1)
- # traces = scenario.verify(10)
-
- # fig = plt.figure(2)
- # fig = plot_map(tmp_map, 'g', fig)
- # plt.show()
- # # fig = plot_reachtube_tree(traces, 'car1', 0, [1], 'b', fig, (1000,-1000), (1000,-1000))
- # # fig = plot_reachtube_tree(traces, 'car2', 0, [1], 'r', fig)
- # # AnalysisTreeNode
- # for traces in res_list:
- # # fig = plot_simulation_tree(
- # # traces, 'car1', 0, [1], 'b', fig, (1000, -1000), (1000, -1000))
- # fig = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig)
- # # generate_simulation_anime(traces, tmp_map, fig)
-
- # plt.show()
+ traces = scenario.simulate(10, 0.01)
fig = go.Figure()
- for traces in res_list:
- # plotly_map(tmp_map, 'g', fig)
- fig = test_simu_anime(traces, tmp_map, fig, 1, 2, 'lines')
- # fig = plotly_simulation_anime(traces, tmp_map, fig)
+ fig = test_simu_anime(traces, tmp_map, fig, 1, 2, 'lines')
fig.show()
diff --git a/demo/demo2.py b/demo/demo2.py
index 29491a64ea1cfbaf54a468f9085392e410bf84c2..2386b599c4e6e2f6fb3338c18664db1533c60665 100644
--- a/demo/demo2.py
+++ b/demo/demo2.py
@@ -5,7 +5,8 @@ from dryvr_plus_plus.plotter.plotter2D import *
from dryvr_plus_plus.plotter.plotter2D_new import *
from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor2
import plotly.graph_objects as go
-import matplotlib.pyplot as plt
+# import matplotlib.pyplot as plt
+
import numpy as np
from enum import Enum, auto
@@ -56,36 +57,9 @@ if __name__ == "__main__":
(VehicleMode.Normal, LaneMode.Lane1),
]
)
- # res_list = scenario.simulate_multi(40,1)
-
- # traces = scenario.simulate(40)
-
- # fig = plt.figure(2)
- # fig = plot_map(tmp_map, 'g', fig)
- # # fig = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig)
- # # fig = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig)
- # fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
- # fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
- # plt.show()
- # # fig1 = plt.figure(2)
- # fig = generate_simulation_anime(traces, tmp_map, fig)
- # plt.show()
- # fig = go.Figure()
- # fig = plotly_simulation_tree(traces, 'car1', 1, [2], 'b', fig)
- # fig.show()
- # traces = scenario.simulate(40)
- # fig = go.Figure()
- # # fig = plotly_simulation_anime(traces, tmp_map, fig)
- # fig.show()
-
- # fig = go.Figure()
- # fig = plotly_reachtube_tree_v2(traces, 'car1', 1, [2], 'blue', fig)
- # fig = plotly_reachtube_tree_v2(traces, 'car2', 1, [2], 'red', fig)
- # fig.show()
traces = scenario.simulate(30)
fig = go.Figure()
fig = test_simu_anime(traces, tmp_map, fig, 1, 2, 'lines')
- # fig = plotly_simulation_anime(traces, tmp_map, fig)
- # # fig = plotly_reachtube_tree_v2(traces, 'car2', 1, [2], 'red', fig)
fig.show()
+
diff --git a/demo/demo3.py b/demo/demo3.py
new file mode 100644
index 0000000000000000000000000000000000000000..a459d2ecf1799151f86ef91b974ddcc3dbc5eb1a
--- /dev/null
+++ b/demo/demo3.py
@@ -0,0 +1,86 @@
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent, NPCAgent
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent
+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
+
+import matplotlib.pyplot as plt
+import plotly.graph_objects as go
+import numpy as np
+from enum import Enum, 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 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
+ type_mode: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type_mode: LaneObjectMode):
+ pass
+
+
+if __name__ == "__main__":
+ input_code_name = './example_controller4.py'
+ scenario = Scenario()
+
+ car = CarAgent('car1', file_name=input_code_name)
+ scenario.add_agent(car)
+ car = NPCAgent('car2')
+ scenario.add_agent(car)
+ car = NPCAgent('car3')
+ scenario.add_agent(car)
+ car = NPCAgent('car4')
+ scenario.add_agent(car)
+ tmp_map = SimpleMap3()
+ scenario.set_map(tmp_map)
+ scenario.set_init(
+ [
+ [[0, -0.2, 0, 1.0],[0.01, 0.2, 0, 1.0]],
+ [[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
+ [[20, 3, 0, 0.5],[20, 3, 0, 0.5]],
+ [[30, 0, 0, 0.5],[30, 0, 0, 0.5]],
+ ],
+ [
+ (VehicleMode.Normal, LaneMode.Lane1, LaneObjectMode.Vehicle),
+ (VehicleMode.Normal, LaneMode.Lane1, LaneObjectMode.Vehicle),
+ (VehicleMode.Normal, LaneMode.Lane0, LaneObjectMode.Vehicle),
+ (VehicleMode.Normal, LaneMode.Lane1, LaneObjectMode.Vehicle),
+ ]
+ )
+ traces = scenario.simulate(70, 0.05)
+ # traces = scenario.verify(70, 0.05)
+
+ # 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()
+
diff --git a/demo/demo4.py b/demo/demo4.py
new file mode 100644
index 0000000000000000000000000000000000000000..a0025e590cdc268c8ae6d27accc5bed50bd614d0
--- /dev/null
+++ b/demo/demo4.py
@@ -0,0 +1,98 @@
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent, NPCAgent
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent
+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
+import numpy as np
+from enum import Enum, 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 VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ Lane1 = auto()
+ Lane2 = auto()
+ Lane3 = 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
+ type: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type: LaneObjectMode):
+ pass
+
+
+if __name__ == "__main__":
+ input_code_name = './example_controller5.py'
+ scenario = Scenario()
+
+ car = CarAgent('car1', file_name=input_code_name)
+ scenario.add_agent(car)
+ car = NPCAgent('car2')
+ scenario.add_agent(car)
+ car = NPCAgent('car3')
+ scenario.add_agent(car)
+ car = NPCAgent('car4')
+ scenario.add_agent(car)
+ car = NPCAgent('car5')
+ scenario.add_agent(car)
+ car = NPCAgent('car6')
+ scenario.add_agent(car)
+ tmp_map = SimpleMap4()
+ scenario.set_map(tmp_map)
+ scenario.set_init(
+ [
+ [[0, -0.2, 0, 1.0],[0.05, 0.2, 0, 1.0]],
+ [[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
+ [[20, 3, 0, 0.5],[20, 3, 0, 0.5]],
+ [[30, 0, 0, 0.5],[30, 0, 0, 0.5]],
+ [[23, -3, 0, 0.5],[23, -3, 0, 0.5]],
+ [[40, -6, 0, 0.5],[40, -6, 0, 0.5]],
+ ],
+ [
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane0),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane2),
+ (VehicleMode.Normal, LaneMode.Lane3),
+ ]
+ )
+ traces = scenario.simulate(80, 0.05)
+ # traces = scenario.verify(80, 0.05)
+
+ # 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)
+ # fig = plot_reachtube_tree(traces, 'car5', 1, [2], 'r', fig)
+ # fig = plot_reachtube_tree(traces, 'car6', 1, [2], 'r', fig)
+ # plt.show()
+
+ # fig = go.Figure()
+ # fig = plotly_simulation_anime(traces, tmp_map, fig)
+ # fig.show()
+
diff --git a/demo/demo5.py b/demo/demo5.py
new file mode 100644
index 0000000000000000000000000000000000000000..8c9ffdd990c1db4a8bd528114f2ba5f66204eac0
--- /dev/null
+++ b/demo/demo5.py
@@ -0,0 +1,77 @@
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent, NPCAgent
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent
+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
+import numpy as np
+from enum import Enum, 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 VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ Lane1 = auto()
+ Lane2 = auto()
+
+if __name__ == "__main__":
+ input_code_name = './example_controller7.py'
+ scenario = Scenario()
+
+ car = CarAgent('car1', file_name=input_code_name)
+ scenario.add_agent(car)
+ car = NPCAgent('car2')
+ scenario.add_agent(car)
+ car = NPCAgent('car3')
+ scenario.add_agent(car)
+ car = NPCAgent('car4')
+ scenario.add_agent(car)
+ # car = NPCAgent('car5')
+ # scenario.add_agent(car)
+ tmp_map = SimpleMap3()
+ scenario.set_map(tmp_map)
+ scenario.set_init(
+ [
+ [[0, -0.0, 0, 1.0],[0.0, 0.0, 0, 1.0]],
+ [[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
+ [[30, 0, 0, 0.5],[30, 0, 0, 0.5]],
+ [[10, 3, 0, 0.5],[10, 3, 0, 0.5]],
+ ],
+ [
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane0),
+ ]
+ )
+ # traces = scenario.simulate(70, 0.05)
+ traces = scenario.verify(60, 0.05)
+
+ 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()
+
diff --git a/demo/demo6.py b/demo/demo6.py
new file mode 100644
index 0000000000000000000000000000000000000000..b2e6563b9a16357921aeab17038cbce4dcdcbdb0
--- /dev/null
+++ b/demo/demo6.py
@@ -0,0 +1,102 @@
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent, NPCAgent
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent
+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
+import numpy as np
+from enum import Enum, 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 VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ Lane1 = auto()
+ Lane2 = auto()
+ Lane3 = 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
+# type: LaneObjectMode = LaneObjectMode.Vehicle
+
+# def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type: LaneObjectMode):
+# pass
+
+
+if __name__ == "__main__":
+ input_code_name = './example_controller7.py'
+ scenario = Scenario()
+
+ car = CarAgent('car1', file_name=input_code_name)
+ scenario.add_agent(car)
+ car = NPCAgent('car2')
+ scenario.add_agent(car)
+ car = CarAgent('car3', file_name=input_code_name)
+ scenario.add_agent(car)
+ car = NPCAgent('car4')
+ scenario.add_agent(car)
+ car = NPCAgent('car5')
+ scenario.add_agent(car)
+ car = NPCAgent('car6')
+ scenario.add_agent(car)
+ car = NPCAgent('car7')
+ scenario.add_agent(car)
+ tmp_map = SimpleMap4()
+ scenario.set_map(tmp_map)
+ scenario.set_init(
+ [
+ [[0, -0.0, 0, 1.0],[0.0, 0.0, 0, 1.0]],
+ [[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
+ [[14.5, 3, 0, 0.6],[14.5, 3, 0, 0.6]],
+ [[20, 3, 0, 0.5],[20, 3, 0, 0.5]],
+ [[30, 0, 0, 0.5],[30, 0, 0, 0.5]],
+ [[23, -3, 0, 0.5],[23, -3, 0, 0.5]],
+ [[40, -6, 0, 0.5],[40, -6, 0, 0.5]],
+ ],
+ [
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane0),
+ (VehicleMode.Normal, LaneMode.Lane0),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane2),
+ (VehicleMode.Normal, LaneMode.Lane3),
+ ]
+ )
+ # traces = scenario.simulate(80, 0.05)
+ traces = scenario.verify(50, 0.05)
+
+ 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)
+ fig = plot_reachtube_tree(traces, 'car5', 1, [2], 'r', fig)
+ fig = plot_reachtube_tree(traces, 'car6', 1, [2], 'r', fig)
+ plt.show()
+
+ # fig = go.Figure()
+ # fig = plotly_simulation_anime(traces, tmp_map, fig)
+ # fig.show()
+
diff --git a/demo/demo7.py b/demo/demo7.py
new file mode 100644
index 0000000000000000000000000000000000000000..3d6b1de9b250575aeec0c2b059a4edf83b6246fb
--- /dev/null
+++ b/demo/demo7.py
@@ -0,0 +1,110 @@
+# SM: Noting some things about the example
+
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent, NPCAgent
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent
+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
+import numpy as np
+from enum import Enum, 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 VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ Lane1 = auto()
+ Lane2 = auto()
+ Lane3 = 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
+ type: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type: LaneObjectMode):
+ pass
+
+
+if __name__ == "__main__":
+ input_code_name = './example_controller8.py'
+ scenario = Scenario()
+
+ car = CarAgent('car1', file_name=input_code_name)
+ scenario.add_agent(car)
+ car = NPCAgent('car2')
+ scenario.add_agent(car)
+ car = CarAgent('car3', file_name=input_code_name)
+ scenario.add_agent(car)
+ car = NPCAgent('car4')
+ scenario.add_agent(car)
+ car = NPCAgent('car5')
+ scenario.add_agent(car)
+ car = CarAgent('car8', file_name=input_code_name)
+ scenario.add_agent(car)
+ car = NPCAgent('car6')
+ scenario.add_agent(car)
+ car = NPCAgent('car7')
+ scenario.add_agent(car)
+ tmp_map = SimpleMap4()
+ scenario.set_map(tmp_map)
+ scenario.set_init(
+ [
+ [[0, -0.0, 0, 1.0],[0.0, 0.0, 0, 1.0]],
+ [[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
+ [[14, 3, 0, 0.6],[14, 3, 0, 0.6]],
+ [[20, 3, 0, 0.5],[20, 3, 0, 0.5]],
+ [[30, 0, 0, 0.5],[30, 0, 0, 0.5]],
+ [[23, -3, 0, 0.6],[23, -3, 0, 0.6]],
+ [[28.5, -3, 0, 0.5],[28.5, -3, 0, 0.5]],
+ [[40, -6, 0, 0.5],[40, -6, 0, 0.5]],
+ ],
+ [
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane0),
+ (VehicleMode.Normal, LaneMode.Lane0),
+ (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane2),
+ (VehicleMode.Normal, LaneMode.Lane2),
+ (VehicleMode.Normal, LaneMode.Lane3),
+ ]
+ )
+ traces = scenario.simulate(80, 0.05)
+ # traces = scenario.verify(15, 0.05)
+
+ # fig = plt.figure(2)
+ # fig = plot_map(tmp_map, 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'r', fig)
+ # fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car3', 1, [2], 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car4', 1, [2], 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car5', 1, [2], 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car6', 1, [2], 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car7', 1, [2], 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car8', 1, [2], 'g', fig)
+ # plt.show()
+
+ fig = go.Figure()
+ fig = plotly_simulation_anime(traces, tmp_map, fig)
+ fig.show()
+
diff --git a/demo/demo8.py b/demo/demo8.py
new file mode 100644
index 0000000000000000000000000000000000000000..99b4db8f2ab3251a48ec93afc2a9a169f12cde16
--- /dev/null
+++ b/demo/demo8.py
@@ -0,0 +1,78 @@
+from dryvr_plus_plus.example.example_agent.car_agent import CarAgent, NPCAgent
+from dryvr_plus_plus.example.example_agent.sign_agent import SignAgent
+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
+
+import matplotlib.pyplot as plt
+import plotly.graph_objects as go
+import numpy as np
+from enum import Enum, 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 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
+ type_mode: LaneObjectMode
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type_mode: LaneObjectMode):
+ pass
+
+
+if __name__ == "__main__":
+ input_code_name = './example_controller9.py'
+ scenario = Scenario()
+
+ car = CarAgent('car1', file_name=input_code_name)
+ scenario.add_agent(car)
+ sign = SignAgent('stop_sign')
+ scenario.add_agent(sign)
+ tmp_map = SimpleMap3()
+ scenario.set_map(tmp_map)
+ scenario.set_init(
+ [
+ [[0, 0, 0, 1.0],[0, 0, 0, 1.0]],
+ [[15, 0, 0, 0.5],[15, 0, 0, 0.5]],
+ ],
+ [
+ (VehicleMode.Normal, LaneMode.Lane1, LaneObjectMode.Vehicle),
+ (VehicleMode.Normal, LaneMode.Lane1, LaneObjectMode.Sign),
+ ]
+ )
+ traces = scenario.simulate(13, 0.05)
+ # traces = scenario.verify(70, 0.05)
+
+ # 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()
+
diff --git a/demo/example_controller3.py b/demo/example_controller3.py
index 1f606e0c0463ed5e60950d9b481d5036f761f27f..95209827ff4bd10619d39a3a734cc525d85c81a7 100644
--- a/demo/example_controller3.py
+++ b/demo/example_controller3.py
@@ -27,9 +27,9 @@ class State:
v = 0.0
vehicle_mode: VehicleMode = VehicleMode.Normal
lane_mode: LaneMode = LaneMode.Lane0
- type: LaneObjectMode
+ type_mode: LaneObjectMode
- def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type: LaneObjectMode):
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type_mode: LaneObjectMode):
self.data = []
def controller(ego:State, other:State, sign:State, lane_map:LaneMap):
diff --git a/demo/example_controller4.py b/demo/example_controller4.py
new file mode 100644
index 0000000000000000000000000000000000000000..8645d7d812ffefe3237801a8aeb563d5deb01deb
--- /dev/null
+++ b/demo/example_controller4.py
@@ -0,0 +1,54 @@
+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 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
+ type_mode: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type_mode: LaneObjectMode):
+ pass
+
+def controller(ego:State, others:List[State], lane_map):
+ output = copy.deepcopy(ego)
+ if ego.vehicle_mode == VehicleMode.Normal:
+ if any((other.x-ego.x > 3 and other.x-ego.x < 5 and ego.lane_mode == other.lane_mode and other.type_mode==LaneObjectMode.Vehicle) for other in others):
+ if lane_map.has_left(ego.lane_mode):
+ output.vehicle_mode = VehicleMode.SwitchLeft
+ if any((other.x-ego.x > 3 and other.x-ego.x < 5 and ego.lane_mode == other.lane_mode) for other in others):
+ 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/demo/example_controller5.py b/demo/example_controller5.py
new file mode 100644
index 0000000000000000000000000000000000000000..1e8f409efacacc066da76784e93bebc498b7b6d2
--- /dev/null
+++ b/demo/example_controller5.py
@@ -0,0 +1,58 @@
+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 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
+ type_mode: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type_mode: LaneObjectMode):
+ pass
+
+def controller(ego:State, others:List[State], lane_map):
+ output = copy.deepcopy(ego)
+ if ego.vehicle_mode == VehicleMode.Normal:
+ if any((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) for other in others):
+ if lane_map.has_left(ego.lane_mode):
+ output.vehicle_mode = VehicleMode.SwitchLeft
+ if any((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) for other in others):
+ 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/demo/example_controller6.py b/demo/example_controller6.py
new file mode 100644
index 0000000000000000000000000000000000000000..abb73f48d2cec3213d01ea389743989dcccac733
--- /dev/null
+++ b/demo/example_controller6.py
@@ -0,0 +1,56 @@
+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 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
+ type: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type: LaneObjectMode):
+ pass
+
+def controller(ego:State, others:List[State], lane_map):
+ output = copy.deepcopy(ego)
+ if ego.vehicle_mode == VehicleMode.Normal:
+ if any((other.x-ego.x > 3 and other.x-ego.x < 5 and ego.lane_mode == other.lane_mode) for other in others):
+ if lane_map.has_left(ego.lane_mode) and \
+ not any((other.x-ego.x<8 and other.lane_mode==lane_map.left_lane(ego.lane_mode)) for other in others):
+ output.vehicle_mode = VehicleMode.SwitchLeft
+ if any((other.x-ego.x > 3 and other.x-ego.x < 5 and ego.lane_mode == other.lane_mode) for other in others):
+ if lane_map.has_right(ego.lane_mode) and \
+ not any((other.x-ego.x<8 and other.lane_mode==lane_map.right_lane(ego.lane_mode)) for other in others):
+ 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/demo/example_controller7.py b/demo/example_controller7.py
new file mode 100644
index 0000000000000000000000000000000000000000..655278de8f2a845d20f559881080547187120bb0
--- /dev/null
+++ b/demo/example_controller7.py
@@ -0,0 +1,64 @@
+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 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
+ type_mode: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type_mode: LaneObjectMode):
+ pass
+
+def controller(ego:State, others:List[State], lane_map):
+ output = copy.deepcopy(ego)
+ if ego.vehicle_mode == VehicleMode.Normal:
+ if any((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) for other in others):
+ if lane_map.has_left(ego.lane_mode) and \
+ not any((lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 8 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 \
+ other.lane_mode==lane_map.left_lane(ego.lane_mode)) for other in others):
+ output.vehicle_mode = VehicleMode.SwitchLeft
+ if any((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) for other in others):
+ if lane_map.has_right(ego.lane_mode) and \
+ not any((lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 8 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 \
+ other.lane_mode==lane_map.right_lane(ego.lane_mode)) for other in others):
+ 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/demo/example_controller8.py b/demo/example_controller8.py
new file mode 100644
index 0000000000000000000000000000000000000000..a88ac0707335dc9bf3cec042435a2fba73c58e74
--- /dev/null
+++ b/demo/example_controller8.py
@@ -0,0 +1,89 @@
+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 VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+ Stop = 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
+ type: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type: LaneObjectMode):
+ pass
+
+def controller(ego:State, others:List[State], lane_map):
+ output = copy.deepcopy(ego)
+ if ego.vehicle_mode == VehicleMode.Normal:
+ # Switch left if left lane is empty
+ if any((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) for other in others):
+ if lane_map.has_left(ego.lane_mode) and \
+ not any((lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 8 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 \
+ other.lane_mode==lane_map.left_lane(ego.lane_mode)) for other in others):
+ output.vehicle_mode = VehicleMode.SwitchLeft
+
+ # Switch right if right lane is empty
+ if any((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) for other in others):
+ if lane_map.has_right(ego.lane_mode) and \
+ not any((lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 8 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 \
+ other.lane_mode==lane_map.right_lane(ego.lane_mode)) for other in others):
+ output.vehicle_mode = VehicleMode.SwitchRight
+
+ # If really close just brake
+ if any((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]) > -0.5 \
+ and ego.lane_mode == other.lane_mode) for other in others):
+ output.vehicle_mode = VehicleMode.Stop
+ output.v = 0.1
+
+ # If switched left enough, return to normal mode
+ 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 switched right enough,return to normal 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)
+
+ if ego.vehicle_mode == VehicleMode.Brake:
+ if all((\
+ (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 or \
+ 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.5) and\
+ other.lane_mode==ego.lane_mode) for other in others):
+ output.vehicle_mode = VehicleMode.Normal
+ output.v = 1.0
+
+ return output
+
diff --git a/demo/example_controller9.py b/demo/example_controller9.py
new file mode 100644
index 0000000000000000000000000000000000000000..1f7b38c95885533fc2c96f8fc249a15b18713f76
--- /dev/null
+++ b/demo/example_controller9.py
@@ -0,0 +1,47 @@
+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 VehicleMode(Enum):
+ Normal = auto()
+ SwitchLeft = auto()
+ SwitchRight = auto()
+ Brake = auto()
+ Stop = 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
+ type_mode: LaneObjectMode = LaneObjectMode.Vehicle
+
+ def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type_mode: LaneObjectMode):
+ pass
+
+def controller(ego:State, others:List[State], lane_map):
+ output = copy.deepcopy(ego)
+ # Detect the stop sign
+ if ego.vehicle_mode == VehicleMode.Normal:
+ if any(other.x - ego.x < 5 and other.x - ego.x > -1 and other.type_mode==LaneObjectMode.Sign for other in others):
+ output.vehicle_mode = VehicleMode.Brake
+ if ego.vehicle_mode == VehicleMode.Brake:
+ if ego.v <= 0:
+ output.vehicle_mode = VehicleMode.Stop
+ output.v = 0
+ return output
+
diff --git a/dryvr_plus_plus/example/example_agent/ball_agent.py b/dryvr_plus_plus/example/example_agent/ball_agent.py
new file mode 100644
index 0000000000000000000000000000000000000000..e4658ba8452e10dd67f02acf79b2d5322bb4f54d
--- /dev/null
+++ b/dryvr_plus_plus/example/example_agent/ball_agent.py
@@ -0,0 +1,55 @@
+# Example agent.
+from typing import Tuple, List
+
+import numpy as np
+from scipy.integrate import ode
+
+from dryvr_plus_plus.scene_verifier.agents.base_agent import BaseAgent
+from dryvr_plus_plus.scene_verifier.map.lane_map import LaneMap
+from dryvr_plus_plus.scene_verifier.code_parser.pythonparser import EmptyAst
+
+
+class BallAgent(BaseAgent):
+ '''Dynamics of a frictionless billiard ball
+ on a 2D-plane'''
+ def __init__(self, id, code = None, file_name = None):
+ '''Contructor for tha agent
+ EXACTLY one of the following should be given
+ file_name: name of the controller
+ code: pyhton string ddefning the controller
+ '''
+ # Calling the constructor of tha base class
+ super().__init__(id, code, file_name)
+
+ @staticmethod
+ def dynamic(t, state):
+ '''Defines the RHS of the ODE used to simulate trajectories'''
+ x, y, vx, vy = state
+ x_dot = vx
+ y_dot = vy
+ vx_dot = 0
+ vy_dot = 0
+ return [x_dot, y_dot, vx_dot, vy_dot]
+
+ def TC_simulate(self, mode: List[str], initialCondition, time_bound, time_step, lane_map:LaneMap=None)->np.ndarray:
+ # TODO: P1. Should TC_simulate really be part of the agent definition or should it be something more generic?
+ # TODO: P2. Looks like this should be a global parameter; some config file should be setting this.
+ time_bound = float(time_bound)
+ number_points = int(np.ceil(time_bound/time_step))
+ t = [round(i*time_step,10) for i in range(0,number_points)]
+
+ init = initialCondition
+ trace = [[0]+init]
+ for i in range(len(t)):
+ r = ode(self.dynamic)
+ r.set_initial_value(init)
+ res:np.ndarray = r.integrate(r.t + time_step)
+ init = res.flatten().tolist()
+ trace.append([t[i] + time_step] + init)
+
+ return np.array(trace)
+
+if __name__ == '__main__':
+ aball = BallAgent('red_ball',file_name="/Users/mitras/Dpp/GraphGeneration/demo/ball_bounces.py")
+ trace = aball.TC_simulate({'none'},[5, 10, 2, 2],10)
+ print(trace)
\ No newline at end of file
diff --git a/dryvr_plus_plus/example/example_agent/car_agent.py b/dryvr_plus_plus/example/example_agent/car_agent.py
index b4e292f3e3fb9d4a04f289ffd025535e98ef2a64..2470e10801588d30b4756fdae91d2126704f3ab6 100644
--- a/dryvr_plus_plus/example/example_agent/car_agent.py
+++ b/dryvr_plus_plus/example/example_agent/car_agent.py
@@ -1,3 +1,4 @@
+# Example agent.
from typing import Tuple, List
import numpy as np
@@ -23,7 +24,10 @@ class NPCAgent(BaseAgent):
v_dot = a
return [x_dot, y_dot, theta_dot, v_dot]
+
def action_handler(self, mode, state, lane_map: LaneMap) -> Tuple[float, float]:
+ ''' Computes steering and acceleration based on current lane, target lane and
+ current state using a Stanley controller-like rule'''
x, y, theta, v = state
vehicle_mode = mode[0]
vehicle_lane = mode[1]
@@ -35,8 +39,8 @@ class NPCAgent(BaseAgent):
a = 0
return steering, a
- def TC_simulate(self, mode: List[str], initialCondition, time_bound, lane_map: LaneMap = None) -> np.ndarray:
- time_step = 0.05
+
+ def TC_simulate(self, mode: List[str], initialCondition, time_bound, time_step, lane_map:LaneMap=None)->np.ndarray:
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)]
@@ -101,12 +105,11 @@ class CarAgent(BaseAgent):
steering = np.clip(steering, -0.61, 0.61)
return steering, a
- def TC_simulate(self, mode: List[str], initialCondition, time_bound, lane_map: LaneMap = None) -> np.ndarray:
- time_step = 0.05
+
+ def TC_simulate(self, mode: List[str], initialCondition, time_bound, time_step, lane_map:LaneMap=None)->np.ndarray:
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)]
-
+ t = [round(i*time_step,10) for i in range(0,number_points)]
init = initialCondition
# [time, x, y, theta, v]
trace = [[0]+init]
diff --git a/dryvr_plus_plus/example/example_agent/sign_agent.py b/dryvr_plus_plus/example/example_agent/sign_agent.py
index b6eb7fe383eb62f64a053312b5ef063f9cf40dab..2b97fa273ab00d7df9576fdd17a9fd21cbf2f0aa 100644
--- a/dryvr_plus_plus/example/example_agent/sign_agent.py
+++ b/dryvr_plus_plus/example/example_agent/sign_agent.py
@@ -7,8 +7,7 @@ class SignAgent(BaseAgent):
self.id = id
self.controller = EmptyAst()
- def TC_simulate(self, mode, init, time_horizon, map=None):
- time_step = 0.01
+ def TC_simulate(self, mode, init, time_horizon, time_step, map=None):
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]
diff --git a/dryvr_plus_plus/example/example_map/simple_map2.py b/dryvr_plus_plus/example/example_map/simple_map2.py
index 7686907647aad1464efc1df7c06671b5582de232..257b2308188045131c5f5d5736e10e7be05dc60f 100644
--- a/dryvr_plus_plus/example/example_map/simple_map2.py
+++ b/dryvr_plus_plus/example/example_map/simple_map2.py
@@ -23,22 +23,22 @@ class SimpleMap3(LaneMap):
super().__init__()
segment0 = StraightLane(
'Seg0',
- [0, 3],
- [50, 3],
+ [0,3],
+ [100,3],
3
)
lane0 = Lane('Lane0', [segment0])
segment1 = StraightLane(
'seg0',
- [0, 0],
- [50, 0],
+ [0,0],
+ [100,0],
3
)
lane1 = Lane('Lane1', [segment1])
segment2 = StraightLane(
'seg0',
- [0, -3],
- [50, -3],
+ [0,-3],
+ [100,-3],
3
)
lane2 = Lane('Lane2', [segment2])
@@ -50,7 +50,6 @@ class SimpleMap3(LaneMap):
self.right_lane_dict[lane0.id].append(lane1.id)
self.right_lane_dict[lane1.id].append(lane2.id)
-
class SimpleMap3_v2(LaneMap):
def __init__(self):
super().__init__()
@@ -86,6 +85,56 @@ class SimpleMap3_v2(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],
+ [100,3],
+ 3
+ )
+ lane0 = Lane('Lane0', [segment0])
+ segment1 = StraightLane(
+ 'seg0',
+ [0,0],
+ [100,0],
+ 3
+ )
+ lane1 = Lane('Lane1', [segment1])
+ segment2 = StraightLane(
+ 'seg0',
+ [0,-3],
+ [100,-3],
+ 3
+ )
+ lane2 = Lane('Lane2', [segment2])
+ segment3 = StraightLane(
+ 'seg3',
+ [0,-6],
+ [100,-6],
+ 3
+ )
+ lane3 = Lane('Lane3', [segment3])
+ segment4 = StraightLane(
+ 'Seg4',
+ [0,6],
+ [100,6],
+ 3
+ )
+ lane4 = Lane('Lane4', [segment4])
+
+ # segment2 = LaneSegment('Lane1', 3)
+ # self.add_lanes([segment1,segment2])
+ self.add_lanes([lane0, lane1, lane2, lane3, lane4])
+ self.left_lane_dict[lane0.id].append(lane4.id)
+ self.left_lane_dict[lane1.id].append(lane0.id)
+ self.left_lane_dict[lane2.id].append(lane1.id)
+ self.left_lane_dict[lane3.id].append(lane2.id)
+ self.right_lane_dict[lane4.id].append(lane0.id)
+ self.right_lane_dict[lane0.id].append(lane1.id)
+ self.right_lane_dict[lane1.id].append(lane2.id)
+ self.right_lane_dict[lane2.id].append(lane3.id)
class SimpleMap5(LaneMap):
def __init__(self):
diff --git a/dryvr_plus_plus/example/example_sensor/fake_sensor.py b/dryvr_plus_plus/example/example_sensor/fake_sensor.py
index 77b0169057b210bf0fc58068442a9949e3b06c2b..a982ef6b451eca842e460635f1b2fdaff2e933f4 100644
--- a/dryvr_plus_plus/example/example_sensor/fake_sensor.py
+++ b/dryvr_plus_plus/example/example_sensor/fake_sensor.py
@@ -12,11 +12,30 @@ class FakeSensor1:
cnts['ego.v'] = state[4]
disc['ego.vehicle_mode'] = mode[0]
disc['ego.lane_mode'] = mode[1]
- return cnts, disc
+ return cnts, disc, {}
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 add_states_2d(cont, disc, thing, val):
+ state, mode = val
+ adds(cont, thing, ['x','y','theta','v'], state[1:5])
+ adds(disc, thing, ["vehicle_mode", "lane_mode", "type"], mode)
+
+def add_states_3d(cont, disc, thing, val):
+ state, mode = val
+ transp = np.transpose(np.array(state)[:, 1:5])
+ assert len(transp) == 4
+ adds(cont, thing, ["x", "y", "theta", "v"], transp)
+ adds(disc, thing, ["vehicle_mode", "lane_mode", "type"], mode)
+
def set_states_2d(cnts, disc, thing, val):
state, mode = val
sets(cnts, thing, ["x", "y", "theta", "v"], state[1:5])
@@ -50,7 +69,7 @@ class FakeSensor2:
set_states_2d(cnts, disc, "other", state_dict["car2"])
if "sign" in state_dict:
set_states_2d(cnts, disc, "sign", state_dict["sign"])
- return cnts, disc
+ return cnts, disc, {}
else:
if agent.id == 'car1':
set_states_3d(cnts, disc, "ego", state_dict["car1"])
@@ -67,4 +86,66 @@ class FakeSensor2:
set_states_3d(cnts, disc, "other", state_dict["car2"])
if "sign" in state_dict:
set_states_3d(cnts, disc, "sign", state_dict["sign"])
- return cnts, disc
+ return cnts, disc, {}
+
+class FakeSensor3:
+ def sense(self, scenario, agent, state_dict, lane_map):
+ cont = {}
+ disc = {}
+ len_dict = {'others':len(state_dict)-1}
+ tmp = np.array(state_dict['car1'][0])
+ if tmp.ndim < 2:
+ for agent_id in state_dict:
+ if agent_id == agent.id:
+ set_states_2d(cont, disc, 'ego', state_dict[agent_id])
+ else:
+ add_states_2d(cont, disc, 'others', state_dict[agent_id])
+ else:
+ for agent_id in state_dict:
+ if agent_id == agent.id:
+ set_states_3d(cont, disc, "ego", state_dict[agent_id])
+ else:
+ add_states_3d(cont, disc, 'others', state_dict[agent_id])
+ return cont, disc, len_dict
+
+
+def set_states_2d_ball(cnts, disc, thing, val):
+ state, mode = val
+ sets(cnts, thing, ["x", "y", "vx", "vy"], state[1:5])
+ sets(disc, thing, ["ball_mode", "lane_mode"], mode)
+def set_states_3d_ball(cnts, disc, thing, val):
+ state, mode = val
+ transp = np.transpose(np.array(state)[:, 1:5])
+ assert len(transp) == 4
+ sets(cnts, thing, ["x", "y", "vx", "vy"], transp)
+ sets(disc, thing, ["ball_mode", "lane_mode"], mode)
+def add_states_2d_ball(cont, disc, thing, val):
+ state, mode = val
+ adds(cont, thing, ['x','y','vx','vy'], state[1:5])
+ adds(disc, thing, ["ball_mode", "lane_mode", "type"], mode)
+def add_states_3d_ball(cont, disc, thing, val):
+ state, mode = val
+ transp = np.transpose(np.array(state)[:, 1:5])
+ assert len(transp) == 4
+ adds(cont, thing, ['x','y','vx','vy'], transp)
+ adds(disc, thing, ["ball_mode", "lane_mode", "type"], mode)
+
+class FakeSensor4:
+ 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])
+ if tmp.ndim < 2:
+ for agent_id in state_dict:
+ if agent_id == agent.id:
+ set_states_2d_ball(cont, disc, 'ego', state_dict[agent_id])
+ else:
+ add_states_2d_ball(cont, disc, 'others', state_dict[agent_id])
+ else:
+ for agent_id in state_dict:
+ if agent_id == agent.id:
+ set_states_3d_ball(cont, disc, "ego", state_dict[agent_id])
+ else:
+ add_states_3d_ball(cont, disc, 'others', state_dict[agent_id])
+ return cont, disc, len_dict
\ No newline at end of file
diff --git a/dryvr_plus_plus/plotter/plotter2D.py b/dryvr_plus_plus/plotter/plotter2D.py
index 018fa448c840a60da1c9a613826c6e6c4ef047d9..8dc9f5e644c22a97adee89d0cc1902050257660d 100644
--- a/dryvr_plus_plus/plotter/plotter2D.py
+++ b/dryvr_plus_plus/plotter/plotter2D.py
@@ -23,7 +23,6 @@ from dryvr_plus_plus.scene_verifier.analysis.analysis_tree_node import AnalysisT
colors = ['red', 'green', 'blue', 'yellow', 'black']
-
def plotly_plot(data,
x_dim: int = 0,
y_dim_list: List[int] = [1],
@@ -60,7 +59,6 @@ def plotly_plot(data,
# fig.update_yaxes(range=[y_min-1, y_max+1])
return fig, (x_min, x_max), (y_min, y_max)
-
def plot(
data,
x_dim: int = 0,
@@ -557,13 +555,36 @@ def plot_reachtube_tree(root, agent_id, x_dim: int = 0, y_dim_list: List[int] =
trace = traces[agent_id]
# print(trace)
data = []
+
for i in range(0, len(trace)-1, 2):
data.append([trace[i], trace[i+1]])
fig, x_lim, y_lim = plot(
data, x_dim, y_dim_list, color, fig, x_lim, y_lim)
- # print(data)
queue += node.child
+ return fig
+def plot_reachtube_tree_branch(root, agent_id, x_dim: int=0, y_dim_list: List[int]=[1], color='b', fig = None, x_lim = None, y_lim = None):
+ if fig is None:
+ fig = plt.figure()
+
+ ax = fig.gca()
+ if x_lim is None:
+ x_lim = ax.get_xlim()
+ if y_lim is None:
+ y_lim = ax.get_ylim()
+
+ stack = [root]
+ while stack != []:
+ node = stack.pop()
+ traces = node.trace
+ trace = traces[agent_id]
+ data = []
+ for i in range(0,len(trace),2):
+ data.append([trace[i], trace[i+1]])
+ fig, x_lim, y_lim = plot(data, x_dim, y_dim_list, color, fig, x_lim, y_lim)
+
+ if node.child:
+ stack += [node.child[0]]
return fig
@@ -703,7 +724,6 @@ def plot_map(map, color='b', fig=None, x_lim=None, y_lim=None):
if lane_seg.type == 'Straight':
start1 = lane_seg.start + lane_seg.width/2 * lane_seg.direction_lateral
end1 = lane_seg.end + lane_seg.width/2 * lane_seg.direction_lateral
-
ax.plot([start1[0], end1[0]], [start1[1], end1[1]], color)
start2 = lane_seg.start - lane_seg.width/2 * lane_seg.direction_lateral
end2 = lane_seg.end - lane_seg.width/2 * lane_seg.direction_lateral
@@ -793,18 +813,15 @@ def plot_simulation_tree(root: AnalysisTreeNode, agent_id, x_dim: int = 0, y_dim
ax.plot(trace[:, x_dim], trace[:, y_dim], color)
x_min = min(x_min, trace[:, x_dim].min())
x_max = max(x_max, trace[:, x_dim].max())
-
y_min = min(y_min, trace[:, y_dim].min())
y_max = max(y_max, trace[:, y_dim].max())
queue += node.child
ax.set_xlim([x_min-1, x_max+1])
ax.set_ylim([y_min-1, y_max+1])
- # plt.show()
- # generate_simulation_anime(root, None, fig)
return fig
-def generate_simulation_anime(root, map=None, fig=None):
+def generate_simulation_anime(root, map, fig=None):
if fig is None:
fig = plt.figure()
# fig = plot_map(map, 'g', fig)
@@ -851,7 +868,7 @@ def generate_simulation_anime(root, map=None, fig=None):
dx = np.cos(point[2])*point[3]
dy = np.sin(point[2])*point[3]
ax.arrow(x_tail, y_tail, dx, dy, head_width=1, head_length=0.5)
- plt.pause(0.005)
+ plt.pause(0.05)
plt.clf()
return fig
# img_buf = io.BytesIO()
@@ -873,7 +890,8 @@ def plotly_simulation_anime(root, map=None, fig=None):
# fig = plot_map(map, 'g', fig)
timed_point_dict = {}
stack = [root]
- print("plot")
+
+ # print("plot")
# print(root.mode)
x_min, x_max = float('inf'), -float('inf')
y_min, y_max = float('inf'), -float('inf')
@@ -887,8 +905,7 @@ def plotly_simulation_anime(root, map=None, fig=None):
traces = node.trace
for agent_id in traces:
trace = np.array(traces[agent_id])
- print(trace)
- # segment_start.add(round(trace[0][0], 2))
+
for i in range(len(trace)):
x_min = min(x_min, trace[i][1])
x_max = max(x_max, trace[i][1])
@@ -913,6 +930,7 @@ def plotly_simulation_anime(root, map=None, fig=None):
# fill in most of layout
# print(segment_start)
# print(timed_point_dict.keys())
+
duration = int(600/time)
fig_dict["layout"]["xaxis"] = {
"range": [(x_min-10), (x_max+10)],
@@ -968,6 +986,7 @@ def plotly_simulation_anime(root, map=None, fig=None):
}
# make data
point_list = timed_point_dict[0]
+
print(point_list)
x_list = []
y_list = []
@@ -997,6 +1016,7 @@ def plotly_simulation_anime(root, map=None, fig=None):
# make frames
for time_point in timed_point_dict:
+
# print(time_point)
frame = {"data": [], "layout": {
"annotations": []}, "name": '{:.2f}'.format(time_point)}
@@ -1019,6 +1039,7 @@ def plotly_simulation_anime(root, map=None, fig=None):
"x": trace_x,
"y": trace_y,
"mode": "markers + text",
+
# "text": [(round(trace_theta[i]/pi*180, 2), round(trace_v[i], 3)) for i in range(len(trace_theta))],
"text": [('{:.2f}'.format(trace_theta[i]/pi*180), '{:.3f}'.format(trace_v[i])) for i in range(len(trace_theta))],
"textfont": dict(size=14, color="black"),
@@ -1049,26 +1070,6 @@ def plotly_simulation_anime(root, map=None, fig=None):
"arrowhead": 1,
"arrowcolor": "black"}
frame["layout"]["annotations"].append(annotations_dict)
-
- # if (time_point in segment_start) and (operator.ne(previous_mode[agent_id], node.mode[agent_id])):
- # annotations_dict = {"x": trace_x[i], "y": trace_y[i],
- # # "xshift": ax, "yshift": ay,
- # # "ax": trace_x[i], "ay": trace_y[i],
- # # "arrowwidth": 2,
- # # "arrowside": 'end',
- # "showarrow": False,
- # # "arrowsize": 1,
- # # "xref": 'x', "yref": 'y',
- # # "axref": 'x', "ayref": 'y',
- # "text": str(node.mode[agent_id][0]),
- # # "arrowhead": 1,
- # # "arrowcolor": "black"
- # }
- # frame["layout"]["annotations"].append(annotations_dict)
- # print(frame["layout"]["annotations"])
- # i += 1
- # previous_mode[agent_id] = node.mode[agent_id]
-
fig_dict["frames"].append(frame)
slider_step = {"args": [
[time_point],
@@ -1105,6 +1106,7 @@ def plotly_simulation_anime(root, map=None, fig=None):
trace_y = trace[:, 2].tolist()
trace_x = trace[:, 1].tolist()
# theta = [i/pi*180 for i in trace[:, 3]]
+
i = agent_list.index(agent_id)
color = colors[i % 5]
fig.add_trace(go.Scatter(x=trace[:, 1], y=trace[:, 2],
@@ -1203,3 +1205,4 @@ def plotly_simulation_anime(root, map=None, fig=None):
# springgreen, steelblue, tan, teal, thistle, tomato,
# turquoise, violet, wheat, white, whitesmoke,
# yellow, yellowgreen
+
diff --git a/dryvr_plus_plus/plotter/plotter3D.py b/dryvr_plus_plus/plotter/plotter3D.py
index 1ede38e0f2e077b92f048da1d9a9c88c9dedbdb2..68a3a96f6d2e2aba25bd8b7f2ac2cbcd9705847f 100644
--- a/dryvr_plus_plus/plotter/plotter3D.py
+++ b/dryvr_plus_plus/plotter/plotter3D.py
@@ -2,10 +2,7 @@
# Written by: Kristina Miller
import numpy as np
-import matplotlib.pyplot as plt
-import mpl_toolkits.mplot3d as a3
-from scipy.spatial import ConvexHull
import polytope as pc
import pyvista as pv
@@ -40,6 +37,7 @@ def plot_polytope_3d(A, b, ax = None, color = 'red', trans = 0.2, edge = True):
if edge:
edges = shell.extract_feature_edges(20)
ax.add_mesh(edges, color="k", line_width=1)
+ return ax
def plot_line_3d(start, end, ax = None, color = 'blue', line_width = 1):
if ax is None:
@@ -52,6 +50,13 @@ def plot_line_3d(start, end, ax = None, color = 'blue', line_width = 1):
line = pv.Line(a, b)
ax.add_mesh(line, color=color, line_width=line_width)
+ return ax
+
+def plot_point_3d(points, ax=None, color='blue', point_size = 100):
+ if ax is None:
+ ax = pv.Plotter()
+ ax.add_points(points, render_points_as_spheres=True, point_size = point_size, color = color)
+ return ax
if __name__ == '__main__':
A = np.array([[-1, 0, 0],
@@ -62,7 +67,7 @@ if __name__ == '__main__':
[0, 0, 1]])
b = np.array([[1], [1], [1], [1], [1], [1]])
b2 = np.array([[-1], [2], [-1], [2], [-1], [2]])
- ax1 = a3.Axes3D(plt.figure())
- plot_polytope_3d(A, b, ax = ax1, color = 'red')
- plot_polytope_3d(A, b2, ax = ax1, color = 'green')
- plt.show()
\ No newline at end of file
+ fig = pv.Plotter()
+ fig = plot_polytope_3d(A, b, ax = fig, color = 'red')
+ fig = plot_polytope_3d(A, b2, ax = fig, color = 'green')
+ fig.show()
\ No newline at end of file
diff --git a/dryvr_plus_plus/scene_verifier/agents/base_agent.py b/dryvr_plus_plus/scene_verifier/agents/base_agent.py
index 62d92d82e1a214554b24f6b1d45469d6e9d4cc68..96d912e91b5611cdda1785dd9c171b262c52b4df 100644
--- a/dryvr_plus_plus/scene_verifier/agents/base_agent.py
+++ b/dryvr_plus_plus/scene_verifier/agents/base_agent.py
@@ -5,5 +5,5 @@ class BaseAgent:
self.controller = ControllerAst(code, file_name)
self.id = id
- def TC_simulate(self, mode, initialSet, time_horizon, map=None):
+ def TC_simulate(self, mode, initialSet, time_horizon, time_step, map=None):
raise NotImplementedError
\ No newline at end of file
diff --git a/dryvr_plus_plus/scene_verifier/analysis/simulator.py b/dryvr_plus_plus/scene_verifier/analysis/simulator.py
index dd6f03f8182e68a3cc1b07ffdabec84d79a94ae9..c7567cef5aaa6562b0cb16b252400f48df702491 100644
--- a/dryvr_plus_plus/scene_verifier/analysis/simulator.py
+++ b/dryvr_plus_plus/scene_verifier/analysis/simulator.py
@@ -1,5 +1,6 @@
from typing import List, Dict
import copy
+import itertools
import numpy as np
@@ -11,7 +12,7 @@ class Simulator:
def __init__(self):
self.simulation_tree_root = None
- def simulate(self, init_list, init_mode_list, agent_list: List[BaseAgent], transition_graph, time_horizon, lane_map):
+ def simulate(self, init_list, init_mode_list, agent_list:List[BaseAgent], transition_graph, time_horizon, time_step, lane_map):
# Setup the root of the simulation tree
root = AnalysisTreeNode(
trace={},
@@ -32,9 +33,9 @@ class Simulator:
simulation_queue.append(root)
# Perform BFS through the simulation tree to loop through all possible transitions
while simulation_queue != []:
- node: AnalysisTreeNode = simulation_queue.pop(0)
- print(node.mode)
- remain_time = time_horizon - node.start_time
+ node:AnalysisTreeNode = simulation_queue.pop(0)
+ print(node.start_time, node.mode)
+ remain_time = round(time_horizon - node.start_time,10)
if remain_time <= 0:
continue
# For trace not already simulated
@@ -44,60 +45,47 @@ class Simulator:
# [time, x, y, theta, v]
mode = node.mode[agent_id]
init = node.init[agent_id]
- trace = node.agent[agent_id].TC_simulate(
- mode, init, remain_time, lane_map)
- trace[:, 0] += node.start_time
+ trace = node.agent[agent_id].TC_simulate(mode, init, remain_time, time_step, lane_map)
+ trace[:,0] += node.start_time
node.trace[agent_id] = trace.tolist()
- trace_length = len(list(node.trace.values())[0])
- print(trace_length)
- transitions = []
- # time step
- for idx in range(trace_length):
- # For each trace, check with the guard to see if there's any possible transition
- # Store all possible transition in a list
- # A transition is defined by (agent, src_mode, dest_mode, corresponding reset, transit idx)
- # Here we enforce that only one agent transit at a time
- all_agent_state = {}
- for agent_id in node.agent:
- all_agent_state[agent_id] = (
- node.trace[agent_id][idx], node.mode[agent_id])
- # keyi: whether it is possible to not call get_all_transition every time
- possible_transitions = transition_graph.get_all_transition(
- all_agent_state)
- if possible_transitions != []:
- print(possible_transitions)
- for agent_idx, src_mode, dest_mode, next_init in possible_transitions:
- transitions.append(
- (agent_idx, src_mode, dest_mode, next_init, idx))
- break
+ transitions, transition_idx = transition_graph.get_transition_simulate_new(node)
+
+ # If there's no transitions (returned transitions is empty), continue
+ if not transitions:
+ continue
+
# truncate the computed trajectories from idx and store the content after truncate
truncated_trace = {}
print("idx", idx)
for agent_idx in node.agent:
- truncated_trace[agent_idx] = node.trace[agent_idx][idx:]
- node.trace[agent_idx] = node.trace[agent_idx][:idx+1]
+ truncated_trace[agent_idx] = node.trace[agent_idx][transition_idx:]
+ node.trace[agent_idx] = node.trace[agent_idx][:transition_idx+1]
+
+ # Generate the transition combinations if multiple agents can transit at the same time step
+ transition_list = list(transitions.values())
+ all_transition_combinations = itertools.product(*transition_list)
# For each possible transition, construct the new node.
# Obtain the new initial condition for agent having transition
# copy the traces that are not under transition
- for transition in transitions:
- # mode including veh and lane
- 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] = list(dest_mode)
- next_node_agent = node.agent
+
+ for transition_combination in all_transition_combinations:
+ next_node_mode = copy.deepcopy(node.mode)
+ next_node_agent = node.agent
next_node_start_time = list(truncated_trace.values())[0][0][0]
next_node_init = {}
next_node_trace = {}
+ for transition in transition_combination:
+ 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[transit_agent_idx] = dest_mode
+ next_node_init[transit_agent_idx] = next_init
for agent_idx in next_node_agent:
- if agent_idx == transit_agent_idx:
- next_node_init[agent_idx] = next_init
- else:
+ if agent_idx not in next_node_init:
next_node_trace[agent_idx] = truncated_trace[agent_idx]
tmp = AnalysisTreeNode(
diff --git a/dryvr_plus_plus/scene_verifier/analysis/verifier.py b/dryvr_plus_plus/scene_verifier/analysis/verifier.py
index 5267f5e8efb645485ebabdf5958f831fe59123cc..56a131b72dd8dd2883c0bd3ea146012c610afd10 100644
--- a/dryvr_plus_plus/scene_verifier/analysis/verifier.py
+++ b/dryvr_plus_plus/scene_verifier/analysis/verifier.py
@@ -21,6 +21,7 @@ class Verifier:
agent_list:List[BaseAgent],
transition_graph,
time_horizon,
+ time_step,
lane_map
):
root = AnalysisTreeNode()
@@ -35,8 +36,8 @@ class Verifier:
verification_queue.append(root)
while verification_queue != []:
node:AnalysisTreeNode = verification_queue.pop(0)
- print(node.mode)
- remain_time = time_horizon - node.start_time
+ print(node.start_time, node.mode)
+ remain_time = round(time_horizon - node.start_time,10)
if remain_time <= 0:
continue
# For reachtubes not already computed
@@ -53,6 +54,7 @@ class Verifier:
cur_bloated_tube = calc_bloated_tube(mode,
init,
remain_time,
+ time_step,
node.agent[agent_id].TC_simulate,
'PW',
100,
@@ -67,7 +69,7 @@ class Verifier:
# TODO: Check safety conditions here
# Get all possible transitions to next mode
- all_possible_transitions = transition_graph.get_all_transition_set(node)
+ all_possible_transitions = transition_graph.get_transition_verify_new(node)
max_end_idx = 0
for transition in all_possible_transitions:
transit_agent_idx, src_mode, dest_mode, next_init, idx = transition
@@ -100,7 +102,7 @@ class Verifier:
mode = next_node_mode,
agent = next_node_agent,
child = [],
- start_time = next_node_start_time,
+ start_time = round(next_node_start_time,10),
type = 'reachtube'
)
node.child.append(tmp)
diff --git a/dryvr_plus_plus/scene_verifier/automaton/guard.py b/dryvr_plus_plus/scene_verifier/automaton/guard.py
index a96fd0554fef02d0b7bee38a3b58e08fc320206d..33908c1caeb5f008576572e9876f1e940b5bf2ac 100644
--- a/dryvr_plus_plus/scene_verifier/automaton/guard.py
+++ b/dryvr_plus_plus/scene_verifier/automaton/guard.py
@@ -1,9 +1,7 @@
import enum
import re
-from typing import List, Dict
+from typing import List, Dict, Any
import pickle
-# from scene_verifier.automaton.hybrid_io_automaton import HybridIoAutomaton
-# from pythonparser import Guard
import ast
import copy
@@ -25,13 +23,74 @@ class LogicTreeNode:
self.mode_guard = mode_guard
+class NodeSubstituter(ast.NodeTransformer):
+ def __init__(self, old_node, new_node):
+ super().__init__()
+ self.old_node = old_node
+ self.new_node = new_node
+
+ def visit_Call(self, node: ast.Call) -> Any:
+ if node == self.old_node:
+ self.generic_visit(node)
+ return self.new_node
+ else:
+ self.generic_visit(node)
+ return node
+
+class ValueSubstituter(ast.NodeTransformer):
+ def __init__(self, val:str, node):
+ super().__init__()
+ self.val = val
+ self.node = node
+
+ def visit_Attribute(self, node: ast.Attribute) -> Any:
+ # Substitute attribute node in the ast
+ if node == self.node:
+ return ast.Name(
+ id = self.val,
+ ctx = ast.Load()
+ )
+ return node
+
+ def visit_Name(self, node: ast.Attribute) -> Any:
+ # Substitute name node in the ast
+ if node == self.node:
+ return ast.Name(
+ id = self.val,
+ ctx = ast.Load
+ )
+ return node
+
+ def visit_Call(self, node: ast.Call) -> Any:
+ # Substitute call node in the ast
+ if node == self.node:
+ if len(self.val) == 1:
+ self.generic_visit(node)
+ return self.val[0]
+ elif node.func.id == 'any':
+ self.generic_visit(node)
+ return ast.BoolOp(
+ op = ast.Or(),
+ values = self.val
+ )
+ elif node.func.id == 'all':
+ self.generic_visit(node)
+ return ast.BoolOp(
+ op = ast.And(),
+ values = self.val
+ )
+ self.generic_visit(node)
+ return node
+
+
class GuardExpressionAst:
def __init__(self, guard_list):
self.ast_list = []
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):
"""
@@ -139,8 +198,6 @@ class GuardExpressionAst:
# The reachtube hits the guard
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:
@@ -210,7 +267,10 @@ class GuardExpressionAst:
else:
return False
z3_str.append(tmp)
- z3_str = 'And('+','.join(z3_str)+')'
+ if len(z3_str) == 1:
+ z3_str = z3_str[0]
+ else:
+ z3_str = 'And('+','.join(z3_str)+')'
return z3_str
elif isinstance(node.op, ast.Or):
z3_str = []
@@ -222,7 +282,10 @@ class GuardExpressionAst:
else:
continue
z3_str.append(tmp)
- z3_str = 'Or('+','.join(z3_str)+')'
+ if len(z3_str) == 1:
+ z3_str = z3_str[0]
+ else:
+ z3_str = 'Or('+','.join(z3_str)+')'
return z3_str
# If string, construct string
# If bool, check result and discard/evaluate result according to operator
@@ -241,6 +304,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)
@@ -283,13 +351,12 @@ class GuardExpressionAst:
break
return res, root
elif isinstance(root.op, ast.Or):
+ res = False
for val in root.values:
tmp, val = self._evaluate_guard_hybrid(
val, agent, disc_var_dict, cont_var_dict, lane_map)
res = res or tmp
- if res:
- break
- return res, root
+ return res, root
elif isinstance(root, ast.BinOp):
left, root.left = self._evaluate_guard_hybrid(
root.left, agent, disc_var_dict, cont_var_dict, lane_map)
@@ -304,8 +371,12 @@ class GuardExpressionAst:
# Get function arguments
arg0_node = root.args[0]
arg1_node = root.args[1]
- assert isinstance(arg0_node, ast.Attribute)
- arg0_var = arg0_node.value.id + '.' + arg0_node.attr
+ if isinstance(arg0_node, ast.Attribute):
+ arg0_var = arg0_node.value.id + '.' + arg0_node.attr
+ elif isinstance(arg0_node, ast.Name):
+ arg0_var = arg0_node.id
+ else:
+ raise ValueError(f"Node type {type(arg0_node)} is not supported")
vehicle_lane = disc_var_dict[arg0_var]
assert isinstance(arg1_node, ast.List)
arg1_lower = []
@@ -313,8 +384,12 @@ class GuardExpressionAst:
for elt in arg1_node.elts:
if isinstance(elt, ast.Attribute):
var = elt.value.id + '.' + elt.attr
- arg1_lower.append(cont_var_dict[var][0])
- arg1_upper.append(cont_var_dict[var][1])
+ elif isinstance(elt, ast.Name):
+ var = elt.id
+ else:
+ raise ValueError(f"Node type {type(elt)} is not supported")
+ arg1_lower.append(cont_var_dict[var][0])
+ arg1_upper.append(cont_var_dict[var][1])
vehicle_pos = (arg1_lower, arg1_upper)
# Get corresponding lane segments with respect to the set of vehicle pos
@@ -344,8 +419,13 @@ class GuardExpressionAst:
# Get function arguments
arg0_node = root.args[0]
arg1_node = root.args[1]
- assert isinstance(arg0_node, ast.Attribute)
- arg0_var = arg0_node.value.id + '.' + arg0_node.attr
+ # assert isinstance(arg0_node, ast.Attribute)
+ if isinstance(arg0_node, ast.Attribute):
+ arg0_var = arg0_node.value.id + '.' + arg0_node.attr
+ elif isinstance(arg0_node, ast.Name):
+ arg0_var = arg0_node.id
+ else:
+ raise ValueError(f"Node type {type(arg0_node)} is not supported")
vehicle_lane = disc_var_dict[arg0_var]
assert isinstance(arg1_node, ast.List)
arg1_lower = []
@@ -353,8 +433,12 @@ class GuardExpressionAst:
for elt in arg1_node.elts:
if isinstance(elt, ast.Attribute):
var = elt.value.id + '.' + elt.attr
- arg1_lower.append(cont_var_dict[var][0])
- arg1_upper.append(cont_var_dict[var][1])
+ elif isinstance(elt, ast.Name):
+ var = elt.id
+ else:
+ raise ValueError(f"Node type {type(elt)} is not supported")
+ arg1_lower.append(cont_var_dict[var][0])
+ arg1_upper.append(cont_var_dict[var][1])
vehicle_pos = (arg1_lower, arg1_upper)
# Get corresponding lane segments with respect to the set of vehicle pos
@@ -392,11 +476,17 @@ class GuardExpressionAst:
elif isinstance(root, ast.Attribute):
return True, root
elif isinstance(root, ast.Constant):
- return root.value, root
+ return root.value, root
+ elif isinstance(root, ast.Name):
+ return True, root
elif isinstance(root, ast.UnaryOp):
if isinstance(root.op, ast.USub):
- res, root.operand = self._evaluate_guard_hybrid(
- root.operand, agent, disc_var_dict, cont_var_dict, lane_map)
+ res, root.operand = self._evaluate_guard_hybrid(root.operand, agent, disc_var_dict, cont_var_dict, lane_map)
+ elif isinstance(root.op, ast.Not):
+ res, root.operand = self._evaluate_guard_hybrid(root.operand, agent, disc_var_dict, cont_var_dict, lane_map)
+ if not res:
+ root.operand = ast.parse('False').body[0].value
+ return True, ast.parse('True').body[0].value
else:
raise ValueError(
f'Node type {root} from {astunparse.unparse(root)} is not supported')
@@ -568,9 +658,7 @@ class GuardExpressionAst:
tmp, val = self._evaluate_guard_disc(
val, agent, disc_var_dict, cont_var_dict, lane_map)
res = res or tmp
- if res:
- break
- return res, root
+ return res, root
elif isinstance(root, ast.BinOp):
# Check left and right in the binop and replace all attributes involving discrete variables
left, root.left = self._evaluate_guard_disc(
@@ -598,6 +686,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:
@@ -620,21 +712,35 @@ class GuardExpressionAst:
return root.value, root
elif isinstance(root, ast.UnaryOp):
if isinstance(root.op, ast.USub):
- res, root.operand = self._evaluate_guard_disc(
- root.operand, agent, disc_var_dict, cont_var_dict, lane_map)
+ res, root.operand = self._evaluate_guard_disc(root.operand, agent, disc_var_dict, cont_var_dict, lane_map)
+ elif isinstance(root.op, ast.Not):
+ res, root.operand = self._evaluate_guard_disc(root.operand, agent, disc_var_dict, cont_var_dict, lane_map)
+ if not res:
+ root.operand = ast.parse('False').body[0].value
+ return True, ast.parse('True').body[0].value
else:
raise ValueError(
f'Node type {root} from {astunparse.unparse(root)} is not supported')
return True, root
+ elif isinstance(root, ast.Name):
+ expr = root.id
+ if expr in disc_var_dict:
+ val = disc_var_dict[expr]
+ for mode_name in agent.controller.modes:
+ if val in agent.controller.modes[mode_name]:
+ val = mode_name + '.' + val
+ break
+ return val, root
+ else:
+ return True, root
else:
raise ValueError(
f'Node type {root} from {astunparse.unparse(root)} is not supported')
def evaluate_guard(self, agent, continuous_variable_dict, discrete_variable_dict, lane_map):
res = True
- for node in self.ast_list:
- tmp = self._evaluate_guard(
- node, agent, continuous_variable_dict, discrete_variable_dict, lane_map)
+ for i, node in enumerate(self.ast_list):
+ tmp = self._evaluate_guard(node, agent, continuous_variable_dict, discrete_variable_dict, lane_map)
res = tmp and res
if not res:
break
@@ -696,7 +802,8 @@ class GuardExpressionAst:
elif isinstance(root, ast.Call):
expr = astunparse.unparse(root)
# Check if the root is a function
- if 'map' in expr:
+ if isinstance(root.func, ast.Attribute) and "map" in root.func.value.id:
+ # if 'map' in expr:
# tmp = re.split('\(|\)',expr)
# while "" in tmp:
# tmp.remove("")
@@ -709,6 +816,10 @@ class GuardExpressionAst:
for arg in cnts_var_dict:
expr = expr.replace(arg, str(cnts_var_dict[arg]))
res = eval(expr)
+ for mode_name in agent.controller.modes:
+ if res in agent.controller.modes[mode_name]:
+ res = mode_name+'.'+res
+ break
return res
elif isinstance(root, ast.Attribute):
expr = astunparse.unparse(root)
@@ -732,14 +843,278 @@ class GuardExpressionAst:
root.operand, agent, cnts_var_dict, disc_var_dict, lane_map)
if isinstance(root.op, ast.USub):
return -val
+ if isinstance(root.op, ast.Not):
+ return not val
else:
- raise ValueError(
- f'Node type {root} from {astunparse.unparse(root)} is not supported')
+ raise ValueError(f'Node type {root} from {astunparse.unparse(root)} is not supported')
+ elif isinstance(root, ast.Name):
+ variable = root.id
+ if variable in cnts_var_dict:
+ val = cnts_var_dict[variable]
+ return val
+ elif variable in disc_var_dict:
+ val = disc_var_dict[variable]
+ for mode_name in agent.controller.modes:
+ if val in agent.controller.modes[mode_name]:
+ val = mode_name+'.'+val
+ break
+ return val
+ else:
+ raise ValueError(f"{variable} doesn't exist in either continuous varibales or discrete variables")
else:
raise ValueError(
f'Node type {root} from {astunparse.unparse(root)} is not supported')
+ def parse_any_all(self, cont_var_dict: Dict[str, float], disc_var_dict: Dict[str, float], len_dict: Dict[str, int]) -> None:
+ for i in range(len(self.ast_list)):
+ root = self.ast_list[i]
+ j = 0
+ while j < sum(1 for _ in ast.walk(root)):
+ # TODO: Find a faster way to access nodes in the tree
+ node = list(ast.walk(root))[j]
+ if isinstance(node, ast.Call) and\
+ isinstance(node.func, ast.Name) and\
+ (node.func.id=='any' or node.func.id=='all'):
+ new_node = self.unroll_any_all(node, cont_var_dict, disc_var_dict, len_dict)
+ root = NodeSubstituter(node, new_node).visit(root)
+ j += 1
+ self.ast_list[i] = root
+
+ def unroll_any_all(
+ self, node: ast.Call,
+ cont_var_dict: Dict[str, float],
+ disc_var_dict: Dict[str, float],
+ len_dict: Dict[str, float]
+ ) -> ast.BoolOp:
+ parse_arg = node.args[0]
+ if isinstance(parse_arg, ast.GeneratorExp):
+ iter_name_list = []
+ targ_name_list = []
+ iter_len_list = []
+ # Get all the iter, targets and the length of iter list
+ for generator in parse_arg.generators:
+ iter_name_list.append(generator.iter.id) # a_list
+ targ_name_list.append(generator.target.id) # a
+ iter_len_list.append(range(len_dict[generator.iter.id])) # len(a_list)
+
+ elt = parse_arg.elt
+ expand_elt_ast_list = []
+ iter_len_list = list(itertools.product(*iter_len_list))
+ # Loop through all possible combination of iter value
+ for i in range(len(iter_len_list)):
+ changed_elt = copy.deepcopy(elt)
+ iter_pos_list = iter_len_list[i]
+ # substitute temporary variable in each of the elt and add corresponding variables in the variable dicts
+ parsed_elt = self._parse_elt(changed_elt, cont_var_dict, disc_var_dict, iter_name_list, targ_name_list, iter_pos_list)
+ # Add the expanded elt into the list
+ expand_elt_ast_list.append(parsed_elt)
+ # Create the new boolop (and/or) node based on the list of expanded elt
+ return ValueSubstituter(expand_elt_ast_list, node).visit(node)
+ else:
+ return node
+
+ def _parse_elt(self, root, cont_var_dict, disc_var_dict, iter_name_list, targ_name_list, iter_pos_list) -> Any:
+ # Loop through all node in the elt ast
+ for node in ast.walk(root):
+ # If the node is an attribute
+ if isinstance(node, ast.Attribute):
+ if node.value.id in targ_name_list:
+ # Find corresponding targ_name in the targ_name_list
+ targ_name = node.value.id
+ var_index = targ_name_list.index(targ_name)
+
+ # Find the corresponding iter_name in the iter_name_list
+ iter_name = iter_name_list[var_index]
+
+ # Create the name for the tmp variable
+ iter_pos = iter_pos_list[var_index]
+ tmp_variable_name = f"{iter_name}_{iter_pos}.{node.attr}"
+
+ # Replace variables in the etl by using tmp variables
+ root = ValueSubstituter(tmp_variable_name, node).visit(root)
+
+ # Find the value of the tmp variable in the cont/disc_var_dict
+ # Add the tmp variables into the cont/disc_var_dict
+ # NOTE: At each time step, for each agent, the variable value mapping and their
+ # sequence in the list is single. Therefore, for the same key, we will always rewrite
+ # its content.
+ variable_name = iter_name + '.' + node.attr
+ variable_val = None
+ if variable_name in cont_var_dict:
+ variable_val = cont_var_dict[variable_name][iter_pos]
+ cont_var_dict[tmp_variable_name] = variable_val
+ elif variable_name in disc_var_dict:
+ variable_val = disc_var_dict[variable_name][iter_pos]
+ disc_var_dict[tmp_variable_name] = variable_val
+
+ elif isinstance(node, ast.Name):
+ if node.id in targ_name_list:
+ node:ast.Name
+ # Find corresponding targ_name in the targ_name_list
+ targ_name = node.id
+ var_index = targ_name_list.index(targ_name)
+
+ # Find the corresponding iter_name in the iter_name_list
+ iter_name = iter_name_list[var_index]
+
+ # Create the name for the tmp variable
+ iter_pos = iter_pos_list[var_index]
+ tmp_variable_name = f"{iter_name}_{iter_pos}"
+
+ # Replace variables in the etl by using tmp variables
+ root = ValueSubstituter(tmp_variable_name, node).visit(root)
+
+ # Find the value of the tmp variable in the cont/disc_var_dict
+ # Add the tmp variables into the cont/disc_var_dict
+ variable_name = iter_name
+ variable_val = None
+ if variable_name in cont_var_dict:
+ variable_val = cont_var_dict[variable_name][iter_pos]
+ cont_var_dict[tmp_variable_name] = variable_val
+ elif variable_name in disc_var_dict:
+ variable_val = disc_var_dict[variable_name][iter_pos]
+ disc_var_dict[tmp_variable_name] = variable_val
+
+ # Return the modified node
+ return root
+
+ def parse_any_all_new(self, cont_var_dict: Dict[str, float], disc_var_dict: Dict[str, float], len_dict: Dict[str, int]) -> Dict[str, List[str]]:
+ cont_var_updater = {}
+ for i in range(len(self.ast_list)):
+ root = self.ast_list[i]
+ j = 0
+ while j < sum(1 for _ in ast.walk(root)):
+ # TODO: Find a faster way to access nodes in the tree
+ node = list(ast.walk(root))[j]
+ if isinstance(node, ast.Call) and\
+ isinstance(node.func, ast.Name) and\
+ (node.func.id=='any' or node.func.id=='all'):
+ new_node = self.unroll_any_all_new(node, cont_var_dict, disc_var_dict, len_dict, cont_var_updater)
+ root = NodeSubstituter(node, new_node).visit(root)
+ j += 1
+ self.ast_list[i] = root
+ return cont_var_updater
+
+ def unroll_any_all_new(
+ self, node: ast.Call,
+ cont_var_dict: Dict[str, float],
+ disc_var_dict: Dict[str, float],
+ len_dict: Dict[str, float],
+ cont_var_updater: Dict[str, List[str]],
+ ) -> Tuple[ast.BoolOp, Dict[str, List[str]]]:
+ parse_arg = node.args[0]
+ if isinstance(parse_arg, ast.GeneratorExp):
+ iter_name_list = []
+ targ_name_list = []
+ iter_len_list = []
+ # Get all the iter, targets and the length of iter list
+ for generator in parse_arg.generators:
+ iter_name_list.append(generator.iter.id) # a_list
+ targ_name_list.append(generator.target.id) # a
+ iter_len_list.append(range(len_dict[generator.iter.id])) # len(a_list)
+
+ elt = parse_arg.elt
+ expand_elt_ast_list = []
+ iter_len_list = list(itertools.product(*iter_len_list))
+ # Loop through all possible combination of iter value
+ for i in range(len(iter_len_list)):
+ changed_elt = copy.deepcopy(elt)
+ iter_pos_list = iter_len_list[i]
+ # substitute temporary variable in each of the elt and add corresponding variables in the variable dicts
+ parsed_elt = self._parse_elt_new(changed_elt, cont_var_dict, disc_var_dict, cont_var_updater, iter_name_list, targ_name_list, iter_pos_list)
+ # Add the expanded elt into the list
+ expand_elt_ast_list.append(parsed_elt)
+ # Create the new boolop (and/or) node based on the list of expanded elt
+ return ValueSubstituter(expand_elt_ast_list, node).visit(node)
+ else:
+ return node
+
+ def _parse_elt_new(self, root, cont_var_dict, disc_var_dict, cont_var_updater, iter_name_list, targ_name_list, iter_pos_list) -> Any:
+ # Loop through all node in the elt ast
+ for node in ast.walk(root):
+ # If the node is an attribute
+ if isinstance(node, ast.Attribute):
+ if node.value.id in targ_name_list:
+ # Find corresponding targ_name in the targ_name_list
+ targ_name = node.value.id
+ var_index = targ_name_list.index(targ_name)
+
+ # Find the corresponding iter_name in the iter_name_list
+ iter_name = iter_name_list[var_index]
+
+ # Create the name for the tmp variable
+ iter_pos = iter_pos_list[var_index]
+ tmp_variable_name = f"{iter_name}_{iter_pos}.{node.attr}"
+
+ # Replace variables in the etl by using tmp variables
+ root = ValueSubstituter(tmp_variable_name, node).visit(root)
+
+ # Find the value of the tmp variable in the cont/disc_var_dict
+ # Add the tmp variables into the cont/disc_var_dict
+ # NOTE: At each time step, for each agent, the variable value mapping and their
+ # sequence in the list is single. Therefore, for the same key, we will always rewrite
+ # its content.
+ variable_name = iter_name + '.' + node.attr
+ variable_val = None
+ if variable_name in cont_var_dict:
+ # variable_val = cont_var_dict[variable_name][iter_pos]
+ # cont_var_dict[tmp_variable_name] = variable_val
+ if variable_name not in cont_var_updater:
+ cont_var_updater[variable_name] = [(tmp_variable_name, iter_pos)]
+ else:
+ if (tmp_variable_name, iter_pos) not in cont_var_updater[variable_name]:
+ cont_var_updater[variable_name].append((tmp_variable_name, iter_pos))
+ elif variable_name in disc_var_dict:
+ variable_val = disc_var_dict[variable_name][iter_pos]
+ disc_var_dict[tmp_variable_name] = variable_val
+ # if variable_name not in disc_var_updater:
+ # disc_var_updater[variable_name] = [(tmp_variable_name, iter_pos)]
+ # else:
+ # if (tmp_variable_name, iter_pos) not in disc_var_updater[variable_name]:
+ # disc_var_updater[variable_name].append((tmp_variable_name, iter_pos))
+
+ elif isinstance(node, ast.Name):
+ if node.id in targ_name_list:
+ node:ast.Name
+ # Find corresponding targ_name in the targ_name_list
+ targ_name = node.id
+ var_index = targ_name_list.index(targ_name)
+
+ # Find the corresponding iter_name in the iter_name_list
+ iter_name = iter_name_list[var_index]
+
+ # Create the name for the tmp variable
+ iter_pos = iter_pos_list[var_index]
+ tmp_variable_name = f"{iter_name}_{iter_pos}"
+
+ # Replace variables in the etl by using tmp variables
+ root = ValueSubstituter(tmp_variable_name, node).visit(root)
+
+ # Find the value of the tmp variable in the cont/disc_var_dict
+ # Add the tmp variables into the cont/disc_var_dict
+ variable_name = iter_name
+ variable_val = None
+ if variable_name in cont_var_dict:
+ # variable_val = cont_var_dict[variable_name][iter_pos]
+ # cont_var_dict[tmp_variable_name] = variable_val
+ if variable_name not in cont_var_updater:
+ cont_var_updater[variable_name] = [(tmp_variable_name, iter_pos)]
+ else:
+ if (tmp_variable_name, iter_pos) not in cont_var_updater[variable_name]:
+ cont_var_updater.append(tmp_variable_name, iter_pos)
+ elif variable_name in disc_var_dict:
+ variable_val = disc_var_dict[variable_name][iter_pos]
+ disc_var_dict[tmp_variable_name] = variable_val
+ # if variable_name not in disc_var_updater:
+ # disc_var_updater[variable_name] = [(tmp_variable_name, iter_pos)]
+ # else:
+ # if (tmp_variable_name, iter_pos) not in disc_var_updater[variable_name]:
+ # disc_var_updater[variable_name].append((tmp_variable_name, iter_pos))
+
+ # Return the modified node
+ return root
+
if __name__ == "__main__":
with open('tmp.pickle', 'rb') as f:
guard_list = pickle.load(f)
diff --git a/dryvr_plus_plus/scene_verifier/automaton/reset.py b/dryvr_plus_plus/scene_verifier/automaton/reset.py
index 96233b8b5b3be205e5598cc7480ef39c85decdff..699c369e3eaf95090a584b2363e4de90901e3119 100644
--- a/dryvr_plus_plus/scene_verifier/automaton/reset.py
+++ b/dryvr_plus_plus/scene_verifier/automaton/reset.py
@@ -71,7 +71,10 @@ class ResetExpression:
tmp = tmp[1]
for var in discrete_variable_dict:
tmp = tmp.replace(var, f"'{discrete_variable_dict[var]}'")
- possible_dest[i] = eval(tmp)
+ res = eval(tmp)
+ if not isinstance(res, list):
+ res = [res]
+ possible_dest[i] = res
else:
tmp = tmp[1].split('.')
if tmp[0].strip(' ') in agent.controller.modes:
diff --git a/dryvr_plus_plus/scene_verifier/code_parser/pythonparser.py b/dryvr_plus_plus/scene_verifier/code_parser/pythonparser.py
index 7f75ed5a7b26d38d1dd429b5639dcab60580f985..d276e52d5d6264797e8e5bf0dac4fc94d1842121 100644
--- a/dryvr_plus_plus/scene_verifier/code_parser/pythonparser.py
+++ b/dryvr_plus_plus/scene_verifier/code_parser/pythonparser.py
@@ -81,12 +81,11 @@ class Guard(Statement):
return Guard(ast.get_source_segment(code, node.test), None, None, node.test)
elif isinstance(node.test, ast.Call): # function not used
source_segment = ast.get_source_segment(code, node.test)
- if "map" in source_segment:
- func = node.test.func.value.id + '.' + node.test.func.attr
- args = []
- for arg in node.test.args:
- args.append(arg.value.id + '.' + arg.attr)
- return Guard(source_segment, None, None, node.test, func, args)
+ # func = node.test.func.id
+ # args = []
+ # for arg in node.test.args:
+ # args.append(arg.value.id + '.' + arg.attr)
+ return Guard(source_segment, None, None, node.test)
'''
@@ -266,8 +265,7 @@ class ControllerAst():
self.code = code
self.tree = ast.parse(code)
- self.statementtree, self.variables, self.modes, self.discrete_variables, self.state_object_dict, self.vars_dict, self.type_vars = self.initalwalktree(
- code, self.tree)
+ self.statementtree, self.variables, self.modes, self.discrete_variables, self.state_object_dict, self.vars_dict = self.initalwalktree(code, self.tree)
self.vertices = []
self.vertexStrings = []
for vertex in itertools.product(*self.modes.values()):
@@ -390,7 +388,6 @@ class ControllerAst():
def initalwalktree(self, code, tree):
vars = []
discrete_vars = []
- type_vars = []
out = []
mode_dict = {}
state_object_dict = {}
@@ -415,12 +412,8 @@ class ControllerAst():
if "init" in item.name:
for arg in item.args.args:
if "self" not in arg.arg:
- if "type" == arg.arg:
- state_object_dict[node.name]["type"].append(
- arg.arg)
- elif "mode" not in arg.arg:
- state_object_dict[node.name]['cont'].append(
- arg.arg)
+ if "mode" not in arg.arg:
+ state_object_dict[node.name]['cont'].append(arg.arg)
# vars.append(arg.arg)
else:
state_object_dict[node.name]['disc'].append(
@@ -436,9 +429,22 @@ class ControllerAst():
for arg in args:
if arg.annotation is None:
continue
- if arg.annotation.id not in state_object_dict:
- continue
- arg_annotation = arg.annotation.id
+ # Handle case when input is a single variable
+ if isinstance(arg.annotation, ast.Name):
+ if arg.annotation.id not in state_object_dict:
+ continue
+ arg_annotation = arg.annotation.id
+ # Handle case when input is a list of variables
+ elif isinstance(arg.annotation, ast.Subscript):
+ if isinstance(arg.annotation.slice, ast.Index):
+ if arg.annotation.slice.value.id not in state_object_dict:
+ continue
+ arg_annotation = arg.annotation.slice.value.id
+ elif isinstance(arg.annotation.slice, ast.Name):
+ if arg.annotation.slice.id not in state_object_dict:
+ continue
+ arg_annotation = arg.annotation.slice.id
+
arg_name = arg.arg
vars_dict[arg_name] = {
'cont': [], 'disc': [], "type": []}
@@ -448,17 +454,8 @@ class ControllerAst():
for var in state_object_dict[arg_annotation]['disc']:
discrete_vars.append(arg_name+"."+var)
vars_dict[arg_name]['disc'].append(var)
- for var in state_object_dict[arg_annotation]['type']:
- type_vars.append(arg_name+"."+var)
- vars_dict[arg_name]['type'].append(var)
+ return [statementtree, vars, mode_dict, discrete_vars, state_object_dict, vars_dict]
- # if "mode" not in arg.arg:
- # vars.append(arg.arg)
- # #todo: what to add for return values
- # else:
- # discrete_vars.append(arg.arg)
- # statementtree.show()
- return [statementtree, vars, mode_dict, discrete_vars, state_object_dict, vars_dict, type_vars]
'''
Helper function for initalwalktree which parses the statements in the controller function into a statement tree
@@ -533,7 +530,6 @@ class EmptyAst(ControllerAst):
'type': []
}
}
- self.type_vars = []
self.variables = []
self.vars_dict = []
self.vertexStrings = ['Null,Normal']
diff --git a/dryvr_plus_plus/scene_verifier/dryvr/common/utils.py b/dryvr_plus_plus/scene_verifier/dryvr/common/utils.py
index 1276754aadeea4f64a161b89f624db6cbcdbaf98..7b79b60897d57c8a19d5aa989f8d0f941688af1e 100644
--- a/dryvr_plus_plus/scene_verifier/dryvr/common/utils.py
+++ b/dryvr_plus_plus/scene_verifier/dryvr/common/utils.py
@@ -65,7 +65,7 @@ def randomPoint(lower, upper):
random point (either float or list of float)
"""
- # random.seed(4)
+ random.seed(4)
if isinstance(lower, int) or isinstance(lower, float):
return random.uniform(lower, upper)
diff --git a/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrcore.py b/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrcore.py
index f3bd8861809044283c381e7665c12a1f150a685c..bbb1ccfe00b07b393d6ae00e26a31d6fe7bd7530 100644
--- a/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrcore.py
+++ b/dryvr_plus_plus/scene_verifier/dryvr/core/dryvrcore.py
@@ -245,6 +245,7 @@ def calc_bloated_tube(
mode_label,
initial_set,
time_horizon,
+ time_step,
sim_func,
bloating_method,
kvalue,
@@ -275,11 +276,11 @@ def calc_bloated_tube(
random.seed(4)
cur_center = calcCenterPoint(initial_set[0], initial_set[1])
cur_delta = calcDelta(initial_set[0], initial_set[1])
- traces = [sim_func(mode_label, cur_center, time_horizon, lane_map)]
+ traces = [sim_func(mode_label, cur_center, time_horizon, time_step, lane_map)]
# Simulate SIMTRACENUM times to learn the sensitivity
for _ in range(sim_trace_num):
new_init_point = randomPoint(initial_set[0], initial_set[1])
- traces.append(sim_func(mode_label, new_init_point, time_horizon, lane_map))
+ traces.append(sim_func(mode_label, new_init_point, time_horizon, time_step, lane_map))
# Trim the trace to the same length
traces = trimTraces(traces)
@@ -294,11 +295,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 579ab29b594b05e37aa37dbd79404aa1ccd5b437..b294aae441508b220604e1baa6149099c8daa7f0 100644
--- a/dryvr_plus_plus/scene_verifier/dryvr/core/guard.py
+++ b/dryvr_plus_plus/scene_verifier/dryvr/core/guard.py
@@ -178,7 +178,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:
@@ -213,11 +212,8 @@ class Guard:
init_upper = guard_set_upper[0][1:]
for j in range(1, len(guard_set_lower)):
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_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])
# 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/map/lane_map.py b/dryvr_plus_plus/scene_verifier/map/lane_map.py
index 9f0d4ffc26ddca1482548950022a427e4e77acff..c4a5672e12b9cebe520c9e7ffbb9a52bea3da892 100644
--- a/dryvr_plus_plus/scene_verifier/map/lane_map.py
+++ b/dryvr_plus_plus/scene_verifier/map/lane_map.py
@@ -42,8 +42,7 @@ class LaneMap:
if lane_idx not in self.left_lane_dict:
raise ValueError(f"lane_idx {lane_idx} not in lane_dict")
left_lane_list = self.left_lane_dict[lane_idx]
- return copy.deepcopy(left_lane_list)
-
+ return copy.deepcopy(left_lane_list[0])
def has_right(self, lane_idx):
if isinstance(lane_idx, Enum):
lane_idx = lane_idx.name
@@ -60,7 +59,7 @@ class LaneMap:
if lane_idx not in self.right_lane_dict:
raise ValueError(f"lane_idx {lane_idx} not in lane_dict")
right_lane_list = self.right_lane_dict[lane_idx]
- return copy.deepcopy(right_lane_list)
+ return copy.deepcopy(right_lane_list[0])
def lane_geometry(self, lane_idx):
if isinstance(lane_idx, Enum):
diff --git a/dryvr_plus_plus/scene_verifier/scenario/scenario.py b/dryvr_plus_plus/scene_verifier/scenario/scenario.py
index dcbf44ed628f17d632d04fa69db4fda989fe9232..e42f2d6930a26a41d5a213c2c9af2896e4bcc6b2 100644
--- a/dryvr_plus_plus/scene_verifier/scenario/scenario.py
+++ b/dryvr_plus_plus/scene_verifier/scenario/scenario.py
@@ -4,6 +4,7 @@ import itertools
import warnings
import numpy as np
+from sympy import Q
from dryvr_plus_plus.scene_verifier.agents.base_agent import BaseAgent
from dryvr_plus_plus.scene_verifier.automaton.guard import GuardExpressionAst
@@ -13,6 +14,9 @@ from dryvr_plus_plus.scene_verifier.analysis.simulator import Simulator
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:
@@ -22,8 +26,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
@@ -43,7 +47,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))
@@ -63,8 +67,8 @@ class Scenario:
trace = self.simulate(time_horizon)
res_list.append(trace)
return res_list
-
- def simulate(self, time_horizon):
+
+ def simulate(self, time_horizon, time_step):
init_list = []
init_mode_list = []
agent_list = []
@@ -73,9 +77,9 @@ class Scenario:
init_mode_list.append(self.init_mode_dict[agent_id])
agent_list.append(self.agent_dict[agent_id])
print(init_list)
- return self.simulator.simulate(init_list, init_mode_list, agent_list, self, time_horizon, self.map)
+ return self.simulator.simulate(init_list, init_mode_list, agent_list, self, time_horizon, time_step, self.map)
- def verify(self, time_horizon):
+ def verify(self, time_horizon, time_step):
init_list = []
init_mode_list = []
agent_list = []
@@ -88,12 +92,12 @@ class Scenario:
init_list.append(init)
init_mode_list.append(self.init_mode_dict[agent_id])
agent_list.append(self.agent_dict[agent_id])
- return self.verifier.compute_full_reachtube(init_list, init_mode_list, agent_list, self, time_horizon, self.map)
+ return self.verifier.compute_full_reachtube(init_list, init_mode_list, agent_list, self, time_horizon, time_step, self.map)
def check_guard_hit(self, state_dict):
lane_map = self.map
guard_hits = []
- any_contained = False # TODO: Handle this
+ any_contained = False
for agent_id in state_dict:
agent: BaseAgent = self.agent_dict[agent_id]
agent_state, agent_mode = state_dict[agent_id]
@@ -113,8 +117,10 @@ class Scenario:
# guard_expression = GuardExpression(guard_list=guard_list)
guard_expression = GuardExpressionAst(guard_list)
# Map the values to variables using sensor
- continuous_variable_dict, discrete_variable_dict = self.sensor.sense(
- self, agent, state_dict, self.map)
+ continuous_variable_dict, discrete_variable_dict, length_dict = self.sensor.sense(self, agent, state_dict, self.map)
+
+ # Unroll all the any/all functions in the guard
+ guard_expression.parse_any_all(continuous_variable_dict, discrete_variable_dict, length_dict)
# Check if the guard can be satisfied
# First Check if the discrete guards can be satisfied by actually evaluate the values
@@ -124,10 +130,10 @@ class Scenario:
if not guard_can_satisfied:
continue
- # TODO: Handle hybrid guards that involves both continuous and discrete dynamics
# Will have to limit the amount of hybrid guards that we want to handle. The difficulty will be handle function guards.
- guard_can_satisfied = guard_expression.evaluate_guard_hybrid(
- agent, discrete_variable_dict, continuous_variable_dict, self.map)
+ guard_can_satisfied = guard_expression.evaluate_guard_hybrid(agent, discrete_variable_dict, continuous_variable_dict, self.map)
+ if not guard_can_satisfied:
+ continue
# Handle guards realted only to continuous variables using SMT solvers. These types of guards can be pretty general
guard_satisfied, is_contained = guard_expression.evaluate_guard_cont(
@@ -137,7 +143,7 @@ class Scenario:
guard_hits.append((agent_id, guard_list, reset_list))
return guard_hits, any_contained
- def get_all_transition_set(self, node):
+ def get_transition_verify(self, node):
possible_transitions = []
trace_length = int(len(list(node.trace.values())[0])/2)
guard_hits = []
@@ -207,12 +213,9 @@ class Scenario:
def apply_reset(self, agent, reset_list, all_agent_state) -> Tuple[str, np.ndarray]:
reset_expr = ResetExpression(reset_list)
- continuous_variable_dict, discrete_variable_dict = self.sensor.sense(
- self, agent, all_agent_state, self.map)
- dest = reset_expr.get_dest(
- agent, all_agent_state[agent.id], discrete_variable_dict, self.map)
- rect = reset_expr.apply_reset_continuous(
- agent, continuous_variable_dict, self.map)
+ continuous_variable_dict, discrete_variable_dict, _ = self.sensor.sense(self, agent, all_agent_state, self.map)
+ dest = reset_expr.get_dest(agent, all_agent_state[agent.id], discrete_variable_dict, self.map)
+ rect = reset_expr.apply_reset_continuous(agent, continuous_variable_dict, self.map)
return dest, rect
def get_all_transition(self, state_dict: Dict[str, Tuple[List[float], List[str]]]):
@@ -237,10 +240,11 @@ class Scenario:
# guard_expression = GuardExpression(guard_list=guard_list)
guard_expression = GuardExpressionAst(guard_list)
# Map the values to variables using sensor
- continuous_variable_dict, discrete_variable_dict = self.sensor.sense(
- self, agent, state_dict, self.map)
- # print(continuous_variable_dict)
- # print(discrete_variable_dict)
+ continuous_variable_dict, discrete_variable_dict, length_dict = self.sensor.sense(self, agent, state_dict, self.map)
+
+ # Unroll all the any/all functions in the guard
+ guard_expression.parse_any_all(continuous_variable_dict, discrete_variable_dict, length_dict)
+
'''Execute functions related to map to see if the guard can be satisfied'''
'''Check guards related to modes to see if the guards can be satisfied'''
'''Actually plug in the values to see if the guards can be satisfied'''
@@ -270,10 +274,11 @@ class Scenario:
if 'map' in tmp[1]:
tmp = tmp[1]
for var in discrete_variable_dict:
- tmp = tmp.replace(
- var, f"'{discrete_variable_dict[var]}'")
- possible_dest[i] = eval(tmp)
- # like output.vehicle_mode = VehicleMode.Normal
+ tmp = tmp.replace(var, f"'{discrete_variable_dict[var]}'")
+ res = eval(tmp)
+ if not isinstance(res, list):
+ res = [res]
+ possible_dest[i] = res
else:
tmp = tmp[1].split('.')
if tmp[0].strip(' ') in agent.controller.modes:
@@ -304,3 +309,244 @@ class Scenario:
# print(next_transition)
satisfied_guard.append(next_transition)
return satisfied_guard
+
+ def get_transition_simulate(self, node:AnalysisTreeNode) -> Tuple[Dict[str,List[Tuple[float]]], int]:
+ trace_length = len(list(node.trace.values())[0])
+ transitions = {}
+ for idx in range(trace_length):
+ # For each trace, check with the guard to see if there's any possible transition
+ # Store all possible transition in a list
+ # A transition is defined by (agent, src_mode, dest_mode, corresponding reset, transit idx)
+ # Here we enforce that only one agent transit at a time
+ all_agent_state = {}
+ for agent_id in node.agent:
+ all_agent_state[agent_id] = (node.trace[agent_id][idx], node.mode[agent_id])
+ possible_transitions = self.get_all_transition(all_agent_state)
+ if possible_transitions != []:
+ for agent_idx, src_mode, dest_mode, next_init in possible_transitions:
+ if agent_idx not in transitions:
+ transitions[agent_idx] = [(agent_idx, src_mode, dest_mode, next_init, idx)]
+ else:
+ transitions[agent_idx].append((agent_idx, src_mode, dest_mode, next_init, idx))
+ break
+ return transitions, idx
+
+ def apply_cont_var_updater(self,cont_var_dict, updater):
+ for variable in updater:
+ for unrolled_variable, unrolled_variable_index in updater[variable]:
+ cont_var_dict[unrolled_variable] = cont_var_dict[variable][unrolled_variable_index]
+
+ # def apply_disc_var_updater(self,disc_var_dict, updater):
+ # for variable in updater:
+ # unrolled_variable, unrolled_variable_index = updater[variable]
+ # disc_var_dict[unrolled_variable] = disc_var_dict[variable][unrolled_variable_index]
+
+ def get_transition_simulate_new(self, node:AnalysisTreeNode) -> Tuple[Dict[str, List[Tuple[float]]], float]:
+ lane_map = self.map
+ trace_length = len(list(node.trace.values())[0])
+
+ # For each agent
+ agent_guard_dict:Dict[str,List[GuardExpressionAst]] = {}
+
+ for agent_id in node.agent:
+ # Get guard
+ agent:BaseAgent = self.agent_dict[agent_id]
+ agent_mode = node.mode[agent_id]
+ paths = agent.controller.getNextModes(agent_mode)
+ state_dict = {}
+ for tmp in node.agent:
+ state_dict[tmp] = (node.trace[tmp][0], node.mode[tmp])
+ cont_var_dict_template, discrete_variable_dict, len_dict = self.sensor.sense(self, agent, state_dict, self.map)
+ for path in paths:
+ guard_list = []
+ reset_list = []
+ for item in path:
+ if isinstance(item, Guard):
+ guard_list.append(item)
+ elif isinstance(item, Reset):
+ reset_list.append(item)
+ guard_expression = GuardExpressionAst(guard_list)
+
+ continuous_variable_updater = guard_expression.parse_any_all_new(cont_var_dict_template, discrete_variable_dict, len_dict)
+ if agent_id not in agent_guard_dict:
+ agent_guard_dict[agent_id] = [(guard_expression, continuous_variable_updater, copy.deepcopy(discrete_variable_dict), reset_list)]
+ else:
+ agent_guard_dict[agent_id].append((guard_expression, continuous_variable_updater, copy.deepcopy(discrete_variable_dict), reset_list))
+
+ transitions = {}
+ for idx in range(trace_length):
+ satisfied_guard = []
+ for agent_id in agent_guard_dict:
+ agent:BaseAgent = self.agent_dict[agent_id]
+ state_dict = {}
+ for tmp in node.agent:
+ state_dict[tmp] = (node.trace[tmp][idx], node.mode[tmp])
+ agent_state, agent_mode = state_dict[agent_id]
+ agent_state = agent_state[1:]
+ continuous_variable_dict, _, _ = self.sensor.sense(self, agent, state_dict, self.map)
+ for guard_expression, continuous_variable_updater, discrete_variable_dict, reset_list in agent_guard_dict[agent_id]:
+ new_cont_var_dict = copy.deepcopy(continuous_variable_dict)
+ # new_disc_var_dict = copy.deepcopy(discrete_variable_dict)
+ one_step_guard:GuardExpressionAst = copy.deepcopy(guard_expression)
+ self.apply_cont_var_updater(new_cont_var_dict, continuous_variable_updater)
+ # self.apply_disc_var_updater(new_disc_var_dict, discrete_variable_updater)
+ guard_satisfied = one_step_guard.evaluate_guard(agent, new_cont_var_dict, discrete_variable_dict, self.map)
+ if guard_satisfied:
+ # If the guard can be satisfied, handle resets
+ next_init = agent_state
+ dest = copy.deepcopy(agent_mode)
+ possible_dest = [[elem] for elem in dest]
+ for reset in reset_list:
+ # Specify the destination mode
+ reset = reset.code
+ if "mode" in reset:
+ for i, discrete_variable_ego in enumerate(agent.controller.vars_dict['ego']['disc']):
+ if discrete_variable_ego in reset:
+ break
+ tmp = reset.split('=')
+ if 'map' in tmp[1]:
+ tmp = tmp[1]
+ for var in discrete_variable_dict:
+ tmp = tmp.replace(var, f"'{discrete_variable_dict[var]}'")
+ res = eval(tmp)
+ if not isinstance(res, list):
+ res = [res]
+ possible_dest[i] = res
+ else:
+ tmp = tmp[1].split('.')
+ if tmp[0].strip(' ') in agent.controller.modes:
+ possible_dest[i] = [tmp[1]]
+ else:
+ for i, cts_variable in enumerate(agent.controller.vars_dict['ego']['cont']):
+ if "output."+cts_variable in reset:
+ break
+ tmp = reset.split('=')
+ tmp = tmp[1]
+ 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 = 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:
+ next_transition = (
+ agent_id, agent_mode, dest, next_init,
+ )
+ satisfied_guard.append(next_transition)
+ if satisfied_guard != []:
+ for agent_idx, src_mode, dest_mode, next_init in satisfied_guard:
+ if agent_idx not in transitions:
+ transitions[agent_idx] = [(agent_idx, src_mode, dest_mode, next_init, idx)]
+ else:
+ transitions[agent_idx].append((agent_idx, src_mode, dest_mode, next_init, idx))
+ break
+ return transitions, idx
+
+
+ def get_transition_verify_new(self, node:AnalysisTreeNode):
+ lane_map = self.map
+ possible_transitions = []
+
+ agent_guard_dict = {}
+ for agent_id in node.agent:
+ agent:BaseAgent = self.agent_dict[agent_id]
+ agent_mode = node.mode[agent_id]
+ state_dict = {}
+ for tmp in node.agent:
+ state_dict[tmp] = (node.trace[tmp][0*2:0*2+2], node.mode[tmp])
+
+ cont_var_dict_template, discrete_variable_dict, length_dict = self.sensor.sense(self, agent, state_dict, self.map)
+ paths = agent.controller.getNextModes(agent_mode)
+ for path in paths:
+ # Construct the guard expression
+ guard_list = []
+ reset_list = []
+ for item in path:
+ if isinstance(item, Guard):
+ guard_list.append(item)
+ elif isinstance(item, Reset):
+ reset_list.append(item)
+ guard_expression = GuardExpressionAst(guard_list)
+
+ cont_var_updater = guard_expression.parse_any_all_new(cont_var_dict_template, discrete_variable_dict, length_dict)
+ self.apply_cont_var_updater(cont_var_dict_template, cont_var_updater)
+ guard_can_satisfied = guard_expression.evaluate_guard_disc(agent, discrete_variable_dict, cont_var_dict_template, self.map)
+ if not guard_can_satisfied:
+ continue
+ if agent_id not in agent_guard_dict:
+ agent_guard_dict[agent_id] = [(guard_expression, cont_var_updater, copy.deepcopy(discrete_variable_dict), reset_list)]
+ else:
+ agent_guard_dict[agent_id].append((guard_expression, cont_var_updater, copy.deepcopy(discrete_variable_dict), reset_list))
+
+ trace_length = int(len(list(node.trace.values())[0])/2)
+ guard_hits = []
+ guard_hit_bool = False
+ for idx in range(0,trace_length):
+ any_contained = False
+ hits = []
+ state_dict = {}
+ for tmp in node.agent:
+ state_dict[tmp] = (node.trace[tmp][idx*2:idx*2+2], node.mode[tmp])
+
+ for agent_id in agent_guard_dict:
+ agent:BaseAgent = self.agent_dict[agent_id]
+ agent_state, agent_mode = state_dict[agent_id]
+ agent_state = agent_state[1:]
+ continuous_variable_dict, _, _ = self.sensor.sense(self, agent, state_dict, self.map)
+ for guard_expression, continuous_variable_updater, discrete_variable_dict, reset_list in agent_guard_dict[agent_id]:
+ new_cont_var_dict = copy.deepcopy(continuous_variable_dict)
+ one_step_guard:GuardExpressionAst = copy.deepcopy(guard_expression)
+
+ self.apply_cont_var_updater(new_cont_var_dict, continuous_variable_updater)
+ guard_can_satisfied = one_step_guard.evaluate_guard_hybrid(agent, discrete_variable_dict, new_cont_var_dict, self.map)
+ if not guard_can_satisfied:
+ continue
+ guard_satisfied, is_contained = one_step_guard.evaluate_guard_cont(agent, new_cont_var_dict, self.map)
+ any_contained = any_contained or is_contained
+ if guard_satisfied:
+ hits.append((agent_id, guard_list, reset_list))
+ if hits != []:
+ guard_hits.append((hits, state_dict, idx))
+ guard_hit_bool = True
+ if hits == [] and guard_hit_bool:
+ break
+ if any_contained:
+ break
+
+ reset_dict = {}
+ reset_idx_dict = {}
+ for hits, all_agent_state, hit_idx in guard_hits:
+ for agent_id, guard_list, reset_list in hits:
+ dest_list,reset_rect = self.apply_reset(node.agent[agent_id], reset_list, all_agent_state)
+ 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] = []
+ reset_idx_dict[agent_id][dest] = []
+ reset_dict[agent_id][dest].append(reset_rect)
+ reset_idx_dict[agent_id][dest].append(hit_idx)
+
+ # Combine reset rects and construct transitions
+ for agent in reset_dict:
+ for dest in reset_dict[agent]:
+ combined_rect = None
+ for rect in reset_dict[agent][dest]:
+ rect = np.array(rect)
+ if combined_rect is None:
+ combined_rect = rect
+ else:
+ combined_rect[0,:] = np.minimum(combined_rect[0,:], rect[0,:])
+ combined_rect[1,:] = np.maximum(combined_rect[1,:], rect[1,:])
+ combined_rect = combined_rect.tolist()
+ min_idx = min(reset_idx_dict[agent][dest])
+ max_idx = max(reset_idx_dict[agent][dest])
+ transition = (agent, node.mode[agent], dest, combined_rect, (min_idx, max_idx))
+ possible_transitions.append(transition)
+ # Return result
+ return possible_transitions
\ No newline at end of file
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
diff --git a/requirements.txt b/requirements.txt
index 2c26c89eb2c9ad080d6502168f48a6f5fbc42e7c..b273c15c14b31750e271ab9c37e02d47fde4be7d 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -10,3 +10,4 @@ astunparse~=1.6.3
treelib~=1.6.1
z3-solver~=4.8.17.0
igraph~=0.9.10
+plotly~=5.8.0
diff --git a/tests/readme.md b/tests/readme.md
index 6a464c82e5da4d25bf26caee8bbc576b80258558..f9b832e7dfc0a4e3518e11e8649d215856cb6d09 100644
--- a/tests/readme.md
+++ b/tests/readme.md
@@ -1,3 +1,7 @@
# Directory with unit tests
-Uses python unittest module
+Uses python unittest module. Starting to add some basic tests. Run as
+
+python3 testdpp.py -v
+
+Multiple files to come.
diff --git a/tests/testdpp.py b/tests/testdpp.py
index 38719c4b3ac767b85cf486b2284ed09de1fea4f0..01a3ee09ac4518876337f7a9ee90c6101d4c119c 100644
--- a/tests/testdpp.py
+++ b/tests/testdpp.py
@@ -59,8 +59,10 @@ class TestSimulatorMethods(unittest.TestCase):
def setUp(self):
self.scenario = Scenario()
self.car = CarAgent('ego', file_name='example_controller1.py')
- #scenario.add_agent(car)
- # self.scenario.add_map(SimpleMap2())
+ self.car2 = CarAgent('other', file_name='example_controller1.py')
+ self.scenario.add_agent(self.car)
+ self.scenario.add_agent(self.car2)
+ self.scenario.add_map(SimpleMap2())
# self.scenario.set_sensor(FakeSensor1())
# self.scenario.set_init(
# [[0, 3, 0, 0.5]],
@@ -75,9 +77,17 @@ class TestSimulatorMethods(unittest.TestCase):
def test_nothing(self):
self.assertEqual(5, 5)
- def test_simulator(self):
+ def test_carid(self):
self.assertEqual(self.car.id, 'ego', msg='Checking agent creation')
+ def test_carinscene(self):
+ # Ego agent was added
+ self.assertEqual(self.scenario.agent_dict['ego'], self.car, msg='Checking adding agent to scenario dict')
+
+ def test_scenedictsize(self):
+ # Check size of scene agent dictionary
+ self.assertEqual(len(self.scenario.agent_dict), 2, msg='Checking adding agent to scenario dict')
+
if __name__ == '__main__':
unittest.main()