version for programming exercise

GSMP/motion_automata/automata/HelperFunctions.py

+import sys
+sys.path.append("../../GSMP/tools/")
+sys.path.append("../../GSMP/tools/commonroad-collision-checker")
+sys.path.append("../../GSMP/tools/commonroad-road-boundary")
+sys.path.append("../../GSMP/motion_automata/vehicle_model")
+
+import os
+import time
+from multiprocessing import Manager, Process
+
+import numpy as np
+from math import atan2
+
+import matplotlib.pyplot as plt
+from IPython import display
+from ipywidgets import widgets
+
+# import CommonRoad-io modules
+from commonroad.visualization.draw_dispatch_cr import draw_object
+from commonroad.common.file_reader import CommonRoadFileReader
+from commonroad.scenario.trajectory import Trajectory, State
+from commonroad.geometry.shape import Rectangle
+from commonroad.prediction.prediction import TrajectoryPrediction
+from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_object
+from commonroad_cc.visualization.draw_dispatch import draw_object as draw_it
+from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleType, CostFunction
+
+# import Motion Automata modules
+from automata.MotionAutomata import MotionAutomata
+from automata.MotionPrimitive import MotionPrimitive
+from automata.States import FinalState
+
+# ================================== scenario ================================== #
+def load_scenario(scenario_path):
+    # open and read in scenario and planning problem set
+    scenario, planning_problem_set = CommonRoadFileReader(scenario_path).open()
+
+    return scenario, planning_problem_set
+
+
+# ================================== automata ================================== #
+def generate_automata(veh_type_id: int):
+    """ 
+    1. We first create a Motion Automata object, then load pre-computed motion primitives into it. 
+    Note that every motion primitive is a short trajectory with varied initial states that lead to different final states.
+
+    2. We create a connectivity list for every pritimive in the Motion Automata, 
+    which contains all connectable primitives from the focused primitive. 
+    If the velocity and steering angel of the start state of primitive B match 
+    those of the final state of primitive A, we say B is connectable from A.
+    """
+
+    # the primitives vary for different vehicle models 1, 2 and 3.
+    assert veh_type_id in (1,2,3), "Input vehicle type id is not valid! Must be either 1, 2 or 3."
+
+    # step 1
+    automata = MotionAutomata(veh_type_id)
+
+    prefix = '../../GSMP/motion_automata/motion_primitives/'
+    print("Reading motion primitives...")
+
+    if veh_type_id == 1:
+        automata.readFromXML(prefix + 'V_0.0_20.0_Vstep_1.0_SA_-0.91_0.91_SAstep_0.23_T_0.5_Model_FORD_ESCORT.xml')
+    elif veh_type_id == 2:
+        automata.readFromXML(prefix + 'V_0.0_20.0_Vstep_1.0_SA_-1.066_1.066_SAstep_0.27_T_0.5_Model_BMW320i.xml')
+    elif veh_type_id == 3:
+        automata.readFromXML(prefix + 'V_0.0_20.0_Vstep_1.0_SA_-1.023_1.023_SAstep_0.26_T_0.5_Model_VW_VANAGON.xml')
+
+    # step 2
+    automata.createConnectivityLists()
+
+    # assign vehicle type id to all primitives
+    automata.setVehicleTypeIdPrimitives()
+
+    print("Automata created.")
+    print('Number of loaded primitives: ' + str(len(automata.Primitives)))
+
+    return automata
+
+def add_initial_state_to_automata(automata, planning_problem, flag_print_states = True):
+    # set initial steering angle to zero
+	planning_problem.initial_state.steering_angle = 0.0
+
+    # the initial velocity of the planning problem may be any value, we need to obtain the closest velocity to it
+    # from StartStates of the primitives in order to get the feasible successors of the planning problem
+	velocity_closest = automata.getClosestStartVelocity(planning_problem.initial_state.velocity)
+	planning_problem.initial_state.velocity = velocity_closest
+
+	initial_state = planning_problem.initial_state
+	goal_region = planning_problem.goal.state_list[0]
+
+    # if flag_print_states:
+    #     print("Initial State:", initial_state)
+    #     print("Goal Region:", goal_region)
+
+	# turn intial state into a motion primitive to check for connectivity to subsequent motion primitives
+	final_state_primitive = FinalState(x=initial_state.position[0],
+                                       y=initial_state.position[1], 
+                                       steering_angle=initial_state.steering_angle,
+    								   velocity=initial_state.velocity, 
+                                       orientation=initial_state.orientation, 
+                                       time_step=initial_state.time_step)
+
+	initial_motion_primitive = MotionPrimitive(startState=None, finalState=final_state_primitive, timeStepSize=0, trajectory=None)
+
+	# create connectivity list for this imaginary motion primitive
+	automata.createConnectivityListPrimitive(initial_motion_primitive)
+
+	return automata, initial_motion_primitive
+
+# ================================== search for solution ================================== #
+"""
+1. Searching for a possible solution could be time consuming, and also we would like to visualize the intermediate states during the search.
+Thus, we create a separate Process to handle the search, and visualize the intermediate states via Main Process. 
+More information on python Prcess could be found at [1](https://docs.python.org/3/library/multiprocessing.html) and
+[2](http://www.blog.pythonlibrary.org/2016/08/02/python-201-a-multiprocessing-tutorial/). 
+
+2. We need multiprocessing.Manager.Value() to help us share variables between different Processes.
+
+3. To execute the search, we need to define an individual wrapper function and set it as the target of the new Process.
+"""
+
+def execute_search(motion_planner, initial_motion_primitive, path_shared, dict_status_shared, max_tree_depth):
+    """
+    definition of the wrapper function for search
+    calls search algorithm and update the status dictionary for visualization in search algorithm
+    note that dict_status_shared is being shared between difference Processes
+    """
+    result = motion_planner.search_alg(initial_motion_primitive.Successors, max_tree_depth, dict_status_shared)
+
+    # result is in form of (final path, used_primitives)
+    if result is None:
+        # no path found
+        return
+    else:
+        display.clear_output(wait=True)
+        path_shared.value = result[0]
+    
+    if len(result[1]) > 0:
+        print("Found primitives")
+        
+    # for primitive in result[1]:
+    #     print('\t', primitive)
+
+def start_search(scenario, planning_problem, automata, motion_planner, initial_motion_primitive, flag_plot_intermediate_results=True, flag_plot_planning_problem=True):
+    # create Manager object to manage shared variables between different Processes
+    process_manager = Manager()
+
+    # create shared variables between Processes
+    path_shared        = process_manager.Value('path_shared', None)
+    dict_status_shared = process_manager.Value('dict_status_shared', None)
+
+    # initialize status with a dictionary.
+    # IMPORTANT NOTE: if the shared variable contains complex object types (list, dict, ...), to change its value,
+    # we need to do it via another object (here dict_status) and pass it to the shared variable at the end
+    dict_status = {'cost_current': 0, 'path_current':[]}
+    dict_status_shared.value = dict_status
+
+    # maximum number of concatenated primitives
+    max_tree_depth = 100 
+
+    # create and start the Process for search
+    process_search = Process(target=execute_search, 
+                             args=(motion_planner, 
+                                   initial_motion_primitive, 
+                                   path_shared, 
+                                   dict_status_shared, 
+                                   max_tree_depth))
+    process_search.start()
+
+    if flag_plot_intermediate_results:
+        # create a figure, plot the scenario and planning problem
+        fig = plt.figure(figsize=(9, 9))   
+        plt.clf()
+        draw_object(scenario)
+        if flag_plot_planning_problem: 
+            draw_object(planning_problem)
+        plt.gca().set_aspect('equal')
+        plt.gca().set_axis_off()
+        plt.margins(0,0)
+        plt.show()
+
+    refresh_rate_plot = 5.0
+
+    while True:
+        time.sleep(1 / refresh_rate_plot)
+        # break the loop if process_search is not alive anymore
+        if not process_search.is_alive():
+            break
+        
+        # print current status
+        string_status = "Cost so far:", round(dict_status_shared.value['cost_current'], 2)
+        string_status += "Time step:", round(dict_status_shared.value['path_current'][-1].time_step, 2)
+        string_status += "Orientation:",round(dict_status_shared.value['path_current'][-1].orientation, 2)
+        string_status += "Velocity:", round(dict_status_shared.value['path_current'][-1].velocity, 2)
+        print (string_status, end='\r')
+        
+        # if there is a path to be visualized
+        if len(dict_status_shared.value['path_current']) > 0:
+            if flag_plot_intermediate_results:
+                plt.clf()
+                draw_object(scenario)
+                if flag_plot_planning_problem:
+                    draw_object(planning_problem)
+                plt.gca().set_aspect('equal')
+                plt.gca().set_axis_off()
+                plt.margins(0,0)
+                plt.show()
+            
+            # create a Trajectory from the current states of the path
+            # Trajectory is essentially a list of states
+            trajectory = Trajectory(initial_time_step=0, state_list=dict_status_shared.value['path_current'])
+            
+            # create an occupancy Prediction via the generated trajectory and shape of ego vehicle
+            # the Prediction includes a Trajectory and Shape of the object
+            prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape)
+            
+            # create a colission object from prediction
+            # here it is used for visualization purpose
+            collision_object = create_collision_object(prediction)
+            
+            # draw this collision object
+            draw_it(collision_object, draw_params={'collision': {'facecolor': 'magenta'}})
+
+        # visualize current trajectory
+        fig.canvas.draw()    
+
+    # wait till process_search terminates
+    process_search.join()
+    time.sleep(0.5)
+    print("Search finished.")
+
+    if path_shared.value is not None and len(path_shared.value) > 1:
+        print("Solution successfully found :)")
+    else:
+        print("Finding solution failed :(")
+
+    return path_shared.value, dict_status_shared.value
+
+# ================================== visualization ================================== #
+def get_state_at_time(t):
+    for state in path_shared.value:
+        # return the first state that has time_step >= given t
+        if state.time_step >= t:
+            return state
+    # else return last state
+    return path[-1]
+
+def draw_state(t):
+    print("current time step: ", t)
+    draw_figure()
+
+    if path_shared is not None:
+        state = get_state_at_time(t)
+        trajectory = Trajectory(initial_time_step=int(state.time_step),state_list=[state])
+        prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape)
+        collision_object = create_collision_object(prediction)
+        draw_it(collision_object)
\ No newline at end of file

motionAutomata/Automata/MotionAutomata.py → GSMP/motion_automata/automata/MotionAutomata.py

+import copy
 import numpy as np
 import xml.etree.ElementTree as et
-from sphinx.util import xmlname_checker
-
-from Automata.MotionPrimitiveParser import MotionPrimitiveParser
-from Automata.MotionPrimitive import MotionPrimitive
+from automata.MotionPrimitiveParser import MotionPrimitiveParser
+from automata.MotionPrimitive import MotionPrimitive
+from commonroad.geometry.shape import Rectangle
 
 class MotionAutomata:
-    def __init__(self, state_tuple, dt):
-        self.state_tuple = state_tuple
+    """
+    Class to handle motion primitives for motion planning
+    """
+    def __init__(self, veh_type_id):
         self.numPrimitives = 0
-        self.Primitives = []#np.empty(0, dtype=MotionPrimitive)
-        #self.dt = dt
-        return
+        self.Primitives = []
+        self.veh_type_id = veh_type_id
+
+        self.egoShape = None
+        if veh_type_id == 1:
+            self.egoShape = Rectangle(length=4.298, width=1.674)
+        elif veh_type_id == 2:
+            self.egoShape = Rectangle(length=4.508, width=1.610)
+        elif veh_type_id == 3:
+            self.egoShape = Rectangle(length=4.569, width=1.844)
 
     def readFromXML(self, filename: str) -> None:
         """
@@ -19,45 +28,73 @@ class MotionAutomata:
 
         :param filename: the name of the file to be read from
         """
+        # parse XML file
         xmlTree = et.parse(filename).getroot()
+
+        # add trajectories
         self.numPrimitives = self.numPrimitives + len(xmlTree.findall('Trajectory'))
-        #self.Primitives = np.empty(self.numPrimitives, dtype=MotionPrimitive)
-        #i = 0
         for t in xmlTree.findall('Trajectory'):
-            self.Primitives.append(MotionPrimitiveParser.createFromNode(t)) # self.dt
-            #self.Primitives[i].id = i
-            #i = i + 1
+            self.Primitives.append(MotionPrimitiveParser.createFromNode(t))
+
+        self.setVehicleTypeIdPrimitives()
         return
 
-    def createConnectivityListPrimitive(self, primitive: MotionPrimitive)->None:
+    def createConnectivityListPrimitive(self, primitive: MotionPrimitive) -> None:
         """
         Creates the successor list for a single primitive and stores them in a successor list of the given primitive.
 
         :param primitive: the primitive to be connected
         """
-        for i in range(self.numPrimitives):
-            if primitive.checkConnectivityToNext(self.Primitives[i]):
-                primitive.Successors.append(self.Primitives[i])
+        for self_primitive in self.Primitives:
+            if primitive.checkConnectivityToNext(self_primitive):
+                primitive.Successors.append(self_primitive)
 
-    def createConnectivityLists(self)->None:
+    def createConnectivityLists(self) -> None:
         """
         Creates a connectivity list for every primitive (let every primitive has its corresponding successor list).
         """
-        for t in self.Primitives:
-            self.createConnectivityListPrimitive(t)
+        for self_primitive in self.Primitives:
+            self.createConnectivityListPrimitive(self_primitive)
         return
 
-    def createMirroredPrimitives(self)->None:
+    def createMirroredPrimitives(self) -> None:
         """
         Creates the mirrored motion primitives since the file to load primitives by default only contains left curves. This function computes the right curves.
         """
-        import copy
         oldPrimitives = self.Primitives
         self.numPrimitives = 2 * self.numPrimitives
         self.Primitives = np.empty(self.numPrimitives, dtype=MotionPrimitive)
+
         for i in range(len(oldPrimitives)):
             self.Primitives[i] = oldPrimitives[i]
-            newPrim = copy.deepcopy(self.Primitives[i])
-            newPrim.mirror() 
-            self.Primitives[i + len(oldPrimitives)] = newPrim
+            # create mirrored primitives for the old primitives
+            newPrimitive = copy.deepcopy(self.Primitives[i])
+            newPrimitive.mirror()
+            # add the new mirrored primitive to self primitive list
+            self.Primitives[i + len(oldPrimitives)] = newPrimitive
         return
+
+    def getClosestStartVelocity(self, initial_velocity):
+        """
+        get the velocity among start states that is closest to the given initial velocity
+        """
+        min_error = float('inf')
+        min_idx = 0
+
+        for i in range(len(self.Primitives)):
+            primitive = self.Primitives[i]
+
+            error_velocity = abs(initial_velocity - primitive.startState.velocity)
+            if error_velocity < min_error:
+                min_error = error_velocity
+                min_idx = i
+
+        return self.Primitives[min_idx].startState.velocity
+
+    def setVehicleTypeIdPrimitives(self):
+        """
+        assign vehicle type id to all primitives
+        """
+        for primitive in self.Primitives:
+            primitive.veh_type_id = self.veh_type_id
+

motionAutomata/Automata/MotionPlanner.py → GSMP/motion_automata/automata/MotionPlanner.py

 from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_checker, create_collision_object
-from commonroad.planning.planning_problem import PlanningProblemSet, PlanningProblem
 from commonroad.scenario.obstacle import StaticObstacle, ObstacleType, Obstacle
 from commonroad.scenario.trajectory import State as StateTupleFactory
 from commonroad.scenario.trajectory import State 
 from commonroad.prediction.prediction import TrajectoryPrediction
-from commonroad.visualization.draw_dispatch_cr import draw_object
 from commonroad.geometry.shape import Polygon, ShapeGroup, Circle
-from commonroad.common.util import Interval, AngleInterval
-from commonroad.scenario.lanelet import LaneletNetwork, Lanelet
+from commonroad.common.util import Interval
+from commonroad.scenario.lanelet import Lanelet
 from commonroad.scenario.trajectory import Trajectory
-from commonroad.planning.goal import GoalRegion
-from commonroad.geometry.shape import Rectangle
-from Automata.States import StartState, FinalState
-from Automata.MotionAutomata import MotionAutomata, MotionPrimitive
-import time
+from automata.MotionAutomata import MotionPrimitive
 import numpy as np
 import math
 import construction
 import heapq
 from typing import *
 
+
 class MotionPlanner:
-    def __init__(self, scenario, planningProblem, automata, state_tuple, shapeEgoVehicle):
+    def __init__(self, scenario, planningProblem, automata):
+        # store input parameters
         self.scenario = scenario
         self.planningProblem = planningProblem
         self.automata = automata
-        self.state_tuple = state_tuple
-        self.frontier = PriorityQueue()
+        self.egoShape = automata.egoShape
+
+        # create necessary attributes
+        self.lanelet_network = self.scenario.lanelet_network
+        self.priority_queue = PriorityQueue()
         self.obstacles = self.scenario.obstacles
         self.initial_state = self.planningProblem.initial_state
-        self.egoShape = shapeEgoVehicle
+
+        # remove unneeded attributes of initial state
         if hasattr(self.initial_state, 'yaw_rate'):
-            del(self.initial_state.yaw_rate)
+            del self.initial_state.yaw_rate
+
         if hasattr(self.initial_state, 'slip_angle'):
-            del(self.initial_state.slip_angle)
-        self.startLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position([self.planningProblem.initial_state.position])[0]
-        if  hasattr(self.planningProblem.goal.state_list[0].position, 'center'):
-            self.goalLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position([self.planningProblem.goal.state_list[0].position.center])[0]
-        elif hasattr(planningProblem.goal.state_list[0].position, 'shapes'):
-            self.goalLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position([self.planningProblem.goal.state_list[0].position.shapes[0].center])[0]
-            self.planningProblem.goal.state_list[0].position.center = self.planningProblem.goal.state_list[0].position.shapes[0].center
-        self.initial_distance = distance(planningProblem.initial_state.position, planningProblem.goal.state_list[0].position.center)
-        self.lanelet_network = self.scenario.lanelet_network
-        self.lanelet_cost = {}
-        for lanelet_id in self.lanelet_network._lanelets.keys():
-            self.lanelet_cost[lanelet_id] = -1
-        for goal_lanelet in self.goalLanelet_ids:
-            self.lanelet_cost[goal_lanelet] = 0
-        for goal_lanelet in self.goalLanelet_ids:
-            visited_lanelets = []
-            self.calc_lanelet_cost(self.scenario.lanelet_network.find_lanelet_by_id(goal_lanelet), 1, visited_lanelets)
+            del self.initial_state.slip_angle
 
+        # get lanelet id of the starting lanelet (of initial state)
+        self.startLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position(
+            [self.planningProblem.initial_state.position])[0]
 
-        build = ['section_triangles', 'triangulation']
-        boundary = construction.construct(self.scenario, build, [], [])
-        road_boundary_shape_list = list()
-        for r in boundary['triangulation'].unpack():
-            initial_state = StateTupleFactory(position=np.array([0, 0]), orientation=0.0, time_step=0)
-            p = Polygon(np.array(r.vertices()))
-            road_boundary_shape_list.append(p)
-        road_bound = StaticObstacle(obstacle_id=scenario.generate_object_id(), obstacle_type=ObstacleType.ROAD_BOUNDARY, obstacle_shape=ShapeGroup(road_boundary_shape_list), initial_state=initial_state)
-        self.collisionChecker = create_collision_checker(self.scenario)
-        self.collisionChecker.add_collision_object(create_collision_object(road_bound))
+        # get lanelet id of the ending lanelet (of goal state),this depends on type of goal state
+        if hasattr(self.planningProblem.goal.state_list[0].position, 'center'):
+            self.goalLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position(
+                [self.planningProblem.goal.state_list[0].position.center])[0]
 
+        elif hasattr(planningProblem.goal.state_list[0].position, 'shapes'):
+            self.goalLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position(
+                [self.planningProblem.goal.state_list[0].position.shapes[0].center])[0]
+            self.planningProblem.goal.state_list[0].position.center = \
+                self.planningProblem.goal.state_list[0].position.shapes[0].center
+
+        # set specifications from given goal state
         if hasattr(self.planningProblem.goal.state_list[0], 'time_step'):
             self.desired_time = self.planningProblem.goal.state_list[0].time_step
         else:
             self.desired_time = Interval(0, np.inf)
+
         if hasattr(self.planningProblem.goal.state_list[0], 'velocity'):
             self.desired_velocity = self.planningProblem.goal.state_list[0].velocity
         else:
             self.desired_velocity = Interval(0, np.inf)
+
         if hasattr(self.planningProblem.goal.state_list[0], 'orientation'):
             self.desired_orientation = self.planningProblem.goal.state_list[0].orientation
         else:
             self.desired_orientation = Interval(-math.pi, math.pi)
 
+        # create necessary attributes
+        self.initial_distance = distance(planningProblem.initial_state.position,
+                                         planningProblem.goal.state_list[0].position.center)
+
+        # set lanelet costs to -1, except goal lanelet
+        self.lanelet_cost = {}
+        for lanelet in scenario.lanelet_network.lanelets:
+            self.lanelet_cost[lanelet.lanelet_id] = -1
+
+        for goal_lanelet_id in self.goalLanelet_ids:
+            self.lanelet_cost[goal_lanelet_id] = 0
+
+        # calculate costs for lanelets, this is a recursive method
+        for goal_lanelet_id in self.goalLanelet_ids:
+            visited_lanelets = []
+            self.calc_lanelet_cost(self.scenario.lanelet_network.find_lanelet_by_id(goal_lanelet_id), 1, visited_lanelets)
+
+        # construct commonroad boundaries and collision checker object
+        build = ['section_triangles', 'triangulation']
+        boundary = construction.construct(self.scenario, build, [], [])
+        road_boundary_shape_list = list()
+        initial_state = None
+        for r in boundary['triangulation'].unpack():
+            initial_state = StateTupleFactory(position=np.array([0, 0]), orientation=0.0, time_step=0)
+            p = Polygon(np.array(r.vertices()))
+            road_boundary_shape_list.append(p)
+        road_bound = StaticObstacle(obstacle_id=scenario.generate_object_id(), obstacle_type=ObstacleType.ROAD_BOUNDARY,
+                                    obstacle_shape=ShapeGroup(road_boundary_shape_list), initial_state=initial_state)
+        self.collisionChecker = create_collision_checker(self.scenario)
+        self.collisionChecker.add_collision_object(create_collision_object(road_bound))
 
-    def map_obstacles_to_lanelets(self, time_step : int) -> dict:
+    def map_obstacles_to_lanelets(self, time_step: int):
         """
-        Finds all obstacles that are located in every lanelet at time step t and returns a dictionary where obstacles are stored according to their lanelet ids.
+        Find all obstacles that are located in every lanelet at time step t and returns a dictionary where obstacles are stored according to lanelet id.
 
-        :param time_step: maps obstacles in this time step to the lanelet
-        
+        :param time_step: The time step in which the obstacle is in the current lanelet network.
+        :Return type: dict[lanelet_id]
         """
         mapping = {}
-        for l in self.lanelet_network._lanelets.values():
+        for lanelet in self.lanelet_network.lanelets:
             # map obstacles to current lanelet
-            mapped_objs = self.get_obstacles(l, time_step)
+            mapped_objs = self.get_obstacles(lanelet, time_step)
             # check if mapping is not empty
             if len(mapped_objs) > 0:
-                mapping[l.lanelet_id] = mapped_objs
+                mapping[lanelet.lanelet_id] = mapped_objs
         return mapping
 
-    def get_obstacles(self, laneletObj: Lanelet, time_step : int) -> List[Obstacle]:
+    def get_obstacles(self, laneletObj: Lanelet, time_step: int) -> List[Obstacle]:
         """
         Returns the subset of obstacles, which are located in the given lanelet.
 
@@ -121,7 +142,7 @@ class MotionPlanner:
                         else:
                             vertices.append(sh.vertices)
                 else:
-                     # distinguish between type of shape (circle has no vertices)
+                    # distinguish between type of shape (circle has no vertices)
                     if isinstance(o_shape, Circle):
                         vertices = o_shape.center
                     else:
@@ -132,37 +153,40 @@ class MotionPlanner:
                     res.append(o)
         return res
 
-    
     def calc_lanelet_cost(self, curLanelet: Lanelet, dist: int, visited_lanelets: List[int]):
         """
         Calculates distances of all lanelets which can be reached through recursive adjacency/predecessor relationship by the current lanelet. 
         This is a recursive implementation.
 
-        :param curLanelet: the current lanelet object (often set to the goal lanelet)
-        :param dist: the initial distance between 2 adjacent lanelets (often set to 1). This value will increase recursively during the execution of this function. 
-        :param visited_lanelets: list of visited lanelet ids. In the iterations, visited lanelets will not be considered. This value changes during the recursive implementation.
+        :param curLanelet: the current lanelet object (Often set to the goal lanelet).
+        :param dist: the initial distance between 2 adjacent lanelets (Often set to 1). This value will increase recursively during the execution of this function.
+        :param visited_lanelets: list of visited lanelet id. In the iterations, visited lanelets will not be considered. This value changes during the recursive implementation.
 
         The calculated costs will be stored in dictionary self.lanelet_cost[Lanelet].
         """
         if curLanelet.lanelet_id in visited_lanelets:
             return
-        visited_lanelets.append(curLanelet.lanelet_id)
-        if curLanelet._predecessor is not None: