diff --git a/demo/demo3.py b/demo/demo3.py
index dd1bb151fb050fcd5528fef802730fe90a4d873d..079c68d6073d22145e15308458d965e548df0800 100644
--- a/demo/demo3.py
+++ b/demo/demo3.py
@@ -42,7 +42,7 @@ class State:
if __name__ == "__main__":
- input_code_name = './example_controller4.py'
+ input_code_name = './demo/example_controller6.py'
scenario = Scenario()
car = CarAgent('car1', file_name=input_code_name)
diff --git a/demo/demo4.py b/demo/demo4.py
index 2a9843f25ab4fb06ff8c031e3571f82b65acd670..04c7c5413b43afe902e19d8bc6210a55d60f784e 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)
diff --git a/demo/demo5.py b/demo/demo5.py
new file mode 100644
index 0000000000000000000000000000000000000000..f3823c326f3a993fd72bd924b49bd6dc43e574bf
--- /dev/null
+++ b/demo/demo5.py
@@ -0,0 +1,96 @@
+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: 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 = './demo/example_controller6.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_sensor(FakeSensor3())
+ 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]],
+ [[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)
+ # traces = scenario.verify(60)
+
+ # fig = plt.figure(2)
+ # fig = plot_map(tmp_map, 'g', fig)
+ # fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
+ # fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
+ # fig = plot_reachtube_tree(traces, 'car3', 1, [2], 'r', fig)
+ # fig = plot_reachtube_tree(traces, 'car4', 1, [2], 'r', fig)
+ # for traces in res_list:
+ # # generate_simulation_anime(traces, tmp_map, fig)
+ # fig = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig)
+ # fig = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig)
+ # fig = plot_simulation_tree(traces, 'car3', 1, [2], 'r', fig)
+ # fig = plot_simulation_tree(traces, 'car4', 1, [2], 'r', fig)
+ # plt.show()
+
+
+ fig = go.Figure()
+ fig = plotly_simulation_anime(traces, tmp_map, fig)
+ fig.show()
+
diff --git a/demo/example_controller5.py b/demo/example_controller5.py
new file mode 100644
index 0000000000000000000000000000000000000000..4294de0e7c0ef59c338a58d637b67898d39055c5
--- /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: 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):
+ 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..f395e779f5ba9431870a99171a705d48586f1aca
--- /dev/null
+++ b/demo/example_controller6.py
@@ -0,0 +1,62 @@
+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 \
+ 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):
+ 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 \
+ 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):
+ 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/dryvr_plus_plus/scene_verifier/automaton/guard.py b/dryvr_plus_plus/scene_verifier/automaton/guard.py
index 0370c8371ac540a4ca3e785c6d38874adb6e42d1..ebaad7760bb48281800f148ce6c849eff70ec583 100644
--- a/dryvr_plus_plus/scene_verifier/automaton/guard.py
+++ b/dryvr_plus_plus/scene_verifier/automaton/guard.py
@@ -622,6 +622,8 @@ class GuardExpressionAst:
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)
+ elif isinstance(root.op, ast.Not):
+ raise NotImplementedError
else:
raise ValueError(f'Node type {root} from {astunparse.unparse(root)} is not supported')
return True, root
@@ -731,6 +733,8 @@ class GuardExpressionAst:
val = self._evaluate_guard(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')
elif isinstance(root, ast.Name):