diff --git a/demo/demo3.py b/demo/demo3.py
index 807c1f4585a58bc36b56c31f1820ee93d47752ed..54121e8fd88607034c01a41e8a9634fd0182fd72 100644
--- a/demo/demo3.py
+++ b/demo/demo3.py
@@ -71,7 +71,7 @@ if __name__ == "__main__":
]
)
# traces = scenario.simulate(70)
- traces = scenario.verify(60)
+ traces = scenario.verify(70)
fig = plt.figure(2)
fig = plot_map(tmp_map, 'g', fig)
@@ -79,7 +79,7 @@ if __name__ == "__main__":
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()
+ plt.show()
# fig = go.Figure()
# fig = plotly_simulation_anime(traces, tmp_map, fig)
diff --git a/demo/demo4.py b/demo/demo4.py
index 2a9843f25ab4fb06ff8c031e3571f82b65acd670..a7b1bb854ba7a8ebd53ebb2583bd8788df3061c9 100644
--- a/demo/demo4.py
+++ b/demo/demo4.py
@@ -43,7 +43,7 @@ class State:
if __name__ == "__main__":
- input_code_name = './example_controller4.py'
+ input_code_name = './example_controller5.py'
scenario = Scenario()
car = CarAgent('car1', file_name=input_code_name)
@@ -79,28 +79,20 @@ if __name__ == "__main__":
(VehicleMode.Normal, LaneMode.Lane3),
]
)
- traces = scenario.simulate(80)
- # traces = scenario.verify(80)
+ # traces = scenario.simulate(80)
+ traces = scenario.verify(80)
- # 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)
- # for traces in res_list:
- # # generate_simulation_anime(traces, tmp_map, fig)
- # fig = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig)
- # fig = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig)
- # fig = plot_simulation_tree(traces, 'car3', 1, [2], 'r', fig)
- # fig = plot_simulation_tree(traces, 'car4', 1, [2], 'r', fig)
- # fig = plot_simulation_tree(traces, 'car5', 1, [2], 'r', fig)
- # fig = plot_simulation_tree(traces, 'car6', 1, [2], 'r', fig)
- # plt.show()
+ fig = plt.figure(2)
+ fig = plot_map(tmp_map, 'g', fig)
+ fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
+ fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
+ fig = plot_reachtube_tree(traces, 'car3', 1, [2], 'r', fig)
+ fig = plot_reachtube_tree(traces, 'car4', 1, [2], 'r', fig)
+ 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()
+ # fig = go.Figure()
+ # fig = plotly_simulation_anime(traces, tmp_map, fig)
+ # fig.show()
diff --git a/demo/demo5.py b/demo/demo5.py
index f3823c326f3a993fd72bd924b49bd6dc43e574bf..dd1323af43628def007ea2bc75929c46a9e817de 100644
--- a/demo/demo5.py
+++ b/demo/demo5.py
@@ -42,7 +42,7 @@ class State:
if __name__ == "__main__":
- input_code_name = './demo/example_controller6.py'
+ input_code_name = './example_controller7.py'
scenario = Scenario()
car = CarAgent('car1', file_name=input_code_name)
@@ -60,7 +60,7 @@ if __name__ == "__main__":
scenario.set_sensor(FakeSensor3())
scenario.set_init(
[
- [[0, -0.2, 0, 1.0],[0.01, 0.2, 0, 1.0]],
+ [[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]],
diff --git a/demo/demo6.py b/demo/demo6.py
new file mode 100644
index 0000000000000000000000000000000000000000..0d4e78211167d347bad883ce79dbd033a8d804c6
--- /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
+
+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_sensor(FakeSensor3())
+ 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)
+ traces = scenario.verify(50)
+
+ 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..ffe3e08a8ac5e236b102c688b3b2fcd235f83049
--- /dev/null
+++ b/demo/demo7.py
@@ -0,0 +1,106 @@
+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
+
+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_sensor(FakeSensor3())
+ 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)
+ # traces = scenario.verify(80)
+
+ # 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/example_controller6.py b/demo/example_controller6.py
index f395e779f5ba9431870a99171a705d48586f1aca..abb73f48d2cec3213d01ea389743989dcccac733 100644
--- a/demo/example_controller6.py
+++ b/demo/example_controller6.py
@@ -35,19 +35,13 @@ class State:
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 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 \
- all((not (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 \
- other.lane_mode==lane_map.left_lane(ego.lane_mode))) for other in others):
+ 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((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 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 \
- all((not (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 \
- other.lane_mode==lane_map.right_lane(ego.lane_mode))) for other in others):
+ 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:
diff --git a/demo/example_controller7.py b/demo/example_controller7.py
new file mode 100644
index 0000000000000000000000000000000000000000..0c30737a351192832788586ef60b5c345df29494
--- /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: 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((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/dryvr_plus_plus/example/example_agent/car_agent.py b/dryvr_plus_plus/example/example_agent/car_agent.py
index 9d1212f514f13bff40bc9f6a7263602ab4f249b3..dd9ec5d6680a17bff6d39b94935d161b02844be6 100644
--- a/dryvr_plus_plus/example/example_agent/car_agent.py
+++ b/dryvr_plus_plus/example/example_agent/car_agent.py
@@ -81,8 +81,9 @@ class CarAgent(BaseAgent):
elif vehicle_mode == "Brake":
d = -lane_map.get_lateral_distance(vehicle_lane, vehicle_pos)
a = -1
- if v<=0.02:
- a = 0
+ elif vehicle_mode == "Accel":
+ d = -lane_map.get_lateral_distance(vehicle_lane, vehicle_pos)
+ a = 1
elif vehicle_mode == 'Stop':
d = -lane_map.get_lateral_distance(vehicle_lane, vehicle_pos)
a = 0
@@ -95,7 +96,7 @@ class CarAgent(BaseAgent):
time_step = 0.05
time_bound = float(time_bound)
number_points = int(np.ceil(time_bound/time_step))
- t = [i*time_step for i in range(0,number_points)]
+ t = [round(i*time_step,10) for i in range(0,number_points)]
init = initialCondition
trace = [[0]+init]
diff --git a/dryvr_plus_plus/scene_verifier/analysis/simulator.py b/dryvr_plus_plus/scene_verifier/analysis/simulator.py
index d5e8c97f6ecadbc30c99df54229d9058c4ad5534..fab7aa29bcdd4c387281ef84e801fbf7c5b560d7 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
@@ -32,7 +33,7 @@ class Simulator:
# Perform BFS through the simulation tree to loop through all possible transitions
while simulation_queue != []:
node:AnalysisTreeNode = simulation_queue.pop(0)
- print(node.mode)
+ print(node.start_time, node.mode)
remain_time = time_horizon - node.start_time
if remain_time <= 0:
continue
@@ -54,26 +55,30 @@ class Simulator:
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:
- 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)
+ for transition_combination in all_transition_combinations:
next_node_mode = copy.deepcopy(node.mode)
- next_node_mode[transit_agent_idx] = dest_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(
trace = next_node_trace,
init = next_node_init,
diff --git a/dryvr_plus_plus/scene_verifier/analysis/verifier.py b/dryvr_plus_plus/scene_verifier/analysis/verifier.py
index d3f8aa663b59ff3fb728ed1ab64c1c20028e31d2..01fcdca10f679b43dcc510bf2c340e7f3d6e3786 100644
--- a/dryvr_plus_plus/scene_verifier/analysis/verifier.py
+++ b/dryvr_plus_plus/scene_verifier/analysis/verifier.py
@@ -35,8 +35,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
@@ -65,7 +65,8 @@ class Verifier:
# print("here")
# TODO: Check safety conditions here
-
+ if node.trace['car1'][-1][0] > 50.01:
+ print("stop here")
# Get all possible transitions to next mode
all_possible_transitions = transition_graph.get_transition_verify_new(node)
max_end_idx = 0
@@ -100,7 +101,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 d0cfb9c269203f562a4076df8555dc8f8e496f0f..56fe2f1d83063df9c6615c4de2bf946367355f1e 100644
--- a/dryvr_plus_plus/scene_verifier/automaton/guard.py
+++ b/dryvr_plus_plus/scene_verifier/automaton/guard.py
@@ -347,8 +347,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 = []
@@ -356,8 +360,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
@@ -382,8 +390,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 = []
@@ -391,8 +404,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
@@ -428,6 +445,11 @@ class GuardExpressionAst:
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)
+ 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')
return True, root
@@ -623,7 +645,10 @@ class GuardExpressionAst:
if isinstance(root.op, ast.USub):
res, root.operand = self._evaluate_guard_disc(root.operand, agent, disc_var_dict, cont_var_dict, lane_map)
elif isinstance(root.op, ast.Not):
- raise NotImplementedError
+ 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
@@ -643,7 +668,7 @@ class GuardExpressionAst:
def evaluate_guard(self, agent, continuous_variable_dict, discrete_variable_dict, lane_map):
res = True
- for node in self.ast_list:
+ 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:
@@ -698,7 +723,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("")
@@ -711,6 +737,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)
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/dryvr/common/utils.py b/dryvr_plus_plus/scene_verifier/dryvr/common/utils.py
index 1923a84ea51badf888fc64852a0d89edd45d3575..4bc395d78f90465297283dc6f5cfc5b0c67bf249 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/map/lane_map.py b/dryvr_plus_plus/scene_verifier/map/lane_map.py
index 13ddcb7ab3b59035ec0d5f561b0f811c3829fbe9..3e5b9c2d58e943269964d354a5a095e3c69ec3a3 100644
--- a/dryvr_plus_plus/scene_verifier/map/lane_map.py
+++ b/dryvr_plus_plus/scene_verifier/map/lane_map.py
@@ -39,7 +39,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):
@@ -57,7 +57,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 9fb105becc033098130239fdb834a3f5346d1eed..142b51726e5c1f7deba285c40e37b7c2b480a4ab 100644
--- a/dryvr_plus_plus/scene_verifier/scenario/scenario.py
+++ b/dryvr_plus_plus/scene_verifier/scenario/scenario.py
@@ -279,9 +279,9 @@ class Scenario:
return satisfied_guard
- def get_transition_simulate(self, node:AnalysisTreeNode) -> Tuple[List[Tuple[float]], int]:
+ def get_transition_simulate(self, node:AnalysisTreeNode) -> Tuple[Dict[str,List[Tuple[float]]], int]:
trace_length = len(list(node.trace.values())[0])
- transitions = []
+ 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
@@ -293,7 +293,10 @@ class Scenario:
possible_transitions = self.get_all_transition(all_agent_state)
if 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))
+ 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
@@ -307,7 +310,7 @@ class Scenario:
# 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) -> List[Tuple[float]]:
+ 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])
@@ -339,7 +342,7 @@ class Scenario:
else:
agent_guard_dict[agent_id].append((guard_expression, continuous_variable_updater, copy.deepcopy(discrete_variable_dict), reset_list))
- transitions = []
+ transitions = {}
for idx in range(trace_length):
satisfied_guard = []
for agent_id in agent_guard_dict:
@@ -374,7 +377,10 @@ class Scenario:
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:
@@ -399,7 +405,10 @@ class Scenario:
satisfied_guard.append(next_transition)
if satisfied_guard != []:
for agent_idx, src_mode, dest_mode, next_init in satisfied_guard:
- transitions.append((agent_idx, src_mode, dest_mode, next_init, idx))
+ 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
@@ -416,7 +425,7 @@ class Scenario:
for tmp in node.agent:
state_dict[tmp] = (node.trace[tmp][0*2:0*2+2], node.mode[tmp])
- continuous_variable_dict, discrete_variable_dict, length_dict = self.sensor.sense(self, agent, state_dict, self.map)
+ 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
@@ -429,9 +438,9 @@ class Scenario:
reset_list.append(item)
guard_expression = GuardExpressionAst(guard_list)
- cont_var_updater = guard_expression.parse_any_all_new(continuous_variable_dict, discrete_variable_dict, length_dict)
-
- guard_can_satisfied = guard_expression.evaluate_guard_disc(agent, discrete_variable_dict, continuous_variable_dict, self.map)
+ 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: