Merge branch 'simple_map'

demo/demo1.py

@@ -69,7 +69,7 @@ if __name__ == "__main__":
 
     # plt.show()
 
-    traces = scenario.simulate(10)
+    traces = scenario.simulate(10, 0.01)
     fig = go.Figure()
     # fig = plotly_map(tmp_map, 'g', fig)
     # fig = plotly_simulation_tree(

demo/demo3.py

@@ -35,14 +35,14 @@ class State:
     v = 0.0
     vehicle_mode: VehicleMode = VehicleMode.Normal
     lane_mode: LaneMode = LaneMode.Lane0
-    type: LaneObjectMode = LaneObjectMode.Vehicle
+    type_mode: LaneObjectMode = LaneObjectMode.Vehicle
 
-    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):
         pass
 
 
 if __name__ == "__main__":
-    input_code_name = './demo/example_controller4.py'
+    input_code_name = './example_controller4.py'
     scenario = Scenario()
 
     car = CarAgent('car1', file_name=input_code_name)
@@ -63,14 +63,14 @@ if __name__ == "__main__":
             [[30, 0, 0, 0.5],[30, 0, 0, 0.5]], 
         ],
         [
-            (VehicleMode.Normal, LaneMode.Lane1),
-            (VehicleMode.Normal, LaneMode.Lane1),
-            (VehicleMode.Normal, LaneMode.Lane0),
-            (VehicleMode.Normal, LaneMode.Lane1),
+            (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)
-    # traces = scenario.verify(70)
+    traces = scenario.simulate(70, 0.05)
+    # traces = scenario.verify(70, 0.05)
 
     # fig = plt.figure(2)
     # fig = plot_map(tmp_map, 'g', fig)

demo/demo4.py

@@ -79,8 +79,8 @@ if __name__ == "__main__":
             (VehicleMode.Normal, LaneMode.Lane3),
         ]
     )
-    traces = scenario.simulate(80)
-    # traces = scenario.verify(80)
+    traces = scenario.simulate(80, 0.05)
+    # traces = scenario.verify(80, 0.05)
 
     # fig = plt.figure(2)
     # fig = plot_map(tmp_map, 'g', fig)

demo/demo5.py

@@ -29,19 +29,6 @@ class LaneMode(Enum):
     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 = './example_controller7.py'
     scenario = Scenario()
@@ -72,8 +59,8 @@ if __name__ == "__main__":
             (VehicleMode.Normal, LaneMode.Lane0),
         ]
     )
-    # traces = scenario.simulate(70)
-    traces = scenario.verify(60)
+    # traces = scenario.simulate(70, 0.05)
+    traces = scenario.verify(60, 0.05)
 
     fig = plt.figure(2)
     fig = plot_map(tmp_map, 'g', fig)

demo/demo6.py

@@ -30,17 +30,17 @@ class LaneMode(Enum):
     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
+# 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 __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode, type: LaneObjectMode):
+#         pass
 
 
 if __name__ == "__main__":
@@ -83,8 +83,8 @@ if __name__ == "__main__":
             (VehicleMode.Normal, LaneMode.Lane3),
         ]
     )
-    # traces = scenario.simulate(80)
-    traces = scenario.verify(50)
+    # traces = scenario.simulate(80, 0.05)
+    traces = scenario.verify(50, 0.05)
 
     fig = plt.figure(2)
     fig = plot_map(tmp_map, 'g', fig)

demo/demo7.py

@@ -89,8 +89,8 @@ if __name__ == "__main__":
             (VehicleMode.Normal, LaneMode.Lane3),
         ]
     )
-    traces = scenario.simulate(80)
-    # traces = scenario.verify(15)
+    traces = scenario.simulate(80, 0.05)
+    # traces = scenario.verify(15, 0.05)
 
     # fig = plt.figure(2)
     # fig = plot_map(tmp_map, 'g', fig)

demo/demo8.py

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

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

demo/example_controller4.py

@@ -27,15 +27,15 @@ class State:
     v = 0.0
     vehicle_mode: VehicleMode = VehicleMode.Normal
     lane_mode: LaneMode = LaneMode.Lane0
-    type: LaneObjectMode = LaneObjectMode.Vehicle
+    type_mode: LaneObjectMode = LaneObjectMode.Vehicle
 
-    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):
         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 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):

demo/example_controller5.py

@@ -27,9 +27,9 @@ class State:
     v = 0.0
     vehicle_mode: VehicleMode = VehicleMode.Normal
     lane_mode: LaneMode = LaneMode.Lane0
-    type: LaneObjectMode = LaneObjectMode.Vehicle
+    type_mode: LaneObjectMode = LaneObjectMode.Vehicle
 
-    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):
         pass
 
 def controller(ego:State, others:List[State], lane_map):

demo/example_controller7.py

@@ -27,9 +27,9 @@ class State:
     v = 0.0
     vehicle_mode: VehicleMode = VehicleMode.Normal
     lane_mode: LaneMode = LaneMode.Lane0
-    type: LaneObjectMode = LaneObjectMode.Vehicle
+    type_mode: LaneObjectMode = LaneObjectMode.Vehicle
 
-    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):
         pass
 
 def controller(ego:State, others:List[State], lane_map):

demo/example_controller9.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()
+    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
+

dryvr_plus_plus/example/example_agent/ball_agent.py

@@ -31,10 +31,9 @@ class BallAgent(BaseAgent):
         vy_dot = 0
         return [x_dot, y_dot, vx_dot, vy_dot]
 
-    def TC_simulate(self, mode: List[str], initialCondition, time_bound, lane_map:LaneMap=None)->np.ndarray:
-        # P1. Should TC_simulate really be part of the agent definition or should it be something more generic?
-        time_step = 0.05
-        # P2. Looks like this should be a global parameter; some config file should be setting this.
+    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)]

dryvr_plus_plus/example/example_agent/car_agent.py

@@ -37,8 +37,7 @@ 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)]
@@ -95,8 +94,7 @@ 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 = [round(i*time_step,10) for i in range(0,number_points)]

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]

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

dryvr_plus_plus/scene_verifier/analysis/simulator.py

@@ -11,7 +11,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={},
@@ -43,7 +43,7 @@ class Simulator:
                     # Simulate the trace starting from initial condition
                     mode = node.mode[agent_id]
                     init = node.init[agent_id]
-                    trace = node.agent[agent_id].TC_simulate(mode, init, remain_time,lane_map)
+                    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()
 

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()
@@ -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,

dryvr_plus_plus/scene_verifier/code_parser/pythonparser.py

@@ -249,7 +249,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()):
@@ -367,7 +367,6 @@ class ControllerAst():
     def initalwalktree(self, code, tree):
         vars = []
         discrete_vars = []
-        type_vars = []
         out = []
         mode_dict = {}
         state_object_dict = {}
@@ -390,9 +389,7 @@ 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:
+                                        if "mode" not in arg.arg:
                                             state_object_dict[node.name]['cont'].append(arg.arg)
                                             # vars.append(arg.arg)
                                         else:
@@ -431,11 +428,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, type_vars]
+                        
+        return [statementtree, vars, mode_dict, discrete_vars, state_object_dict, vars_dict]
 
 
     '''
@@ -495,7 +489,6 @@ class EmptyAst(ControllerAst):
                 'type':[]
             }
         }
-        self.type_vars = []
         self.variables = []
         self.vars_dict = []
         self.vertexStrings = ['Null,Normal']

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)