testing various examples

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)

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)

demo/demo5.py

+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()
+

demo/example_controller5.py

+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
+

demo/example_controller6.py

+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
+

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):