working on the curved lanes

common_road_vis.py

+import matplotlib.pyplot as plt 
+import xml.etree.ElementTree as ET
+
+if __name__ == "__main__":
+    fn = 'ARG_Carcarana-11_1_T-1.xml'
+    tree = ET.parse(fn)
+    root = tree.getroot()
+
+    lanelet_list = root.findall('lanelet')
+    for j in range(len(lanelet_list)):
+        example_lanelet = lanelet_list[j]
+        left_bound = example_lanelet.find('leftBound')
+        right_bound = example_lanelet.find('rightBound')
+        leftpoint_list = left_bound.findall('point')
+        rightpoint_list = right_bound.findall('point')
+        assert(len(leftpoint_list)==len(rightpoint_list))
+        left_x = []
+        left_y = []
+        right_x = []
+        right_y = []
+        middle_x = []
+        middle_y = []
+        for i in range(len(leftpoint_list)):
+            leftpoint = leftpoint_list[i]
+            left_x.append(float(leftpoint.find('x').text))
+            left_y.append(float(leftpoint.find('y').text))
+
+            rightpoint = rightpoint_list[i]
+            right_x.append(float(rightpoint.find('x').text))
+            right_y.append(float(rightpoint.find('y').text))
+        
+            middle_x.append((float(leftpoint.find('x').text)+float(rightpoint.find('x').text))/2)
+            middle_y.append((float(leftpoint.find('y').text)+float(rightpoint.find('y').text))/2)
+
+        # plt.plot(left_x, left_y,'-*')
+        # plt.plot(right_x, right_y,'-*')
+        plt.plot(middle_x, middle_y, '-*')
+
+    plt.show()
\ No newline at end of file

example_two_car_lane_switch.py

@@ -31,6 +31,10 @@ class State:
 def controller(ego: State, other: State, lane_map):
     output = ego
     if ego.vehicle_mode == VehicleMode.Normal:
+        # A simple example to demonstrate how our tool can handle change in controller
+        # if ego.x > 30 and ego.lane_mode == LaneMode.Lane0:
+        #     output.vehicle_mode = VehicleMode.SwitchRight
+        
         if other.x - ego.x > 3 and other.x - ego.x < 5 and ego.lane_mode == other.lane_mode:
             if lane_map.has_left(ego.lane_mode):
                 output.vehicle_mode = VehicleMode.SwitchLeft
@@ -69,7 +73,7 @@ if __name__ == "__main__":
     scenario.set_init(
         [
             [[10, 0, 0, 0.5],[10, 0, 0, 0.5]], 
-            [[-0.5, -0.2, 0, 1.0],[0.5, 0.2, 0, 1.0]],
+            [[-0.2, -0.2, 0, 1.0],[0.2, 0.2, 0, 1.0]],
         ],
         [
             (VehicleMode.Normal, LaneMode.Lane1),
@@ -81,7 +85,7 @@ if __name__ == "__main__":
     traces = scenario.verify(40)
 
     fig = plt.figure()
-    # fig = plot_tree(traces, 'car1', 1, [2], 'b', fig)
+    fig = plot_tree(traces, 'car1', 1, [2], 'b', fig)
     fig = plot_tree(traces, 'car2', 1, [2], 'r', fig)
 
     plt.show()

ourtool/analysis/analysis_tree_node.py

@@ -15,11 +15,12 @@ class AnalysisTreeNode:
         mode={},
         agent={},
         child=[],
-        start_time = 0
+        start_time = 0,
+        ndigits = 10
     ):
         self.trace:Dict = trace
         self.init:Dict = init
         self.mode:Dict = mode
         self.agent:Dict = agent
         self.child:List[AnalysisTreeNode] = child
-        self.start_time:float = start_time
+        self.start_time:float = round(start_time,ndigits)

ourtool/scenario/scenario.py

@@ -146,7 +146,7 @@ class Scenario:
     def check_guard_hit(self, state_dict):
         lane_map = self.map 
         guard_hits = []
-        is_contained = False        # TODO: Handle this
+        any_contained = False        # TODO: Handle this
         for agent_id in state_dict:
             agent:BaseAgent = self.agent_dict[agent_id]
             agent_state, agent_mode = state_dict[agent_id]
@@ -176,9 +176,10 @@ class Scenario:
                 if not guard_can_satisfied:
                     continue
                 guard_satisfied, is_contained = guard_expression.evaluate_guard_cont(agent, continuous_variable_dict, self.map)
+                any_contained = any_contained or is_contained
                 if guard_satisfied:
                     guard_hits.append((agent_id, guard_list, reset_list))
-        return guard_hits, is_contained
+        return guard_hits, any_contained
 
     def get_all_transition_set(self, node):
         possible_transitions = []