Merge pull request #6 from braught2/simple_map

Simple map

.gitignore

@@ -7,3 +7,4 @@ venv/
 **.egg-info/
 .VSCodeCounter/
 dist/dryvr_plus_plus-0.1-py3.8.egg
+tmp/
\ No newline at end of file

demo/ball_bounces.py

+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__":
+    ball_controller = './ball_bounces.py'
+    bouncingBall = Scenario()
+    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()
+

demo/demo1.py

@@ -4,7 +4,6 @@ from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap2, SimpleMa
 from dryvr_plus_plus.plotter.plotter2D import *
 from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor2
 import plotly.graph_objects as go
-import matplotlib.pyplot as plt
 import numpy as np
 from enum import Enum, auto
 

demo/demo2.py

@@ -4,7 +4,6 @@ from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap2, SimpleMa
 from dryvr_plus_plus.plotter.plotter2D import *
 from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor2
 import plotly.graph_objects as go
-import matplotlib.pyplot as plt
 import numpy as np
 from enum import Enum, auto
 
@@ -55,22 +54,17 @@ if __name__ == "__main__":
             (VehicleMode.Normal, LaneMode.Lane1),
         ]
     )
-    # # res_list = scenario.simulate_multi(40,1)
-    # traces = scenario.verify(40)
+    # res_list = scenario.simulate(40)
+    traces = scenario.verify(40)
 
-    # 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)
-    # # for traces in res_list:
-    # #     generate_simulation_anime(traces, tmp_map, fig)
-    # #     # fig,x_lim,y_lim = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig,x_lim,y_lim)
-    # #     # fig,x_lim,y_lim = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig,x_lim,y_lim)
+    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)
+    plt.show()
 
-    # plt.show()
-
-# this is for plot-based visualization
-    traces = scenario.simulate(40)
-    fig = go.Figure()
-    fig = plotly_simulation_anime(traces, tmp_map, fig)
-    fig.show()
+    # # this is for plot-based visualization
+    # traces = scenario.simulate(40)
+    # fig = go.Figure()
+    # fig = plotly_simulation_anime(traces, tmp_map, fig)
+    # fig.show()

demo/demo3.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_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),
+            (VehicleMode.Normal, LaneMode.Lane1),
+            (VehicleMode.Normal, LaneMode.Lane0),
+            (VehicleMode.Normal, LaneMode.Lane1),
+        ]
+    )
+    traces = scenario.simulate(70)
+    # traces = scenario.verify(70)
+
+    # 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/demo4.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, 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)
+    # 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()    
+    

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
+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()
+
+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 = 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)
+    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)
+    plt.show()
+        
+
+    # fig = go.Figure()
+    # fig = plotly_simulation_anime(traces, tmp_map, fig)
+    # fig.show()
+

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

demo/demo7.py

+# 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)
+    # traces = scenario.verify(15)
+
+    # 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()    
+    

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

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

demo/example_controller7.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 \
+             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
+

demo/example_controller8.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: 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
+

dryvr_plus_plus/example/example_agent/ball_agent.py

+# 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, 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.
+        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

dryvr_plus_plus/example/example_agent/car_agent.py

+# Example agent.
 from typing import Tuple, List
 
 import numpy as np 
@@ -23,6 +24,8 @@ class NPCAgent(BaseAgent):
         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]
@@ -81,8 +84,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 +99,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]

dryvr_plus_plus/example/example_map/simple_map2.py

@@ -22,21 +22,21 @@ class SimpleMap3(LaneMap):
         segment0 = StraightLane(
             'Seg0',
             [0,3],
-            [50,3],
+            [100,3],
             3
         )
         lane0 = Lane('Lane0', [segment0])
         segment1 = StraightLane(
             'seg0',
             [0,0],
-            [50,0],
+            [100,0],
             3
         )
         lane1 = Lane('Lane1', [segment1])
         segment2 = StraightLane(
             'seg0',
             [0,-3],
-            [50,-3],
+            [100,-3],
             3
         )
         lane2 = Lane('Lane2', [segment2])
@@ -48,6 +48,57 @@ class SimpleMap3(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):
         super().__init__()

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

dryvr_plus_plus/plotter/plotter2D.py

@@ -76,8 +76,32 @@ def plot_reachtube_tree(root, agent_id, x_dim: int = 0, y_dim_list: List[int] =
 
     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()
 
-def plot_map(map, color='b', fig=None, x_lim=None, y_lim=None):
+    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
+
+def plot_map(map, color = 'b', fig = None, x_lim = None,y_lim = None):
     if fig is None:
         fig = plt.figure()
 
@@ -277,7 +301,7 @@ def plotly_simulation_tree(root, agent_id, x_dim: int = 0, y_dim_list: List[int]
     while queue != []:
         node = queue.pop(0)
         traces = node.trace
-        print(node.mode)
+        # print(node.mode)
         # [[time,x,y,theta,v]...]
         trace = np.array(traces[agent_id])
         # print(trace)
@@ -475,13 +499,14 @@ def plotly_simulation_anime(root, map=None, fig=None):
     fig_dict["layout"]["sliders"] = [sliders_dict]
 
     fig = go.Figure(fig_dict)
-    fig = plotly_map(map, 'g', fig)
+    if map is not None:
+        fig = plotly_map(map, 'g', fig)
     i = 0
     queue = [root]
     while queue != []:
         node = queue.pop(0)
         traces = node.trace
-        print(node.mode)
+        # print(node.mode)
         # [[time,x,y,theta,v]...]
         for agent_id in traces:
             trace = np.array(traces[agent_id])
@@ -490,7 +515,7 @@ def plotly_simulation_anime(root, map=None, fig=None):
             trace_x = trace[:, 1].tolist()
             # theta = [i/pi*180 for i in trace[:, 3]]
             color = 'green'
-            if agent_id == 'car2':
+            if agent_id == 'car1':
                 color = 'red'
             fig.add_trace(go.Scatter(x=trace[:, 1], y=trace[:, 2],
                                      mode='lines',

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