diff --git a/.flake8 b/.flake8
index 5a3d3cd9..6cce5018 100644
--- a/.flake8
+++ b/.flake8
@@ -2,4 +2,6 @@
 exclude= code/planning/src/behavior_agent/behavior_tree.py,
          code/planning/src/behavior_agent/behaviours/__init__.py,
          code/planning/src/behavior_agent/behaviours,
-         code/planning/__init__.py
\ No newline at end of file
+         code/planning/__init__.py,
+         doc/02_development/templates/template_class_no_comments.py,
+         doc/02_development/templates/template_class.py
\ No newline at end of file
diff --git a/build/docker-compose.yml b/build/docker-compose.yml
index cb719fb8..1372dfdb 100644
--- a/build/docker-compose.yml
+++ b/build/docker-compose.yml
@@ -62,7 +62,7 @@ services:
     tty: true
     shm_size: 2gb
     #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS"
-    #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
+    # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
     command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP"
 
     logging:
diff --git a/code/agent/launch/dev.launch b/code/agent/launch/dev.launch
index 071e9813..cc1f148d 100644
--- a/code/agent/launch/dev.launch
+++ b/code/agent/launch/dev.launch
@@ -32,7 +32,7 @@
     <arg name="objects_definition_file" value='$(find agent)/config/dev_objects.json'/>
   </include>
 
-  <include file="$(find agent)/launch/agent_manual.launch">
+  <include file="$(find agent)/launch/agent.launch">
     <arg name='role_name' value='$(arg role_name)'/>
   </include>
 
diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py
index 7345d0c0..bf15a011 100755
--- a/code/perception/src/vision_node.py
+++ b/code/perception/src/vision_node.py
@@ -191,7 +191,7 @@ def setup_camera_publishers(self):
 
     def setup_object_distance_publishers(self):
         self.distance_publisher = self.new_publisher(
-            msg_type=numpy_msg(Float32MultiArray),
+            msg_type=Float32MultiArray,
             topic=f"/paf/{self.role_name}/{self.side}/object_distance",
             qos_profile=1)
 
@@ -227,6 +227,7 @@ def handle_camera_image(self, image):
         if side == "Right":
             self.publisher_right.publish(img_msg)
 
+        # locals().clear()
         # print(f"Published Image on Side: {side}")
         pass
 
@@ -254,6 +255,7 @@ def handle_dist_array(self, dist_array):
                                       desired_encoding='passthrough')
         # print("RECEIVED DIST")
         self.dist_arrays = dist_array
+        # locals().clear()
 
     def predict_torch(self, image):
         self.model.eval()
@@ -285,40 +287,6 @@ def predict_ultralytics(self, image):
 
         output = self.model(cv_image, half=True, verbose=False)
 
-        """distance_output = []
-        c_boxes = []
-        c_labels = []
-        for r in output:
-            boxes = r.boxes
-            for box in boxes:
-                cls = box.cls.item()
-                pixels = box.xyxy[0]
-                print(pixels)
-                if len(self.depth_images) > 0:
-                    distances = np.asarray(
-                        [self.depth_images[i][int(pixels[1]):int(pixels[3]):1,
-                                              int(pixels[0]):int(pixels[2]):1]
-                            for i in range(len(self.depth_images))])
-                    non_zero_filter = distances[distances != 0]
-
-                    if len(non_zero_filter) > 0:
-                        obj_dist = np.min(non_zero_filter)
-                    else:
-                        obj_dist = np.inf
-
-                    c_boxes.append(torch.tensor(pixels))
-                    c_labels.append(f"Class: {cls}, Meters: {obj_dist}")
-                    distance_output.append([cls, obj_dist])
-
-        # print(distance_output)
-        # self.logerr(distance_output)
-        self.distance_publisher.publish(
-            Float32MultiArray(data=distance_output))
-
-        transposed_image = np.transpose(cv_image, (2, 0, 1))
-        image_np_with_detections = torch.tensor(transposed_image,
-                                                dtype=torch.uint8)"""
-
         # handle distance of objects
         distance_output = []
         c_boxes = []
@@ -339,14 +307,30 @@ def predict_ultralytics(self, image):
                     distances_copy[distances_copy == 0] = np.inf
 
                     if len(non_zero_filter) > 0:
-                        sorted_indices = np.argsort(distances_copy[:, :, 0],
-                                                    axis=None)
-                        x1, y1 = np.unravel_index(sorted_indices[0],
-                                                  distances_copy.shape[:2])
-                        x2, y2 = np.unravel_index(sorted_indices[1],
+                        min_x_sorted_indices = np.argsort(
+                            distances_copy[:, :, 0],
+                            axis=None)
+                        x1, y1 = np.unravel_index(min_x_sorted_indices[0],
                                                   distances_copy.shape[:2])
+                        """x2, y2 = np.unravel_index(min_x_sorted_indices[1],
+                                                  distances_copy.shape[:2])"""
+
+                        abs_distance_copy = np.abs(distances_copy.copy())
+                        min_y_sorted_indices = np.argsort(
+                            abs_distance_copy[:, :, 1],
+                            axis=None)
+                        x3, y3 = np.unravel_index(min_y_sorted_indices[0],
+                                                  abs_distance_copy.shape[:2])
+                        """
+                        x4, y4 = np.unravel_index(min_y_sorted_indices[1],
+                                                  abs_distance_copy.shape[:2])
+                        """
+
                         obj_dist1 = distances_copy[x1][y1].copy()
-                        obj_dist2 = distances_copy[x2][y2].copy()
+                        # obj_dist2 = distances_copy[x2][y2].copy()
+                        obj_dist3 = distances_copy[x3][y3].copy()
+                        # obj_dist4 = distances_copy[x4][y4].copy()
+                        # print(obj_dist1, obj_dist3)
 
                         abs_distance = np.sqrt(
                             obj_dist1[0]**2 +
@@ -355,38 +339,53 @@ def predict_ultralytics(self, image):
 
                         # create 2d glass plane at object
                         # with box dimension
-                        scale_width = abs(obj_dist1[1] - obj_dist2[1])\
-                            / abs(y1-y2)
-                        scale_height = abs(obj_dist1[2] - obj_dist2[2])\
-                            / abs(x1-x2)
-                        width = distances_copy.shape[1] * scale_width
-                        height = distances_copy.shape[0] * scale_height
-
-                        # upper left
-                        ul_x = obj_dist1[0]
-                        ul_y = obj_dist1[1] - (-y1 + scale_width)
-                        ul_z = obj_dist1[2] - (-x1 + scale_height)
-
-                        # lower right
-                        lr_x = obj_dist1[0]
-                        lr_y = ul_y + width
-                        lr_z = ul_z + height
+                        # width_diff = abs(y1-y2)
+                        # height_diff = abs(x1-x2)
+
+                        """if width_diff > 0 and height_diff > 0:
+                            scale_width = abs(obj_dist1[1] - obj_dist2[1])\
+                                / width_diff
+                            scale_height = abs(obj_dist1[2] - obj_dist2[2])\
+                                / height_diff
+                            width = distances_copy.shape[1] * scale_width
+                            height = distances_copy.shape[0] * scale_height
+
+                            # upper left
+                            ul_x = obj_dist1[0]
+                            ul_y = obj_dist1[1] - (-y1 + scale_width)
+                            ul_z = obj_dist1[2] - (-x1 + scale_height)
+
+                            # lower right
+                            lr_x = obj_dist1[0]
+                            lr_y = ul_y + width
+                            lr_z = ul_z + height
+
+                            # -5 < y < 8"""
+
+                        distance_output.append(float(cls))
+                        distance_output.append(float(obj_dist1[0]))
+                        distance_output.append(float(obj_dist3[1]))
 
                     else:
                         obj_dist1 = (np.inf, np.inf, np.inf)
+                        obj_dist3 = (np.inf, np.inf, np.inf)
                         abs_distance = np.inf
+                        """distance_output.append(float(cls))
+                        distance_output.append(float(abs_distance))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))"""
 
                     c_boxes.append(torch.tensor(pixels))
                     c_labels.append(f"Class: {cls},"
                                     f"Meters: {round(abs_distance, 2)},"
                                     f"({round(float(obj_dist1[0]), 2)},"
-                                    f"{round(float(obj_dist1[1]), 2)},"
-                                    f"{round(float(obj_dist1[2]), 2)})")
-                    distance_output.append([cls,
-                                            abs_distance,
-                                            ul_x, ul_y, ul_z,
-                                            lr_x, lr_y, lr_z])
+                                    f"{round(float(obj_dist3[1]), 2)})")
 
+        # print("DISTANCE_ARRAY: ", distance_output)
         self.distance_publisher.publish(
            Float32MultiArray(data=distance_output))
 
@@ -398,7 +397,6 @@ def predict_ultralytics(self, image):
             self.process_traffic_lights(output[0], cv_image, image.header)
 
         c_boxes = torch.stack(c_boxes)
-        # print(image_np_with_detections.shape, c_boxes.shape, c_labels)
         box = draw_bounding_boxes(image_np_with_detections,
                                   c_boxes,
                                   c_labels,
@@ -408,7 +406,9 @@ def predict_ultralytics(self, image):
         np_box_img = np.transpose(box.detach().numpy(),
                                   (1, 2, 0))
         box_img = cv2.cvtColor(np_box_img, cv2.COLOR_BGR2RGB)
+        # locals().clear()
         return box_img
+
         # return output[0].plot()
 
     def process_traffic_lights(self, prediction, cv_image, image_header):
@@ -438,6 +438,8 @@ def process_traffic_lights(self, prediction, cv_image, image_header):
             traffic_light_image.header = image_header
             self.traffic_light_publisher.publish(traffic_light_image)
 
+        # locals().clear()
+
     def create_mask(self, input_image, model_output):
         output_predictions = torch.argmax(model_output, dim=0)
         for i in range(21):
diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch
index 9dee1352..7437344e 100644
--- a/code/planning/launch/planning.launch
+++ b/code/planning/launch/planning.launch
@@ -1,11 +1,11 @@
 <launch>
     <node pkg="planning" type="collision_check.py" name="CollisionCheck" output="screen">
            <param name="role_name" value="hero" />
-           <param name="control_loop_rate" value="0.3" />
+           <param name="control_loop_rate" value="0.05" />
     </node>
     <node pkg="planning" type="ACC.py" name="ACC" output="screen">
         <param name="role_name" value="hero" />
-        <param name="control_loop_rate" value="0.5" />
+        <param name="control_loop_rate" value="0.3" />
     </node>
     <!-- <node pkg="local_planner" type="local_planner/dev_collision_publisher.py" name="DevCollisionCheck" output="screen">
         <param name="role_name" value="hero" />
@@ -13,7 +13,7 @@
     </node> -->
     <node pkg="planning" type="motion_planning.py" name="MotionPlanning" output="screen">
            <param name="role_name" value="hero" />
-           <param name="control_loop_rate" value="0.3" />
+           <param name="control_loop_rate" value="0.1" />
     </node>
   
     <!-- <node pkg="planning" type="dev_global_route.py" name="DevGlobalRoute" output="screen">
diff --git a/code/planning/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py
index 330a8f1d..eb73c823 100755
--- a/code/planning/src/behavior_agent/behaviours/__init__.py
+++ b/code/planning/src/behavior_agent/behaviours/__init__.py
@@ -1,3 +1,3 @@
-from . import intersection, lane_change, maneuvers, meta, road_features
+from . import intersection, lane_change, overtake, maneuvers, meta, road_features
 from . import topics2blackboard, traffic_objects
 from . import behavior_speed
diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py
index df88e762..248edd49 100755
--- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py
+++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py
@@ -47,7 +47,7 @@ def convert_to_ms(speed):
 # TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
 lc_app_blocked = Behavior("lc_app_blocked", -2)
 
-lc_app_free = Behavior("lc_app_free", convert_to_ms(20))
+lc_app_free = Behavior("lc_app_free", convert_to_ms(30.0))
 
 # Wait
 lc_wait = Behavior("lc_wait", 0)
@@ -63,6 +63,19 @@ def convert_to_ms(speed):
 lc_exit = Behavior("lc_exit", -1)  # Get SpeedLimit dynamically
 
 
+# Overtake
+
+# Approach
+ot_app_blocked = Behavior("ot_app_blocked", -2)
+ot_app_free = Behavior("ot_app_free", -1)
+# Wait
+ot_wait_stopped = Behavior("ot_wait_stopped", convert_to_ms(0.0))
+ot_wait_free = Behavior("ot_wait_free", -1)
+# Enter
+ot_enter_init = Behavior("ot_enter_init", -1)
+ot_enter_slow = Behavior("ot_enter_slow", -2)
+# Exit
+ot_leave = Behavior("ot_leave", -1)
 # Cruise
 
 cruise = Behavior("Cruise", -1)
diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py
index 36bc2c36..bf0941be 100644
--- a/code/planning/src/behavior_agent/behaviours/overtake.py
+++ b/code/planning/src/behavior_agent/behaviours/overtake.py
@@ -2,6 +2,7 @@
 from std_msgs.msg import String
 
 import rospy
+import numpy as np
 
 from . import behavior_speed as bs
 
@@ -14,6 +15,10 @@ def convert_to_ms(speed):
     return speed / 3.6
 
 
+# Varaible to determine if overtake is currently exec
+OVERTAKE_EXECUTING = False
+
+
 class Approach(py_trees.behaviour.Behaviour):
     """
     This behaviour is executed when the ego vehicle is in close proximity of
@@ -61,13 +66,7 @@ def initialise(self):
         rospy.loginfo("Approaching Overtake")
         self.ot_distance = 30
 
-        self.ot_option = 1  # self.blackboard.get("paf/hero/...")
-        if self.ot_option == 0:
-            self.clear_distance = 15
-            self.side = "LIDAR_range_rear_left"
-        elif self.ot_option == 1:
-            self.clear_distance = 30
-            self.side = "LIDAR_range"
+        self.clear_distance = 30
 
     def update(self):
         """
@@ -85,24 +84,30 @@ def update(self):
                  object or entered the process.
                  py_trees.common.Status.FAILURE,
         """
+        global OVERTAKE_EXECUTING
         # Update distance to collision object
-        _dis = self.blackboard.get("/paf/hero/LIDAR_range")
+        _dis = self.blackboard.get("/paf/hero/collision")
         if _dis is not None:
-            self.ot_distance = _dis.data
+            self.ot_distance = _dis.data[0]
             rospy.loginfo(f"Overtake distance: {self.ot_distance}")
+            OVERTAKE_EXECUTING = self.ot_distance
+
+        if np.isinf(self.ot_distance):
+            rospy.loginfo("OvertakeApproach: Abort")
+            return py_trees.common.Status.FAILURE
 
         # slow down before overtake if blocked
-        if self.ot_distance < 15.0:
-            distance_lidar = self.blackboard. \
-                    get(f"/carla/hero/{self.side}")
-            if distance_lidar is not None:
-                distance_lidar = distance_lidar.data
+        if self.ot_distance < 20.0:
+            data = self.blackboard.get("/paf/hero/oncoming")
+            if data is not None:
+                distance_oncoming = data.data
             else:
-                distance_lidar = None
+                distance_oncoming = 35
 
-            if distance_lidar is not None and \
-                    distance_lidar > self.clear_distance:
+            if distance_oncoming is not None and \
+                    distance_oncoming > self.clear_distance:
                 rospy.loginfo("Overtake is free not slowing down!")
+                self.curr_behavior_pub.publish(bs.ot_app_free.name)
                 return py_trees.common.Status.SUCCESS
             else:
                 rospy.loginfo("Overtake blocked slowing down")
@@ -116,17 +121,17 @@ def update(self):
             rospy.logwarn("no speedometer connected")
             return py_trees.common.Status.RUNNING
 
-        if self.ot_distance > 15.0:
+        if self.ot_distance > 20.0:
             # too far
             rospy.loginfo("still approaching")
             return py_trees.common.Status.RUNNING
         elif speed < convert_to_ms(2.0) and \
-                self.ot_distance < 5.0:
+                self.ot_distance < 6.0:
             # stopped
             rospy.loginfo("stopped")
             return py_trees.common.Status.SUCCESS
         else:
-            rospy.logerr("Overtake Approach: Default Case")
+            # still approaching
             return py_trees.common.Status.RUNNING
 
     def terminate(self, new_status):
@@ -208,37 +213,33 @@ def update(self):
         """
 
         # Update distance to collison and distance for clear
-        self.ot_option = 1  # self.blackboard.get("paf/hero/...")
-        if self.ot_option == 0:
-            distance_lidar = self.blackboard. \
-                get("/carla/hero/LIDAR_range_rear_left")
-            clear_distance = 15
-        elif self.ot_option == 1:
-            distance_lidar = self.blackboard. \
-                get("/carla/hero/LIDAR_range")
-            distance_lidar = distance_lidar.data
-            clear_distance = 30
-        else:
-            distance_lidar = None
-
+        clear_distance = 30
         obstacle_msg = self.blackboard.get("/paf/hero/collision")
         if obstacle_msg is None:
+            rospy.logerr("No OBSTACLE")
             return py_trees.common.Status.FAILURE
 
-        if distance_lidar is not None:
-            collision_distance = distance_lidar.data
-            if collision_distance > clear_distance:
+        data = self.blackboard.get("/paf/hero/oncoming")
+        if data is not None:
+            distance_oncoming = data.data
+        else:
+            distance_oncoming = 35
+
+        if distance_oncoming is not None:
+            if distance_oncoming > clear_distance:
                 rospy.loginfo("Overtake is free!")
+                self.curr_behavior_pub.publish(bs.ot_wait_free.name)
                 return py_trees.common.Status.SUCCESS
             else:
-                rospy.loginfo("Overtake still blocked")
+                rospy.loginfo(f"Overtake still blocked: {distance_oncoming}")
                 self.curr_behavior_pub.publish(bs.ot_wait_stopped.name)
-                return py_trees.commom.Status.RUNNING
-        elif obstacle_msg[1] > convert_to_ms(2):
+                return py_trees.common.Status.RUNNING
+        elif obstacle_msg.data[0] == np.inf:
+            rospy.loginf("No OBSTACLE")
             return py_trees.common.Status.FAILURE
         else:
             rospy.loginfo("No Lidar Distance")
-            return py_trees.common.Success
+            return py_trees.common.Status.SUCCESS
 
     def terminate(self, new_status):
         """
@@ -315,9 +316,21 @@ def update(self):
                  py_trees.common.Status.SUCCESS,
                  py_trees.common.Status.FAILURE,
         """
-
-        return py_trees.common.Status.SUCCESS
-
+        status = self.blackboard.get("/paf/hero/overtake_success")
+        if status is not None:
+            if status.data == 1:
+                rospy.loginfo("Overtake: Trajectory planned")
+                return py_trees.common.Status.SUCCESS
+            elif status.data == 0:
+                self.curr_behavior_pub.publish(bs.ot_enter_slow.name)
+                rospy.loginfo("Overtake: Slowing down")
+                return py_trees.common.Status.RUNNING
+            else:
+                rospy.loginfo("OvertakeEnter: Abort")
+                return py_trees.common.Status.FAILURE
+        else:
+            rospy.loginfo("Overtake: Waiting for status update")
+            return py_trees.common.Status.RUNNING
         # Currently not in use
         # Can be used to check if we can go back to the original lane
 
@@ -377,8 +390,11 @@ def initialise(self):
             Any initialisation you need before putting your behaviour to work.
         This prints a state status message and publishes the behavior
         """
-        rospy.loginfo("Leave Overtake")
         self.curr_behavior_pub.publish(bs.ot_leave.name)
+        data = self.blackboard.get("/paf/hero/current_pos")
+        self.first_pos = np.array([data.pose.position.x,
+                                   data.pose.position.y])
+        rospy.loginfo(f"Leave Overtake: {self.first_pos}")
         return True
 
     def update(self):
@@ -392,7 +408,16 @@ def update(self):
         Abort this subtree
         :return: py_trees.common.Status.FAILURE, to exit this subtree
         """
-        return py_trees.common.Status.FAILURE
+        global OVERTAKE_EXECUTING
+        data = self.blackboard.get("/paf/hero/current_pos")
+        self.current_pos = np.array([data.pose.position.x,
+                                    data.pose.position.y])
+        distance = np.linalg.norm(self.first_pos - self.current_pos)
+        if distance > OVERTAKE_EXECUTING:
+            rospy.loginfo(f"Left Overtake: {self.current_pos}")
+            return py_trees.common.Status.FAILURE
+        else:
+            return py_trees.common.Status.RUNNING
 
     def terminate(self, new_status):
         """
diff --git a/code/planning/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py
index 0e6786db..a00d566b 100755
--- a/code/planning/src/behavior_agent/behaviours/road_features.py
+++ b/code/planning/src/behavior_agent/behaviours/road_features.py
@@ -3,8 +3,8 @@
 
 import py_trees
 import numpy as np
-
-
+from scipy.spatial.transform import Rotation
+import rospy
 """
 Source: https://github.com/ll7/psaf2
 """
@@ -216,6 +216,8 @@ def initialise(self):
         What to do here?
             Any initialisation you need before putting your behaviour to work.
         """
+        # Counter for detecting overtake situation
+        self.counter_overtake = 0
         return True
 
     def update(self):
@@ -235,17 +237,40 @@ def update(self):
         """
 
         obstacle_msg = self.blackboard.get("/paf/hero/collision")
-        if obstacle_msg is None:
+        current_position = self.blackboard.get("/paf/hero/current_pos")
+        current_heading = self.blackboard.get("/paf/hero/current_heading").data
+
+        if obstacle_msg is None or \
+                current_position is None or \
+                current_heading is None:
             return py_trees.common.Status.FAILURE
+        current_position = [current_position.pose.position.x,
+                            current_position.pose.position.y,
+                            current_position.pose.position.z]
 
         obstacle_distance = obstacle_msg.data[0]
         obstacle_speed = obstacle_msg.data[1]
 
         if obstacle_distance == np.Inf:
             return py_trees.common.Status.FAILURE
+        # calculate approx collision position in global coords
+        rotation_matrix = Rotation.from_euler('z', current_heading)
+        # Apply current heading to absolute distance vector
+        # and add to current position
+        pos_moved_in_x_direction = current_position + rotation_matrix.apply(
+                np.array([obstacle_distance, 0, 0]))
+
+        if np.linalg.norm(pos_moved_in_x_direction - current_position) < 1:
+            # current collision is not near trajectory lane
+            rospy.logerr("Obstacle is not near trajectory lane")
+            return py_trees.common.Status.FAILURE
 
         if obstacle_speed < 2 and obstacle_distance < 30:
-            return py_trees.common.Status.SUCCESS
+            self.counter_overtake += 1
+            rospy.loginfo("Overtake counter: " + str(self.counter_overtake))
+            if self.counter_overtake > 3:
+                return py_trees.common.Status.SUCCESS
+            return py_trees.common.Status.RUNNING
         else:
             return py_trees.common.Status.FAILURE
 
diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py
index 86f3c3b1..d26a717d 100755
--- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py
+++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py
@@ -38,8 +38,6 @@ def create_node(role_name):
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/max_velocity", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
-        {'name': f"/carla/{role_name}/LIDAR_range", 'msg': Float32,
-         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/speed_limit", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/lane_change_distance", 'msg': LaneChange,
@@ -47,7 +45,13 @@ def create_node(role_name):
         {'name': f"/paf/{role_name}/collision", 'msg': Float32MultiArray,
          'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE},
         {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped,
-         'clearing-policy': py_trees.common.ClearingPolicy.NEVER}
+         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
+        {'name': f"/paf/{role_name}/current_heading", 'msg': Float32,
+         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
+        {'name': f"/paf/{role_name}/overtake_success", 'msg': Float32,
+         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
+        {'name': f"/paf/{role_name}/oncoming", 'msg': Float32,
+         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
     ]
 
     topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard")
diff --git a/code/planning/src/global_planner/dev_global_route.py b/code/planning/src/global_planner/dev_global_route.py
index 824bafa5..1857293c 100755
--- a/code/planning/src/global_planner/dev_global_route.py
+++ b/code/planning/src/global_planner/dev_global_route.py
@@ -51,7 +51,7 @@ def __init__(self):
 
         self.global_plan_pub = self.new_publisher(
             msg_type=CarlaRoute,
-            topic='/paf/' + self.role_name + '/global_plan',
+            topic='/carla/' + self.role_name + '/global_plan',
             qos_profile=QoSProfile(
                 depth=1,
                 durability=DurabilityPolicy.TRANSIENT_LOCAL)
diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py
index 41ee798c..23442530 100755
--- a/code/planning/src/global_planner/global_planner.py
+++ b/code/planning/src/global_planner/global_planner.py
@@ -121,8 +121,6 @@ def global_route_callback(self, data: CarlaRoute) -> None:
             self.global_route_backup = data
             return
 
-        self.global_route_backup = None
-
         # get the first turn command (1, 2, or 3)
         ind = 0
         for i, opt in enumerate(data.road_options):
@@ -194,7 +192,8 @@ def global_route_callback(self, data: CarlaRoute) -> None:
         self.path_backup.header.frame_id = "global"
         self.path_backup.poses = stamped_poses
         self.path_pub.publish(self.path_backup)
-        self.loginfo("PrePlanner: published trajectory")
+        self.global_route_backup = None
+        self.logerr("PrePlanner: published trajectory")
 
     def world_info_callback(self, opendrive: String) -> None:
         """
@@ -241,7 +240,10 @@ def position_callback(self, data: PoseStamped):
         if self.global_route_backup is not None:
             self.loginfo("PrePlanner: Received a pose update retrying "
                          "route preplanning")
-            self.global_route_callback(self.global_route_backup)
+            try:
+                self.global_route_callback(self.global_route_backup)
+            except Exception:
+                self.logerr("Preplanner failed -> restart")
 
     def dev_load_world_info(self):
         file_path = \
diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py
index e09bc575..e1ac88be 100755
--- a/code/planning/src/local_planner/ACC.py
+++ b/code/planning/src/local_planner/ACC.py
@@ -9,7 +9,7 @@
 # from std_msgs.msg import String
 from std_msgs.msg import Float32MultiArray, Float32
 from collision_check import CollisionCheck
-import time
+import numpy as np
 
 
 class ACC(CompatibleNode):
@@ -21,7 +21,6 @@ def __init__(self):
         super(ACC, self).__init__('ACC')
         self.role_name = self.get_param("role_name", "hero")
         self.control_loop_rate = self.get_param("control_loop_rate", 1)
-        self.current_speed = None  # m/ss
 
         # Get current speed
         self.velocity_sub: Subscriber = self.new_subscription(
@@ -30,13 +29,6 @@ def __init__(self):
             self.__get_current_velocity,
             qos_profile=1)
 
-        # Subscriber for lidar distance
-        # TODO: Change to real lidar distance
-        self.lidar_dist = self.new_subscription(
-            Float32,
-            f"/carla/{self.role_name}/LIDAR_range",
-            self._set_distance,
-            qos_profile=1)
         # Get initial set of speed limits
         self.speed_limit_OD_sub: Subscriber = self.new_subscription(
             Float32MultiArray,
@@ -57,9 +49,9 @@ def __init__(self):
             callback=self.__current_position_callback,
             qos_profile=1)
         self.approx_speed_sub = self.new_subscription(
-            Float32,
-            f"/paf/{self.role_name}/cc_speed",
-            self.__approx_speed_callback,
+            Float32MultiArray,
+            f"/paf/{self.role_name}/collision",
+            self.__collision_callback,
             qos_profile=1)
         # Publish desired speed to acting
         self.velocity_pub: Publisher = self.new_publisher(
@@ -92,15 +84,7 @@ def __init__(self):
 
         self.logdebug("ACC initialized")
 
-    def _set_distance(self, data: Float32):
-        """Get min distance to object in front from perception
-
-        Args:
-            data (Float32): Minimum Distance from LIDAR
-        """
-        self.obstacle_distance = data.data
-
-    def __approx_speed_callback(self, data: Float32):
+    def __collision_callback(self, data: Float32):
         """Safe approximated speed form obstacle in front together with
         timestamp when recieved.
         Timestamp is needed to check wether we still have a vehicle in front
@@ -108,8 +92,12 @@ def __approx_speed_callback(self, data: Float32):
         Args:
             data (Float32): Speed from obstacle in front
         """
-        # self.logerr("ACC: Approx speed recieved: " + str(data.data))
-        self.obstacle_speed = (time.time(), data.data)
+        if np.isinf(data.data[0]):
+            self.obstacle_speed = None
+            self.obstacle_distance = None
+            return
+        self.obstacle_speed = data.data[1]
+        self.obstacle_distance = data.data[0]
 
     def __get_current_velocity(self, data: CarlaSpeedometer):
         """_summary_
@@ -171,11 +159,6 @@ def loop(timer_event=None):
             Checks if distance to a possible object is too small and
             publishes the desired speed to motion planning
             """
-            if self.obstacle_speed is not None:
-                # Check if too much time has passed since last speed update
-                if self.obstacle_speed[0] + 0.5 < time.time():
-                    self.obstacle_speed = None
-
             if self.obstacle_distance is not None and \
                     self.obstacle_speed is not None and \
                     self.__current_velocity is not None:
@@ -186,18 +169,37 @@ def loop(timer_event=None):
                 if self.obstacle_distance < safety_distance:
                     # If safety distance is reached, we want to reduce the
                     # speed to meet the desired distance
-                    safe_speed = self.obstacle_speed[1] * \
+                    # Lerp factor:
+                    # https://encyclopediaofmath.org/index.php?title=Linear_interpolation
+                    safe_speed = self.obstacle_speed * \
                         (self.obstacle_distance / safety_distance)
+                    lerp_factor = 0.2
+                    safe_speed = (1 - lerp_factor) * self.__current_velocity +\
+                        lerp_factor * safe_speed
+                    if safe_speed < 1.0:
+                        safe_speed = 0
+                    self.logerr("ACC: Safe speed: " + str(safe_speed) +
+                                " Distance: " + str(self.obstacle_distance))
                     self.velocity_pub.publish(safe_speed)
                 else:
-                    # If safety distance is reached, drive with same speed as
-                    # Object in front
-                    self.velocity_pub.publish(self.obstacle_speed[1])
+                    # If safety distance is reached just hold current speed
+                    # if self.obstacle_speed < 1.0:
+                    #     self.obstacle_speed = 0
+                    # self.logerr("ACC: my speed: " +
+                    #             str(self.__current_velocity))
+                    if self.__current_velocity < 1.0:
+                        safe_speed = 0
+                    else:
+                        safe_speed = self.__current_velocity
+                    self.velocity_pub.publish(safe_speed)
 
             elif self.speed_limit is not None:
                 # If we have no obstacle, we want to drive with the current
                 # speed limit
+                # self.logerr("ACC: Speed limit: " + str(self.speed_limit))
                 self.velocity_pub.publish(self.speed_limit)
+            else:
+                self.velocity_pub.publish(0)
 
         self.new_timer(self.control_loop_rate, loop)
         self.spin()
diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py
index 53146d2d..85540e21 100755
--- a/code/planning/src/local_planner/collision_check.py
+++ b/code/planning/src/local_planner/collision_check.py
@@ -1,15 +1,17 @@
 #!/usr/bin/env python
 # import rospy
 import numpy as np
+import rospy
 # import tf.transformations
 import ros_compatibility as roscomp
 from ros_compatibility.node import CompatibleNode
 from rospy import Subscriber
+# from rospy.numpy_msg import numpy_msg
 from carla_msgs.msg import CarlaSpeedometer   # , CarlaWorldInfo
 # from std_msgs.msg import String
 from std_msgs.msg import Float32, Float32MultiArray
 from std_msgs.msg import Bool
-import time
+from utils import filter_vision_objects
 
 
 class CollisionCheck(CompatibleNode):
@@ -34,9 +36,9 @@ def __init__(self):
         # Subscriber for lidar distance
         # TODO: Change to real lidar distance
         self.lidar_dist = self.new_subscription(
-            Float32,
-            f"/carla/{self.role_name}/LIDAR_range",
-            self.__set_distance,
+            Float32MultiArray,
+            f"/paf/{self.role_name}/Center/object_distance",
+            self.__set_all_distances,
             qos_profile=1)
         # Publisher for emergency stop
         self.emergency_pub = self.new_publisher(
@@ -48,6 +50,10 @@ def __init__(self):
             Float32MultiArray,
             f"/paf/{self.role_name}/collision",
             qos_profile=1)
+        self.oncoming_pub = self.new_publisher(
+            Float32,
+            f"/paf/{self.role_name}/oncoming",
+            qos_profile=1)
         # Approx speed publisher for ACC
         self.speed_publisher = self.new_publisher(
             Float32,
@@ -57,27 +63,83 @@ def __init__(self):
         self.__current_velocity: float = None
         self.__object_first_position: tuple = None
         self.__object_last_position: tuple = None
+        self.__last_position_oncoming: tuple = None
+        self.__first_position_oncoming: tuple = None
         self.logdebug("CollisionCheck started")
 
-    def update_distance(self):
+    def __set_all_distances(self, data: Float32MultiArray):
+        self.__set_distance(data)
+        self.__set_distance_oncoming(data)
+
+    def update_distance(self, reset):
         """Updates the distance to the obstacle in front
         """
-        if np.isinf(self.__object_last_position[1]):
+        if reset:
+            # Reset all values if we do not have car in front
             self.__object_last_position = None
             self.__object_first_position = None
+            # Signal to ACC to reset speed and distance
+            data = Float32MultiArray(data=[np.Inf, np.Inf])
+            self.collision_pub.publish(data)
             return
         if self.__object_first_position is None:
             self.__object_first_position = self.__object_last_position
             self.__object_last_position = None
             return
 
-    def __set_distance(self, data: Float32):
+    def __set_distance(self, data: Float32MultiArray):
+        """Saves last distance from  LIDAR
+
+        Args:
+            data (Float32): Message from lidar with distance
+        """
+        nearest_object = filter_vision_objects(data.data, False)
+        if nearest_object is None and \
+                self.__object_last_position is not None and \
+                rospy.get_rostime() - self.__object_last_position[0] > \
+                rospy.Duration(3):
+            self.update_distance(True)
+            return
+        elif nearest_object is None:
+            return
+        self.__object_last_position = (rospy.get_rostime(), nearest_object[1])
+        self.update_distance(False)
+        self.calculate_obstacle_speed()
+
+    def __set_distance_oncoming(self, data: Float32MultiArray):
         """Saves last distance from  LIDAR
 
         Args:
             data (Float32): Message from lidar with distance
         """
-        self.__object_last_position = (time.time(), data.data)
+        nearest_object = filter_vision_objects(data.data, True)
+        if (nearest_object is None and
+                self.__last_position_oncoming is not None and
+                rospy.get_rostime() - self.__last_position_oncoming[0] >
+                rospy.Duration(3)):
+            self.update_distance_oncoming(True)
+            return
+        elif nearest_object is None:
+            return
+
+        self.__last_position_oncoming =  \
+            (rospy.get_rostime(), nearest_object[1])
+        self.update_distance_oncoming(False)
+        self.oncoming_pub.publish(Float32(data=nearest_object[1]))
+
+    def update_distance_oncoming(self, reset):
+        """Updates the distance to the obstacle in front
+        """
+        if reset:
+            # Reset all values if we do not have car in front
+            self.__last_position_oncoming = None
+            self.__first_position_oncoming = None
+            self.oncoming_pub.publish(Float32(data=np.inf))
+            return
+        if self.__first_position_oncoming is None:
+            self.__first_position_oncoming = self.__last_position_oncoming
+            self.__last_position_oncoming = None
+            return
 
     def calculate_obstacle_speed(self):
         """Caluclate the speed of the obstacle in front of the ego vehicle
@@ -91,16 +153,21 @@ def calculate_obstacle_speed(self):
         # If distance is np.inf no car is in front
 
         # Calculate time since last position update
-        time_difference = self.__object_last_position[0] - \
+        rospy_time_difference = self.__object_last_position[0] - \
             self.__object_first_position[0]
-
+        time_difference = rospy_time_difference.nsecs/1e9
         # Calculate distance (in m)
-        distance = self.__object_last_position[1] -\
+        distance = self.__object_last_position[1] - \
             self.__object_first_position[1]
+        try:
+            # Speed is distance/time (m/s)
+            relative_speed = distance/time_difference
+        except ZeroDivisionError:
+            return
 
-        # Speed is distance/time (m/s)
-        relative_speed = distance/time_difference
         speed = self.__current_velocity + relative_speed
+        if speed < 0:
+            speed = 0
         # Publish speed to ACC for permanent distance check
         self.speed_publisher.publish(Float32(data=speed))
         # Check for crash
@@ -149,14 +216,16 @@ def calculate_rule_of_thumb(emergency, speed):
 
         Args:
             emergency (bool): if emergency brake is initiated
+            speed (float): speed of the vehicle (km/h)
 
         Returns:
             float: distance calculated with rule of thumb
         """
-        reaction_distance = speed
+        reaction_distance = speed * 0.5
         braking_distance = (speed * 0.36)**2
         if emergency:
-            return reaction_distance + braking_distance / 2
+            # Emergency brake is really effective in Carla
+            return reaction_distance + braking_distance / 4
         else:
             return reaction_distance + braking_distance
 
@@ -178,9 +247,8 @@ def check_crash(self, obstacle):
         if collision_time > 0:
             if distance < emergency_distance2:
                 # Initiate emergency brake
-                self.logerr("Emergency Brake")
+                self.logerr(f"Emergency Brake: {distance}")
                 self.emergency_pub.publish(True)
-                return
             # When no emergency brake is needed publish collision object
             data = Float32MultiArray(data=[distance, obstacle_speed])
             self.collision_pub.publish(data)
@@ -194,14 +262,14 @@ def run(self):
         Control loop
         :return:
         """
-        def loop(timer_event=None):
-            """
-            Checks if distance to a possible object is too small and
-            publishes the desired speed to motion planning
-            """
-            self.update_distance()
-            self.calculate_obstacle_speed()
-        self.new_timer(self.control_loop_rate, loop)
+        # def loop(timer_event=None):
+        #     """
+        #     Checks if distance to a possible object is too small and
+        #     publishes the desired speed to motion planning
+        #     """
+        #     self.update_distance()
+        #     self.calculate_obstacle_speed()
+        # self.new_timer(self.control_loop_rate, loop)
         self.spin()
 
 
diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py
index e72396b5..10f57135 100755
--- a/code/planning/src/local_planner/motion_planning.py
+++ b/code/planning/src/local_planner/motion_planning.py
@@ -2,24 +2,22 @@
 # import tf.transformations
 import ros_compatibility as roscomp
 import rospy
-import tf.transformations
 
 from ros_compatibility.node import CompatibleNode
 from rospy import Publisher, Subscriber
-from std_msgs.msg import String, Float32, Bool
+from std_msgs.msg import String, Float32, Bool, Float32MultiArray
 from nav_msgs.msg import Path
 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
 from carla_msgs.msg import CarlaSpeedometer
 import numpy as np
+from scipy.spatial.transform import Rotation
+import math
 
-from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \
-    import run_fot
 from perception.msg import Waypoint, LaneChange
 import planning  # noqa: F401
 from behavior_agent.behaviours import behavior_speed as bs
 
-from utils import convert_to_ms, approx_obstacle_pos, \
-    hyperparameters
+from utils import convert_to_ms, spawn_car
 
 # from scipy.spatial._kdtree import KDTree
 
@@ -42,23 +40,26 @@ def __init__(self):
         self.__acc_speed = 0.0
         self.__stopline = None  # (Distance, isStopline)
         self.__change_point = None  # (Distance, isLaneChange, roadOption)
+        self.__collision_point = None
+        self.__overtake_status = -1
         self.published = False
         self.current_pos = None
         self.current_heading = None
         self.trajectory = None
         self.overtaking = False
-        self.overtake_start = rospy.get_rostime()
         self.current_wp = None
         self.enhanced_path = None
         self.current_speed = None
         self.speed_limit = None
-
-        self.counter = 0
-        self.speed_list = []
-        self.__first_trajectory = None
         self.__corners = None
         self.__in_corner = False
+        self.calculated = False
         # Subscriber
+        self.test_sub = self.new_subscription(
+            Float32,
+            f"/paf/{self.role_name}/test",
+            spawn_car,
+            qos_profile=1)
         self.speed_limit_sub = self.new_subscription(
             Float32,
             f"/paf/{self.role_name}/speed_limit",
@@ -113,6 +114,12 @@ def __init__(self):
             self.__set_change_point,
             qos_profile=1)
 
+        self.change_point_sub: Subscriber = self.new_subscription(
+            Float32MultiArray,
+            f"/paf/{self.role_name}/collision",
+            self.__set_collision_point,
+            qos_profile=1)
+
         # Publisher
         self.traj_pub: Publisher = self.new_publisher(
             msg_type=Path,
@@ -128,10 +135,9 @@ def __init__(self):
             f"/paf/{self.role_name}/current_wp",
             self.__set_wp,
             qos_profile=1)
-        # Publisher for emergency stop
-        self.emergency_pub = self.new_publisher(
-            Bool,
-            f"/paf/{self.role_name}/emergency",
+        self.overtake_success_pub = self.new_publisher(
+            Float32,
+            f"/paf/{self.role_name}/overtake_success",
             qos_profile=1)
 
         self.logdebug("MotionPlanning started")
@@ -169,12 +175,6 @@ def __set_heading(self, data: Float32):
         """
         self.current_heading = data.data
 
-    def __save_trajectory(self, data):
-        if self.__first_trajectory is None:
-            self.__first_trajectory = data.poses
-
-            self.__corners = self.__calc_corner_points()
-
     def __set_current_pos(self, data: PoseStamped):
         """set current position
         Args:
@@ -184,55 +184,62 @@ def __set_current_pos(self, data: PoseStamped):
                                     data.pose.position.y,
                                     data.pose.position.z])
 
-    def change_trajectory(self, distance: float):
-        self.overtake_start = rospy.get_rostime()
-        limit_waypoints = 30
-        np_array = np.array(self.trajectory.poses)
-        obstacle_position = approx_obstacle_pos(distance,
-                                                self.current_heading,
-                                                self.current_pos,
-                                                self.current_speed)
-        # trajectory_np = self.convert_pose_to_array(np_array)
-        # wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1]
-        selection = np_array[int(self.current_wp):int(self.current_wp) +
-                             int(distance + limit_waypoints)]
+    def change_trajectory(self, distance_obj):
+        """update trajectory for overtaking and convert it
+        to a new Path message
+
+        Args:
+            distance_obj (float): distance to overtake object
+        """
+        pose_list = self.trajectory.poses
+
+        # Only use fallback
+        self.overtake_fallback(distance_obj, pose_list)
+        self.__overtake_status = 1
+        self.overtake_success_pub.publish(self.__overtake_status)
+        return
+
+    def overtake_fallback(self, distance, pose_list):
+        # self.loginfo("Overtake Fallback!")
+        # obstacle_position = approx_obstacle_pos(distance,
+        #                                         self.current_heading,
+        #                                         self.current_pos,
+        #                                         self.current_speed)
+        selection = pose_list[int(self.current_wp):int(self.current_wp) +
+                              int(distance) + 7]
         waypoints = self.convert_pose_to_array(selection)
 
-        initial_conditions = {
-            'ps': 0,
-            'target_speed': self.current_speed,
-            'pos': np.array([self.current_pos[0], self.current_pos[1]]),
-            'vel': np.array([obstacle_position[2][0],
-                             obstacle_position[2][1]]),
-            'wp': waypoints,
-            'obs': np.array([[obstacle_position[0][0],
-                              obstacle_position[0][1],
-                              obstacle_position[1][0],
-                              obstacle_position[1][1]]])
-        }
-        result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \
-            speeds_y, misc, costs, success = run_fot(initial_conditions,
-                                                     hyperparameters)
-        if success:
-            result = []
-            for i in range(len(result_x)):
-                position = Point(result_x[i], result_y[i], 0)
-                quaternion = tf.transformations.quaternion_from_euler(0,
-                                                                      0,
-                                                                      iyaw[i])
-                orientation = Quaternion(x=quaternion[0], y=quaternion[1],
-                                         z=quaternion[2], w=quaternion[3])
-                pose = Pose(position, orientation)
-                pos = PoseStamped()
-                pos.header.frame_id = "global"
-                pos.pose = pose
-                result.append(pos)
-            path = Path()
-            path.header.stamp = rospy.Time.now()
-            path.header.frame_id = "global"
-            path.poses = list(np_array[:int(self.current_wp)]) + \
-                result + list(np_array[int(self.current_wp + 25 + 30):])
-            self.trajectory = path
+        offset = np.array([2, 0, 0])
+        rotation_adjusted = Rotation.from_euler('z', self.current_heading +
+                                                math.radians(90))
+        offset_front = rotation_adjusted.apply(offset)
+        offset_front = offset_front[:2]
+        waypoints_off = waypoints + offset_front
+
+        result_x = waypoints_off[:, 0]
+        result_y = waypoints_off[:, 1]
+
+        result = []
+        for i in range(len(result_x)):
+            position = Point(result_x[i], result_y[i], 0)
+            # quaternion = tf.transformations.quaternion_from_euler(0,
+            #                                                         0,
+            #                                                          iyaw[i])
+            # orientation = Quaternion(x=quaternion[0], y=quaternion[1],
+            #                              z=quaternion[2], w=quaternion[3])
+            orientation = Quaternion(x=0, y=0,
+                                     z=0, w=0)
+            pose = Pose(position, orientation)
+            pos = PoseStamped()
+            pos.header.frame_id = "global"
+            pos.pose = pose
+            result.append(pos)
+        path = Path()
+        path.header.stamp = rospy.Time.now()
+        path.header.frame_id = "global"
+        path.poses = pose_list[:int(self.current_wp)] + \
+            result + pose_list[int(self.current_wp + distance + 7):]
+        self.trajectory = path
 
     def __set_trajectory(self, data: Path):
         """get current trajectory global planning
@@ -241,9 +248,11 @@ def __set_trajectory(self, data: Path):
             data (Path): Trajectory waypoints
         """
         self.trajectory = data
+        self.loginfo("Trajectory received")
+        self.__corners = self.__calc_corner_points()
 
     def __calc_corner_points(self):
-        coords = self.convert_pose_to_array(np.array(self.__first_trajectory))
+        coords = self.convert_pose_to_array(np.array(self.trajectory.poses))
         x_values = np.array([point[0] for point in coords])
         y_values = np.array([point[1] for point in coords])
 
@@ -285,7 +294,7 @@ def create_sublists(self, points, proximity=5):
 
     def get_cornering_speed(self):
         corner = self.__corners[0]
-        pos = self.current_pos
+        pos = self.current_pos[:2]
 
         def euclid_dist(vector1, vector2):
             point1 = np.array(vector1)
@@ -299,7 +308,7 @@ def map_corner(dist):
             if dist < 8:  # lane_change
                 return 8
             elif dist < 25:
-                return 6
+                return 4  # 6
             elif dist < 50:
                 return 7
             else:
@@ -308,7 +317,7 @@ def map_corner(dist):
         distance_corner = 0
         for i in range(len(corner) - 1):
             distance_corner += euclid_dist(corner[i], corner[i + 1])
-        # self.logerr(distance_corner)
+        # self.loginfo(distance_corner)
 
         if self.__in_corner:
             distance_end = euclid_dist(pos, corner[0])
@@ -328,7 +337,8 @@ def map_corner(dist):
         else:
             return self.__get_speed_cruise()
 
-    def convert_pose_to_array(self, poses: np.array):
+    @staticmethod
+    def convert_pose_to_array(poses: np.array):
         """convert pose array to numpy array
 
         Args:
@@ -350,20 +360,18 @@ def __check_emergency(self, data: Bool):
         Args:
             data (Bool): True if emergency stop detected by collision check
         """
-        # self.logerr("Emergency stop detected")
+        # self.loginfo("Emergency stop detected")
         if not self.__curr_behavior == bs.parking.name:
-            # self.logerr("Emergency stop detected and executed")
+            # self.loginfo("Emergency stop detected and executed")
             self.emergency_pub.publish(data)
 
     def update_target_speed(self, acc_speed, behavior):
         be_speed = self.get_speed_by_behavior(behavior)
-        if not behavior == bs.parking.name:
-            self.target_speed = min(be_speed, acc_speed)
-        else:
+        if behavior == bs.parking.name or self.__overtake_status == 1:
             self.target_speed = be_speed
-        # self.logerr("target speed: " + str(self.target_speed))
-        corner_speed = self.get_cornering_speed()
-        self.target_speed = min(self.target_speed, corner_speed)
+        else:
+            corner_speed = self.get_cornering_speed()
+            self.target_speed = min(be_speed, acc_speed, corner_speed)
         # self.target_speed = min(self.target_speed, 8)
         self.velocity_pub.publish(self.target_speed)
         # self.logerr(f"Speed: {self.target_speed}")
@@ -374,6 +382,12 @@ def __set_acc_speed(self, data: Float32):
 
     def __set_curr_behavior(self, data: String):
         self.__curr_behavior = data.data
+        if data.data == bs.ot_enter_init.name:
+            if np.isinf(self.__collision_point):
+                self.__overtake_status = -1
+                self.overtake_success_pub.publish(self.__overtake_status)
+                return
+            self.change_trajectory(self.__collision_point)
 
     def __set_stopline(self, data: Waypoint) -> float:
         if data is not None:
@@ -384,6 +398,10 @@ def __set_change_point(self, data: LaneChange):
             self.__change_point = \
                 (data.distance, data.isLaneChange, data.roadOption)
 
+    def __set_collision_point(self, data: Float32MultiArray):
+        if data.data is not None:
+            self.__collision_point = data.data[0]
+
     def get_speed_by_behavior(self, behavior: str) -> float:
         speed = 0.0
         split = "_"
@@ -392,9 +410,12 @@ def get_speed_by_behavior(self, behavior: str) -> float:
             speed = self.__get_speed_intersection(behavior)
         elif short_behavior == "lc":
             speed = self.__get_speed_lanechange(behavior)
+        elif short_behavior == "ot":
+            speed = self.__get_speed_overtake(behavior)
         elif short_behavior == "parking":
             speed = bs.parking.speed
         else:
+            self.__overtake_status = -1
             speed = self.__get_speed_cruise()
         return speed
 
@@ -432,6 +453,25 @@ def __get_speed_lanechange(self, behavior: str) -> float:
 
         return speed
 
+    def __get_speed_overtake(self, behavior: str) -> float:
+        speed = 0.0
+        if behavior == bs.ot_app_blocked.name:
+            speed = self.__calc_speed_to_stop_overtake()
+        elif behavior == bs.ot_app_free.name:
+            speed = self.__calc_speed_to_stop_overtake()
+        elif behavior == bs.ot_wait_stopped.name:
+            speed = bs.ot_wait_stopped.speed
+        elif behavior == bs.ot_wait_free.name:
+            speed == self.__get_speed_cruise()
+        elif behavior == bs.ot_enter_init.name:
+            speed = self.__get_speed_cruise()
+        elif behavior == bs.ot_enter_slow.name:
+            speed = self.__calc_speed_to_stop_overtake()
+        elif behavior == bs.ot_leave.name:
+            speed = convert_to_ms(10.)
+
+        return speed
+
     def __get_speed_cruise(self) -> float:
         return self.__acc_speed
 
@@ -461,6 +501,18 @@ def __calc_speed_to_stop_lanechange(self) -> float:
             v_stop = 0.0
         return v_stop
 
+    def __calc_speed_to_stop_overtake(self) -> float:
+        stopline = self.__calc_virtual_overtake()
+
+        v_stop = max(convert_to_ms(10.),
+                     convert_to_ms((stopline / 30)
+                                   * 50))
+        if stopline < 6.0:
+            v_stop = 0.0
+
+        self.logerr(f"Speed ovt: {v_stop}")
+        return v_stop
+
     def __calc_virtual_change_point(self) -> float:
         if self.__change_point[0] != np.inf and self.__change_point[1]:
             return self.__change_point[0]
@@ -473,6 +525,14 @@ def __calc_virtual_stopline(self) -> float:
         else:
             return 0.0
 
+    def __calc_virtual_overtake(self) -> float:
+        # self.logerr(f"Overtake point: {self.__collision_point}")
+        if (self.__collision_point is not None) and \
+                self.__collision_point != np.inf:
+            return self.__collision_point
+        else:
+            return 0.0
+
     def run(self):
         """
         Control loop
@@ -483,11 +543,12 @@ def loop(timer_event=None):
             if (self.__curr_behavior is not None and
                     self.__acc_speed is not None and
                     self.__corners is not None):
-                self.update_target_speed(self.__acc_speed,
-                                         self.__curr_behavior)
                 self.trajectory.header.stamp = rospy.Time.now()
                 self.traj_pub.publish(self.trajectory)
-
+                self.update_target_speed(self.__acc_speed,
+                                         self.__curr_behavior)
+            else:
+                self.velocity_pub.publish(0.0)
         self.new_timer(self.control_loop_rate, loop)
         self.spin()
 
diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py
index dee2fc25..c59675db 100644
--- a/code/planning/src/local_planner/utils.py
+++ b/code/planning/src/local_planner/utils.py
@@ -1,12 +1,14 @@
 from scipy.spatial.transform import Rotation
 import numpy as np
 import math
+# import rospy
+
 
 hyperparameters = {
     "max_speed": 15,
     "max_accel": 4.0,
     "max_curvature": 30.0,
-    "max_road_width_l": 0.1,
+    "max_road_width_l": 4,
     "max_road_width_r": 4,
     "d_road_w": 0.2,
     "dt": 0.2,
@@ -14,7 +16,7 @@
     "mint": 6.0,
     "d_t_s": 0.5,
     "n_s_sample": 2.0,
-    "obstacle_clearance": 2,
+    "obstacle_clearance": 1.5,
     "kd": 1.0,
     "kv": 0.1,
     "ka": 0.1,
@@ -112,3 +114,105 @@ def convert_to_ms(speed: float):
         float: speed in m/s
     """
     return speed / 3.6
+
+
+def spawn_car(distance):
+    """Only used for testing, spawns a car in the given distance
+
+    Args:
+        distance (float): distance
+    """
+    import carla
+    import os
+    CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1')
+    CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000'))
+
+    client = carla.Client(CARLA_HOST, CARLA_PORT)
+
+    world = client.get_world()
+    world.wait_for_tick()
+
+    blueprint_library = world.get_blueprint_library()
+    # bp = blueprint_library.filter('vehicle.*')[0]
+    # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0])
+    bp = blueprint_library.filter("model3")[0]
+    for actor in world.get_actors():
+        if actor.attributes.get('role_name') == "hero":
+            ego_vehicle = actor
+            break
+
+    spawnPoint = carla.Transform(ego_vehicle.get_location() +
+                                 carla.Location(y=distance.data),
+                                 ego_vehicle.get_transform().rotation)
+    spawnpoint2 = carla.Transform(ego_vehicle.get_location() +
+                                  carla.Location(x=2.5, y=distance.data + 1),
+                                  ego_vehicle.get_transform().rotation)
+    vehicle = world.spawn_actor(bp, spawnPoint)
+    vehicle2 = world.spawn_actor(bp, spawnpoint2)
+    vehicle2.set_autopilot(False)
+    vehicle.set_autopilot(False)
+    # vehicle.set_location(loc)
+    # coords = vehicle.get_location()
+    # get spectator
+    spectator = world.get_spectator()
+    # set spectator to follow ego vehicle with offset
+    spectator.set_transform(
+        carla.Transform(ego_vehicle.get_location() + carla.Location(z=50),
+                        carla.Rotation(pitch=-90)))
+
+
+def filter_vision_objects(float_array, oncoming):
+    """Filters vision objects to calculate collision check
+    It contains the classId, the absolute Euclidean distance
+    and 6 coordinates for upper left and lower right corner
+    of the bounding box
+
+    Array shape: [classID, EuclidDistance,
+                    UpperLeft(x,y,z), LowerRight(x,y,z)]
+
+    Args:
+        data (ndarray): numpy array with vision objects
+    """
+
+    """
+    LEON:
+
+    Ihr bekommt jetzt nur 3-Werte -> ClassIndex, Min_X, Min_Abs_Y
+
+    Min_Abs_Y ist der nähste Punkt vom Object zu Y=0 also der Mitte.
+
+    Damit habt ihr immer automatisch den
+    nähesten und wichtigsten Punkt des Objekts.
+
+    """
+
+    # Reshape array to 3 columns and n rows (one row per object)
+    float_array = np.asarray(float_array)
+    float_array = np.reshape(float_array, (float_array.size//3, 3))
+    # Filter all rows that contain np.inf
+    float_array = float_array[~np.any(np.isinf(float_array), axis=1), :]
+    if float_array.size == 0:
+        return None
+    # Filter out all objects that are not cars
+    all_cars = float_array[np.where(float_array[:, 0] == 2)]
+
+    # Get cars that are on our lane
+    if oncoming:
+        cars_in_front = \
+            all_cars[np.where(all_cars[:, 2] > 0.3)]
+        if cars_in_front.size != 0:
+            cars_in_front = cars_in_front[np.where(cars_in_front[:, 2] < 1.3)]
+    else:
+        cars_in_front = all_cars[np.where(all_cars[:, 2] < 0.1)]
+        if cars_in_front.size != 0:
+            cars_in_front = cars_in_front[np.where(cars_in_front[:, 2] > -0.2)]
+    if cars_in_front.size == 0:
+        # no car in front
+        return None
+    # Filter for potential recognition of ega vehicle front hood
+    filtered_cars_in_front = cars_in_front[np.where(cars_in_front[:, 1] > 0.7)]
+    if filtered_cars_in_front.size == 0:
+        # no car in front
+        return None
+    # Return nearest car
+    return filtered_cars_in_front[np.argmin(filtered_cars_in_front[:, 1])]
diff --git a/code/test-route/launch/test-route.launch b/code/test-route/launch/test-route.launch
index de985c6b..189d9ace 100644
--- a/code/test-route/launch/test-route.launch
+++ b/code/test-route/launch/test-route.launch
@@ -6,7 +6,7 @@
     <param name="control_loop_rate" value="$(arg control_loop_rate)" />
     <param name="role_name" value="$(arg role_name)" />
     <param name="follow_hero" value="True" />
-    <param name="vehicle_number" value="50" />
+    <param name="vehicle_number" value="0" />
     <param name="only_cars" value="False" />
     <param name="disable_vehicle_lane_change" value="False" />
   </node>
diff --git a/doc/03_research/03_planning/00_paf23/test_traj.py b/doc/03_research/03_planning/00_paf23/test_traj.py
index b4e1165a..97283e6c 100644
--- a/doc/03_research/03_planning/00_paf23/test_traj.py
+++ b/doc/03_research/03_planning/00_paf23/test_traj.py
@@ -1,4 +1,5 @@
-from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper import run_fot
+from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \
+    import run_fot
 import numpy as np
 import matplotlib.pyplot as plt
 
diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/pt.py b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py
index 3b8ccc3f..145fbcfe 100644
--- a/doc/06_perception/experiments/object-detection-model_evaluation/pt.py
+++ b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py
@@ -6,8 +6,11 @@
 from time import perf_counter
 import torch
 import torchvision
-from torchvision.models.detection.faster_rcnn import FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, FasterRCNN_ResNet50_FPN_V2_Weights
-from torchvision.models.detection.retinanet import RetinaNet_ResNet50_FPN_V2_Weights
+from torchvision.models.detection.faster_rcnn import \
+    FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, \
+    FasterRCNN_ResNet50_FPN_V2_Weights
+from torchvision.models.detection.retinanet import \
+    RetinaNet_ResNet50_FPN_V2_Weights
 from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST
 from torchvision.utils import draw_bounding_boxes
 from pathlib import Path
@@ -17,7 +20,8 @@
 from torchvision.transforms.functional import to_pil_image
 
 ALL_MODELS = {
-    'fasterrcnn_mobilenet_v3_large_320_fpn': FasterRCNN_MobileNet_V3_Large_320_FPN_Weights,
+    'fasterrcnn_mobilenet_v3_large_320_fpn':
+    FasterRCNN_MobileNet_V3_Large_320_FPN_Weights,
     'fasterrcnn_resnet50_fpn_v2': FasterRCNN_ResNet50_FPN_V2_Weights,
     'retinanet_resnet50_fpn_v2': RetinaNet_ResNet50_FPN_V2_Weights,
 }
@@ -28,7 +32,9 @@ def load_model(model_name):
     print('Loading model...', end='')
     device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
     weights = ALL_MODELS[model_name].DEFAULT
-    model = torchvision.models.detection.__dict__[model_name](weights=weights).to(device)
+    model = torchvision.models.detection.__dict__[model_name](
+            weights=weights
+        ).to(device)
     model.eval()
     return model, weights, device
 
@@ -73,18 +79,21 @@ def load_image(image_path, model_weights, device):
 
         label_id_offset = -1
 
-        image_np_with_detections = torch.tensor(image_np * 255, dtype=torch.uint8)
+        image_np_with_detections = torch.tensor(image_np * 255,
+                                                dtype=torch.uint8)
         boxes = result['boxes']
         scores = result['scores']
         labels = [weights.meta["categories"][i] for i in result['labels']]
 
-        box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels, colors='red', width=2)
+        box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels,
+                                  colors='red', width=2)
         box_img = to_pil_image(box)
 
         file_name = Path(image_path).stem
 
         plt.figure(figsize=(32, 18))
-        plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms', fontsize=30)
+        plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms',
+                  fontsize=30)
         plt.imshow(box_img)
         plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_PT_{m}.jpg')
         plt.close()
diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py
index f3a670c6..d59e5e75 100644
--- a/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py
+++ b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py
@@ -1,5 +1,6 @@
 '''
-Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection, https://pylot.readthedocs.io/en/latest/perception.detection.html
+Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection,
+https://pylot.readthedocs.io/en/latest/perception.detection.html
 '''
 
 from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST
@@ -61,7 +62,8 @@ def get_category_index(label_file):
     with open(label_file, 'r') as f:
         labels = f.readlines()
         labels = [label.strip() for label in labels]
-        category_index = {i: {'id': i, 'name': name} for i, name in enumerate(labels)}
+        category_index = \
+            {i: {'id': i, 'name': name} for i, name in enumerate(labels)}
     return category_index
 
 
@@ -114,7 +116,8 @@ def get_category_index(label_file):
         file_name = Path(image_path).stem
 
         plt.figure(figsize=(32, 18))
-        plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms', fontsize=30)
+        plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms',
+                  fontsize=30)
         plt.imshow(image_np_with_detections[0])
         plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_TF_{m}.jpg')
         plt.close()