diff --git a/.flake8 b/.flake8
index 845122a1..5a3d3cd9 100644
--- a/.flake8
+++ b/.flake8
@@ -1,4 +1,5 @@
 [flake8]
 exclude= code/planning/src/behavior_agent/behavior_tree.py,
          code/planning/src/behavior_agent/behaviours/__init__.py,
-         code/planning/src/behavior_agent/behaviours
\ No newline at end of file
+         code/planning/src/behavior_agent/behaviours,
+         code/planning/__init__.py
\ No newline at end of file
diff --git a/build/docker-compose.yml b/build/docker-compose.yml
index 3d7f6eb5..1372dfdb 100644
--- a/build/docker-compose.yml
+++ b/build/docker-compose.yml
@@ -62,8 +62,9 @@ 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 && 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=carla-simulator --track=MAP"
+    # 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:
       driver: "local"
     environment:
diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile
index 5a9c37c0..acf7edf7 100644
--- a/build/docker/agent/Dockerfile
+++ b/build/docker/agent/Dockerfile
@@ -162,6 +162,21 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt
 # Add agent code
 COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/
 
+# Install Frenet Optimal Trajectory Planner 
+RUN sudo mkdir /erdos
+RUN sudo mkdir /erdos/dependencies
+RUN sudo mkdir /erdos/dependencies/frenet_optimal_trajectory_planner
+# Needed to resolve dependencies correctly inside freent_optimal_trajectory_planner 
+ENV PYLOT_HOME=/erdos
+
+ENV FREENET_HOME=/erdos/dependencies/frenet_optimal_trajectory_planner
+RUN sudo chown $USERNAME:$USERNAME $PYLOT_HOME
+RUN sudo chown $USERNAME:$USERNAME $FREENET_HOME
+
+RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git $FREENET_HOME
+RUN cd $FREENET_HOME && source ./build.sh
+
+ENV PYTHONPATH=$PYTHONPATH:/erdos/dependencies
 # Link code into catkin workspace
 RUN ln -s /workspace/code /catkin_ws/src
 
@@ -170,6 +185,8 @@ RUN source /opt/ros/noetic/setup.bash && catkin_make
 
 ADD ./build/docker/agent/entrypoint.sh /entrypoint.sh
 
+
+
 # set the default working directory to the code
 WORKDIR /workspace/code
 
diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz
index 5c10eca9..2e419cc2 100644
--- a/code/agent/config/rviz_config.rviz
+++ b/code/agent/config/rviz_config.rviz
@@ -67,7 +67,7 @@ Visualization Manager:
         Imu: false
         Path: false
         PointCloud2: false
-        Value: false
+        Value: true
       Zoom Factor: 1
     - Class: rviz/Image
       Enabled: true
@@ -166,7 +166,7 @@ Visualization Manager:
       Offset:
         X: 0
         Y: 0
-        Z: 39
+        Z: 703
       Pose Color: 255; 85; 255
       Pose Style: None
       Queue Size: 10
diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py
index ada59c5f..cb3d9e5c 100644
--- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py
+++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py
@@ -64,7 +64,7 @@ def __call__(self, img):
         else:
             out = self.model(img)
         _, prediction = torch.max(out.data, 1)
-        return prediction.item()
+        return (prediction.item(), out.data.cpu().numpy())
 
 
 # main function for testing purposes
diff --git a/code/perception/src/traffic_light_node.py b/code/perception/src/traffic_light_node.py
index 6f67b5b1..00034bab 100755
--- a/code/perception/src/traffic_light_node.py
+++ b/code/perception/src/traffic_light_node.py
@@ -1,5 +1,8 @@
 #!/usr/bin/env python3
 
+from datetime import datetime
+import threading
+from time import sleep
 from ros_compatibility.node import CompatibleNode
 import ros_compatibility as roscomp
 from rospy.numpy_msg import numpy_msg
@@ -18,6 +21,9 @@ def __init__(self, name, **kwargs):
         self.role_name = self.get_param("role_name", "hero")
         self.side = self.get_param("side", "Center")
         self.classifier = TrafficLightInference(self.get_param("model", ""))
+        self.last_info_time: datetime = None
+        self.last_state = None
+        threading.Thread(target=self.auto_invalidate_state).start()
 
         # publish / subscribe setup
         self.setup_camera_subscriptions()
@@ -38,14 +44,38 @@ def setup_traffic_light_publishers(self):
             qos_profile=1
         )
 
+    def auto_invalidate_state(self):
+        while True:
+            sleep(1)
+
+            if self.last_info_time is None:
+                continue
+
+            if (datetime.now() - self.last_info_time).total_seconds() >= 2:
+                msg = TrafficLightState()
+                msg.state = 0
+                self.traffic_light_publisher.publish(msg)
+                self.last_info_time = None
+
     def handle_camera_image(self, image):
-        result = self.classifier(self.bridge.imgmsg_to_cv2(image))
+        result, data = self.classifier(self.bridge.imgmsg_to_cv2(image))
+
+        if data[0][0] > 1e-15 and data[0][3] > 1e-15 or \
+           data[0][0] > 1e-10 or data[0][3] > 1e-10:
+            return  # too uncertain, may not be a traffic light
 
-        # 1: Green, 2: Red, 4: Yellow, 0: Unknown
-        msg = TrafficLightState()
-        msg.state = result if result in [1, 2, 4] else 0
+        state = result if result in [1, 2, 4] else 0
+        if self.last_state == state:
+            # 1: Green, 2: Red, 4: Yellow, 0: Unknown
+            msg = TrafficLightState()
+            msg.state = state
+            self.traffic_light_publisher.publish(msg)
+        else:
+            self.last_state = state
 
-        self.traffic_light_publisher.publish(msg)
+        # Automatically invalidates state (state=0) in auto_invalidate_state()
+        if state != 0:
+            self.last_info_time = datetime.now()
 
     def run(self):
         self.spin()
diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py
index e9681a4c..938ee669 100755
--- a/code/perception/src/vision_node.py
+++ b/code/perception/src/vision_node.py
@@ -191,14 +191,19 @@ def process_traffic_lights(self, prediction, cv_image, image_header):
         indices = (prediction.boxes.cls == 9).nonzero().squeeze().cpu().numpy()
         indices = np.asarray([indices]) if indices.size == 1 else indices
 
-        min_x = 550
-        max_x = 700
-        min_prob = 0.35
+        max_y = 360  # middle of image
+        min_prob = 0.30
 
         for index in indices:
             box = prediction.boxes.cpu().data.numpy()[index]
 
-            if box[0] < min_x or box[2] > max_x or box[4] < min_prob:
+            if box[4] < min_prob:
+                continue
+
+            if (box[2] - box[0]) * 1.5 > box[3] - box[1]:
+                continue  # ignore horizontal boxes
+
+            if box[1] > max_y:
                 continue
 
             box = box[0:4].astype(int)
diff --git a/code/planning/__init__.py b/code/planning/__init__.py
index e69de29b..6579338c 100755
--- a/code/planning/__init__.py
+++ b/code/planning/__init__.py
@@ -0,0 +1 @@
+import behavior_agent
diff --git a/code/planning/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py
index e69de29b..330a8f1d 100755
--- a/code/planning/src/behavior_agent/behaviours/__init__.py
+++ b/code/planning/src/behavior_agent/behaviours/__init__.py
@@ -0,0 +1,3 @@
+from . import intersection, lane_change, 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 41e75530..df88e762 100755
--- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py
+++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py
@@ -20,7 +20,7 @@ def convert_to_ms(speed):
 int_app_init = Behavior("int_app_init", convert_to_ms(30.0))
 
 # No Traffic Light or Sign -> stop dynamically at Stopline
-int_app_no_sign = Behavior("int_app_no_sign", -2)
+int_app_to_stop = Behavior("int_app_to_stop", -2)
 
 int_app_green = Behavior("int_app_green", convert_to_ms(30.0))
 
@@ -30,11 +30,7 @@ def convert_to_ms(speed):
 
 # Enter
 
-int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0))
-
-int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0))
-
-int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0))
+int_enter = Behavior("int_enter", convert_to_ms(50.0))
 
 # Exit
 
@@ -45,13 +41,16 @@ def convert_to_ms(speed):
 
 # Approach
 
-lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0))
+lc_app_init = Behavior("lc_app_init", convert_to_ms(30.0))
 
 
 # TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
-lc_app_blocked = Behavior("lc_app_blocked", 0.5)
+lc_app_blocked = Behavior("lc_app_blocked", -2)
+
+lc_app_free = Behavior("lc_app_free", convert_to_ms(20))
 
 # Wait
+lc_wait = Behavior("lc_wait", 0)
 
 # Has a publisher but doesnt publish anything ??
 
diff --git a/code/planning/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py
index 22697f27..7e152726 100755
--- a/code/planning/src/behavior_agent/behaviours/intersection.py
+++ b/code/planning/src/behavior_agent/behaviours/intersection.py
@@ -15,6 +15,18 @@ def convert_to_ms(speed):
     return speed / 3.6
 
 
+# 1: Green, 2: Red, 4: Yellow, 0: Unknown
+def get_color(state):
+    if state == 1:
+        return "green"
+    elif state == 2:
+        return "red"
+    elif state == 4:
+        return "yellow"
+    else:
+        return ""
+
+
 class Approach(py_trees.behaviour.Behaviour):
     """
     This behaviour is executed when the ego vehicle is in close proximity of
@@ -88,12 +100,10 @@ def update(self):
                  detected.
         """
         # Update Light Info
-        light_status_msg = self.blackboard.get("/paf/hero/traffic_light")
+        light_status_msg = self.blackboard.get(
+            "/paf/hero/Center/traffic_light_state")
         if light_status_msg is not None:
-            self.traffic_light_status = light_status_msg.color
-            rospy.loginfo(f"Light Status: {self.traffic_light_status}")
-            self.traffic_light_distance = light_status_msg.distance
-            rospy.loginfo(f"Light distance: {self.traffic_light_distance}")
+            self.traffic_light_status = get_color(light_status_msg.state)
             self.traffic_light_detected = True
 
         # Update stopline Info
@@ -111,15 +121,13 @@ def update(self):
         # calculate virtual stopline
         if self.stopline_distance != np.inf and self.stopline_detected:
             self.virtual_stopline_distance = self.stopline_distance
-        elif self.traffic_light_detected:
-            self.virtual_stopline_distance = self.traffic_light_distance
         elif self.stop_sign_detected:
             self.virtual_stopline_distance = self.stop_distance
         else:
             self.virtual_stopline_distance = 0.0
 
         rospy.loginfo(f"Stopline distance: {self.virtual_stopline_distance}")
-        target_distance = 3.0
+        target_distance = 5.0
         # stop when there is no or red/yellow traffic light or a stop sign is
         # detected
         if self.traffic_light_status == '' \
@@ -129,7 +137,7 @@ def update(self):
                     not self.traffic_light_detected):
 
             rospy.loginfo("slowing down!")
-            self.curr_behavior_pub.publish(bs.int_app_no_sign.name)
+            self.curr_behavior_pub.publish(bs.int_app_to_stop.name)
 
         # approach slowly when traffic light is green as traffic lights are
         # higher priority than traffic signs this behavior is desired
@@ -170,7 +178,6 @@ def update(self):
         if self.virtual_stopline_distance < target_distance and \
            not self.stopline_detected:
             rospy.loginfo("Leave intersection!")
-            # self.update_local_path(leave_intersection=True)
             return py_trees.common.Status.SUCCESS
         else:
             return py_trees.common.Status.RUNNING
@@ -221,6 +228,8 @@ def setup(self, timeout):
                                                  "curr_behavior", String,
                                                  queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
+        self.red_light_flag = False
+        self.green_light_counter = 0
         return True
 
     def initialise(self):
@@ -234,6 +243,8 @@ def initialise(self):
         :return: True
         """
         rospy.loginfo("Wait Intersection")
+        self.red_light_flag = False
+        self.green_light_counter = 0
         return True
 
     def update(self):
@@ -251,34 +262,49 @@ def update(self):
                  py_trees.common.Status.SUCCESS, if the traffic light switched
                  to green or no traffic light is detected
         """
-        light_status_msg = self.blackboard.get("/paf/hero/traffic_light")
-        # intersection_clear_msg = self.blackboard.get("/paf/hero/"
-        #                                            "intersection_clear")
-        distance_lidar = self.blackboard.get("/carla/hero/LIDAR_range")
+        light_status_msg = self.blackboard.get(
+            "/paf/hero/Center/traffic_light_state")
+
+        lidar_data = self.blackboard.get("/carla/hero/LIDAR_range")
+
         intersection_clear = True
-        if distance_lidar is not None:
+        if lidar_data is not None:
             # if distance smaller than 10m, intersection is blocked
-            if distance_lidar.min_range < 10.0:
+            if lidar_data.data < 10.0:
                 intersection_clear = False
             else:
                 intersection_clear = True
-        # if intersection_clear_msg is not None:
-        #    intersection_clear = intersection_clear_msg.data
-        # else:
-        #    intersection_clear = False
 
         if light_status_msg is not None:
-            traffic_light_status = light_status_msg.color
+            traffic_light_status = get_color(light_status_msg.state)
             if traffic_light_status == "red" or \
                     traffic_light_status == "yellow":
+                self.red_light_flag = True
+                self.green_light_counter = 0
                 rospy.loginfo(f"Light Status: {traffic_light_status}")
                 self.curr_behavior_pub.publish(bs.int_wait.name)
                 return py_trees.common.Status.RUNNING
-            else:
+            elif self.green_light_counter < 6 and \
+                    traffic_light_status == "green":
+                self.green_light_counter += 1
+                rospy.loginfo(f"Light Counter: {self.green_light_counter}")
+                return py_trees.common.Status.RUNNING
+            elif self.red_light_flag and traffic_light_status != "green":
+                rospy.loginfo(f"Light Status: {traffic_light_status}"
+                              "-> prev was red")
+                return py_trees.common.Status.RUNNING
+            elif self.green_light_counter >= 6 and \
+                    traffic_light_status == "green":
                 rospy.loginfo(f"Light Status: {traffic_light_status}")
                 return py_trees.common.Status.SUCCESS
-        elif not intersection_clear:
+            else:
+                rospy.loginfo(f"Light Status: {traffic_light_status}"
+                              "-> No Traffic Light detected")
+
+        # Check clear if no traffic light is detected
+        if not intersection_clear:
             rospy.loginfo("Intersection blocked")
+            self.curr_behavior_pub.publish(bs.int_wait.name)
             return py_trees.common.Status.RUNNING
         else:
             rospy.loginfo("Intersection clear")
@@ -343,18 +369,17 @@ def initialise(self):
         the intersection.
         """
         rospy.loginfo("Enter Intersection")
-        light_status_msg = self.blackboard.get("/paf/hero/traffic_light")
+
+        light_status_msg = self.blackboard.get(
+            "/paf/hero/Center/traffic_light_state")
         if light_status_msg is None:
-            self.curr_behavior_pub.publish(bs.int_enter_no_light.name)
+            self.curr_behavior_pub.publish(bs.int_enter.name)
             return True
-        else:
-            traffic_light_status = light_status_msg.color
 
-        if traffic_light_status == "":
-            self.curr_behavior_pub.publish(bs.int_enter_empty_str.name)
-        else:
-            rospy.loginfo(f"Light Status: {traffic_light_status}")
-            self.curr_behavior_pub.publish(bs.int_enter_light.name)
+        traffic_light_status = get_color(light_status_msg.state)
+
+        rospy.loginfo(f"Light Status: {traffic_light_status}")
+        self.curr_behavior_pub.publish(bs.int_enter.name)
 
     def update(self):
         """
@@ -380,7 +405,7 @@ def update(self):
             # not next_waypoint_msg.isStopLine:
         if next_waypoint_msg.distance < 5:
             rospy.loginfo("Drive through intersection!")
-            # self.update_local_path(leave_intersection=True)
+            self.curr_behavior_pub.publish(bs.int_enter.name)
             return py_trees.common.Status.RUNNING
         else:
             return py_trees.common.Status.SUCCESS
@@ -444,10 +469,7 @@ def initialise(self):
         :return: True
         """
         rospy.loginfo("Leave Intersection")
-        street_speed_msg = self.blackboard.get("/paf/hero/speed_limit")
-        if street_speed_msg is not None:
-            # self.curr_behavior_pub.publish(street_speed_msg.data)
-            self.curr_behavior_pub.publish(bs.int_exit.name)
+        self.curr_behavior_pub.publish(bs.int_exit.name)
         return True
 
     def update(self):
diff --git a/code/planning/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py
index 1a3b7015..cd9833cc 100755
--- a/code/planning/src/behavior_agent/behaviours/lane_change.py
+++ b/code/planning/src/behavior_agent/behaviours/lane_change.py
@@ -1,8 +1,6 @@
 import py_trees
 import numpy as np
 from std_msgs.msg import String
-# from nav_msgs.msg import Odometry
-# from custom_carla_msgs.srv import UpdateLocalPath
 
 import rospy
 
@@ -48,9 +46,6 @@ def setup(self, timeout):
         self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
                                                  "curr_behavior",
                                                  String, queue_size=1)
-        # rospy.wait_for_service('update_local_path') # TODO is this necessary?
-        # self.update_local_path =
-        # rospy.ServiceProxy("update_local_path", UpdateLocalPath)
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -65,12 +60,12 @@ def initialise(self):
         stop line, stop signs and the traffic light.
         """
         rospy.loginfo("Approaching Change")
-        # self.update_local_path(approach_intersection=True)
         self.start_time = rospy.get_time()
         self.change_detected = False
         self.change_distance = np.inf
         self.virtual_change_distance = np.inf
         self.curr_behavior_pub.publish(bs.lc_app_init.name)
+        self.blocked = False
 
     def update(self):
         """
@@ -101,23 +96,25 @@ def update(self):
             self.virtual_change_distance = self.change_distance
 
         # slow down before lane change
-        if self.virtual_change_distance < 15.0:
-            if self.change_option == 5:
-                distance_lidar = self.blackboard. \
-                    get("/carla/hero/LIDAR_range_rear_left")
-            elif self.change_option == 6:
-                distance_lidar = self.blackboard. \
-                    get("/carla/hero/LIDAR_range_rear_right")
-            else:
-                distance_lidar = None
-
-            if distance_lidar is not None and distance_lidar.min_range > 15.0:
+        # if self.virtual_change_distance < 15.0:
+        #     if self.change_option == 5:
+        #         distance_lidar = self.blackboard. \
+        #             get("/carla/hero/LIDAR_range_rear_left")
+        #     elif self.change_option == 6:
+        #         distance_lidar = self.blackboard. \
+        #             get("/carla/hero/LIDAR_range_rear_right")
+        #     else:
+        #         distance_lidar = None
+
+            distance_lidar = 20  # Remove and adjust to check for cars behind
+
+            if distance_lidar is not None and distance_lidar > 15.0:
                 rospy.loginfo("Change is free not slowing down!")
-                # self.update_local_path(leave_intersection=True)
-                return py_trees.common.Status.SUCCESS
+                self.curr_behavior_pub.publish(bs.lc_app_free.name)
+                self.blocked = False
             else:
                 rospy.loginfo("Change blocked slowing down")
-                self.curr_behavior_pub.publish(bs.lc_app_blocked.name)
+                self.blocked = True
 
         # get speed
         speedometer = self.blackboard.get("/carla/hero/Speed")
@@ -126,24 +123,20 @@ def update(self):
         else:
             rospy.logwarn("no speedometer connected")
             return py_trees.common.Status.RUNNING
-        if self.virtual_change_distance > 5.0:
+        if self.virtual_change_distance > 5 and self.blocked:
             # too far
-            print("still approaching")
+            rospy.loginfo("still approaching")
+            self.curr_behavior_pub.publish(bs.lc_app_blocked.name)
             return py_trees.common.Status.RUNNING
         elif speed < convert_to_ms(2.0) and \
-                self.virtual_change_distance < 5.0:
+                self.virtual_change_distance < 5.0 and self.blocked:
             # stopped
-            print("stopped")
+            rospy.loginfo("stopped")
             return py_trees.common.Status.SUCCESS
         elif speed > convert_to_ms(5.0) and \
-                self.virtual_change_distance < 3.5:
+                self.virtual_change_distance < 3.5 and not self.blocked:
             # running over line
             return py_trees.common.Status.SUCCESS
-
-        if self.virtual_change_distance < 5 and not self.change_detected:
-            rospy.loginfo("Leave Change!")
-            # self.update_local_path(leave_intersection=True)
-            return py_trees.common.Status.SUCCESS
         else:
             return py_trees.common.Status.RUNNING
 
@@ -204,8 +197,6 @@ def initialise(self):
             Any initialisation you need before putting your behaviour to work.
         This just prints a state status message.
         """
-        self.old_ro = self.blackboard.\
-            get("/paf/hero/lane_change_distance")
         rospy.loginfo("Wait Change")
         return True
 
@@ -225,32 +216,38 @@ def update(self):
                  to green or no traffic light is detected
         """
 
-        lcd = self.blackboard.\
-            get("/paf/hero/lane_change_distance")
+        speedometer = self.blackboard.get("/carla/hero/Speed")
+        if speedometer is not None:
+            speed = speedometer.speed
+        else:
+            rospy.logwarn("no speedometer connected")
+            return py_trees.common.Status.RUNNING
 
-        if self.old_ro.distance < lcd.distance + 1 or \
-           lcd.roadOption != self.old_ro.roadOption:
+        if speed > convert_to_ms(10):
+            rospy.loginfo("Forward to enter")
             return py_trees.common.Status.SUCCESS
-        self.old_ro = lcd
-        road_option = lcd.roadOption
-        if road_option == 5:
-            distance_lidar = self.blackboard. \
-                get("/carla/hero/LIDAR_range_rear_left")
-        elif road_option == 6:
-            distance_lidar = self.blackboard. \
-                get("/carla/hero/LIDAR_range_rear_right")
-        else:
-            distance_lidar = None
+
+        # if road_option == 5:
+        #     distance_lidar = self.blackboard. \
+        #         get("/carla/hero/LIDAR_range_rear_left")
+        # elif road_option == 6:
+        #     distance_lidar = self.blackboard. \
+        #         get("/carla/hero/LIDAR_range_rear_right")
+        # else:
+        #     distance_lidar = None
+
+        distance_lidar = 20  # Remove to wait
 
         change_clear = False
         if distance_lidar is not None:
             # if distance smaller than 15m, change is blocked
-            if distance_lidar.min_range < 15.0:
+            if distance_lidar < 15.0:
                 change_clear = False
             else:
                 change_clear = True
         if not change_clear:
             rospy.loginfo("Change blocked")
+            self.curr_behavior_pub.publish(bs.lc_wait.name)
             return py_trees.common.Status.RUNNING
         else:
             rospy.loginfo("Change clear")
@@ -301,9 +298,6 @@ def setup(self, timeout):
         self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
                                                  "curr_behavior", String,
                                                  queue_size=1)
-        # rospy.wait_for_service('update_local_path')
-        # self.update_local_path = rospy.ServiceProxy("update_local_path",
-        # UpdateLocalPath)
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -345,7 +339,6 @@ def update(self):
             # not next_waypoint_msg.isStopLine:
         if next_waypoint_msg.distance < 5:
             rospy.loginfo("Drive on the next lane!")
-            # self.update_local_path(leave_intersection=True)
             return py_trees.common.Status.RUNNING
         else:
             return py_trees.common.Status.SUCCESS
@@ -408,10 +401,8 @@ def initialise(self):
         the street speed limit.
         """
         rospy.loginfo("Leave Change")
-        street_speed_msg = self.blackboard.get("/paf/hero/speed_limit")
-        if street_speed_msg is not None:
-            # self.curr_behavior_pub.publish(street_speed_msg.data)
-            self.curr_behavior_pub.publish(bs.lc_exit.name)
+
+        self.curr_behavior_pub.publish(bs.lc_exit.name)
         return True
 
     def update(self):
diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py
index 81ce9eb1..86f3c3b1 100755
--- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py
+++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py
@@ -6,11 +6,10 @@
 
 from std_msgs.msg import Float32, Bool, Float32MultiArray
 from carla_msgs.msg import CarlaSpeedometer
-from sensor_msgs.msg import Range
 from geometry_msgs.msg import PoseStamped
 
-from mock.msg import Traffic_light, Stop_sign
-from perception.msg import Waypoint, LaneChange
+from mock.msg import Stop_sign
+from perception.msg import Waypoint, LaneChange, TrafficLightState
 
 """
 Source: https://github.com/ll7/psaf2
@@ -31,19 +30,15 @@ def create_node(role_name):
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/waypoint_distance", 'msg': Waypoint,
          'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE},
-        {'name': f"/paf/{role_name}/intersection_clear",
-         'msg': Bool, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/stop_sign", 'msg': Stop_sign,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
-        {'name': f"/paf/{role_name}/traffic_light", 'msg': Traffic_light,
+        {'name':
+         f"/paf/{role_name}/Center/traffic_light_state",
+         'msg': TrafficLightState,
          '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': Range,
-         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
-        {'name': f"/carla/{role_name}/LIDAR_range_rear_right", 'msg': Range,
-         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
-        {'name': f"/carla/{role_name}/LIDAR_range_rear_left", 'msg': Range,
+        {'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},
diff --git a/code/planning/src/global_planner/global_route_short.txt b/code/planning/src/global_planner/global_route_short.txt
index 8d7b0446..38397174 100644
--- a/code/planning/src/global_planner/global_route_short.txt
+++ b/code/planning/src/global_planner/global_route_short.txt
@@ -4,7 +4,7 @@ header:
     secs: 1674304465
     nsecs: 171659708
   frame_id: "/map"
-road_options: [4, 2, 4, 3, 4]
+road_options: [4, 2, 4, 5, 3, 4]
 poses:
   -
     position:
@@ -12,48 +12,58 @@ poses:
       y: -5445.88720703125
       z: 371.9093017578125
     orientation:
-      x: 0.07026641552209804
-      y: 0.038563409597558
-      z: -0.8738328670313198
-      w: 0.47957441006136947
+      x: 0
+      y: 0
+      z: 0
+      w: 0
   -
     position:
       x: 983.2598876953125
       y: -5568.28662109375
       z: 370.7098388671875
     orientation:
-      x: 0.12320969016364677
-      y: 0.06761958345056983
-      z: -0.8679519692380901
-      w: 0.47634687285576993
+      x: 0
+      y: 0
+      z: 0
+      w: 0
   -
     position:
       x: 974.2578735351562
       y: -5575.4443359375
       z: 370.6752014160156
     orientation:
-      x: 0.7541724368151881
-      y: -0.3663661511612136
-      z: 0.490197382664243
-      w: 0.2381308565908366
+      x: 0
+      y: 0
+      z: 0
+      w: 0
+  -
+      position:
+      x: 950.2578735351562
+      y: -5575.4443359375
+      z: 370.6752014160156
+    orientation:
+      x: 0
+      y: 0
+      z: 0
+      w: 0
   -
     position:
       x: 805.0078735351562
       y: -5575.51806640625
       z: 370.8327941894531
     orientation:
-      x: 0.7208694596446681
-      y: -0.35018804258474884
-      z: 0.5379746839529138
-      w: 0.26134038418895195
+      x: 0
+      y: 0
+      z: 0
+      w: 0
   -
     position:
       x: 780.8638305664062
       y: -5575.548828125
       z: 370.8050537109375
     orientation:
-      x: 0.6993724403633815
-      y: -0.292214697883295
-      z: 0.6018708299183292
-      w: 0.2514761700331955
+      x: 0
+      y: 0
+      z: 0
+      w: 0
 ---
\ No newline at end of file
diff --git a/code/planning/src/local_planner/behavior_speed.py b/code/planning/src/local_planner/behavior_speed.py
deleted file mode 100755
index e67bdad1..00000000
--- a/code/planning/src/local_planner/behavior_speed.py
+++ /dev/null
@@ -1,69 +0,0 @@
-
-from collections import namedtuple
-
-
-def convert_to_ms(speed):
-    return speed / 3.6
-
-
-Behavior = namedtuple("Behavior", ("name", "speed"))
-
-# Changed target_speed_pub to curr_behavior_pub
-
-# Leave Parking space
-
-parking = Behavior("parking", convert_to_ms(30.0))
-# Intersection - Behaviors
-
-# Approach
-
-int_app_init = Behavior("int_app_init", convert_to_ms(30.0))
-
-# No Traffic Light or Sign -> stop dynamically at Stopline
-int_app_no_sign = Behavior("int_app_no_sign", -2)
-
-int_app_green = Behavior("int_app_green", convert_to_ms(30.0))
-
-# Wait
-
-int_wait = Behavior("int_wait", 0)
-
-# Enter
-
-int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0))
-
-int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0))
-
-int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0))
-
-# Exit
-
-int_exit = Behavior("int_exit", -1)  # Get SpeedLimit dynamically
-
-
-# Lane Change
-
-# Approach
-
-lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0))
-
-
-# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
-lc_app_blocked = Behavior("lc_app_blocked", 0.5)
-
-# Wait
-
-# Has a publisher but doesnt publish anything ??
-
-# Enter
-
-lc_enter_init = Behavior("lc_enter_init", convert_to_ms(20.0))
-
-# Exit
-
-lc_exit = Behavior("lc_exit", -1)  # Get SpeedLimit dynamically
-
-
-# Cruise
-
-cruise = Behavior("Cruise", -1)
diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py
index 952687ab..f6b05087 100755
--- a/code/planning/src/local_planner/motion_planning.py
+++ b/code/planning/src/local_planner/motion_planning.py
@@ -2,14 +2,23 @@
 # import rospy
 # 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 nav_msgs.msg import Path
+from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
+
+
 import numpy as np
 
-# from behavior_agent.msg import BehaviorSpeed
+from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \
+    import run_fot
 from perception.msg import Waypoint, LaneChange
-import behavior_speed as bs
+import planning  # noqa: F401
+from behavior_agent.behaviours import behavior_speed as bs
 
 # from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
 # from carla_msgs.msg import CarlaRoute   # , CarlaWorldInfo
@@ -33,15 +42,30 @@ class MotionPlanning(CompatibleNode):
     def __init__(self):
         super(MotionPlanning, self).__init__('MotionPlanning')
         self.role_name = self.get_param("role_name", "hero")
-        self.control_loop_rate = self.get_param("control_loop_rate", 0.5)
+        self.control_loop_rate = self.get_param("control_loop_rate", 0.1)
 
         self.target_speed = 0.0
         self.__curr_behavior = None
         self.__acc_speed = 0.0
         self.__stopline = None  # (Distance, isStopline)
         self.__change_point = None  # (Distance, isLaneChange, roadOption)
+        self.counter = 0
+        self.speed_list = []
+        self.__first_trajectory = None
+        self.__corners = None
+        self.__in_corner = False
 
         # Subscriber
+        self.trajectory_sub = self.new_subscription(
+            Path,
+            f"/paf/{self.role_name}/trajectory",
+            self.__save_trajectory,
+            qos_profile=1)
+        self.current_pos_sub = self.new_subscription(
+            PoseStamped,
+            f"/paf/{self.role_name}/current_pos",
+            self.__set_current_pos,
+            qos_profile=1)
         self.curr_behavior_sub: Subscriber = self.new_subscription(
             String,
             f"/paf/{self.role_name}/curr_behavior",
@@ -71,6 +95,10 @@ def __init__(self):
             qos_profile=1)
 
         # Publisher
+        # self.traj_pub: Publisher = self.new_publisher(
+        #     Path,
+        #     f"/paf/{self.role_name}/trajectory",
+        #     qos_profile=1)
         self.velocity_pub: Publisher = self.new_publisher(
             Float32,
             f"/paf/{self.role_name}/target_velocity",
@@ -83,6 +111,196 @@ def __init__(self):
             qos_profile=1)
 
         self.logdebug("MotionPlanning started")
+        self.published = False
+        self.current_pos = None
+
+    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:
+            data (PoseStamped): current position
+        """
+        self.current_pos = np.array([data.pose.position.x,
+                                    data.pose.position.y])
+
+    def __set_trajectory(self, data: Path):
+        """get current trajectory global planning
+
+        Args:
+            data (Path): Trajectory waypoints
+        """
+        if len(data.poses) and not self.published > 0:
+            self.published = True
+            np_array = np.array(data.poses)
+            selection = np_array[5:17]
+            waypoints = self.convert_pose_to_array(selection)
+            self.logerr(waypoints)
+            obs = np.array(
+                [[waypoints[5][0]-0.5, waypoints[5][1], waypoints[5][0]+0.5,
+                 waypoints[5][1]-2]])
+            initial_conditions = {
+                'ps': 0,
+                'target_speed': self.__acc_speed,
+                'pos': np.array([983.5807552562393, 5370.014637890163]),
+                'vel': np.array([5, 1]),
+                'wp': waypoints,
+                'obs': obs
+            }
+            hyperparameters = {
+                "max_speed": 25.0,
+                "max_accel": 15.0,
+                "max_curvature": 15.0,
+                "max_road_width_l": 3.0,
+                "max_road_width_r": 0.5,
+                "d_road_w": 0.5,
+                "dt": 0.2,
+                "maxt": 20.0,
+                "mint": 6.0,
+                "d_t_s": 0.5,
+                "n_s_sample": 2.0,
+                "obstacle_clearance": 0.1,
+                "kd": 1.0,
+                "kv": 0.1,
+                "ka": 0.1,
+                "kj": 0.1,
+                "kt": 0.1,
+                "ko": 0.1,
+                "klat": 1.0,
+                "klon": 1.0,
+                "num_threads": 0,  # set 0 to avoid using threaded algorithm
+            }
+            result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \
+                speeds_y, misc, costs, success = run_fot(initial_conditions,
+                                                         hyperparameters)
+            if success:
+                print("Success!")
+                print("result_x: ", result_x)
+                print("result_y: ", result_y)
+                result = []
+                for i in range(len(result_x)):
+                    position = Point(result_x[i], result_y[i], 703)
+                    quat = tf.transformations.quaternion_from_euler(0, 0,
+                                                                    iyaw[i])
+                    orientation = Quaternion(x=quat[0], y=quat[1],
+                                             z=quat[2], w=quat[3])
+                    pose = Pose(position, orientation)
+                    pos = PoseStamped()
+                    pos.header.stamp = rospy.Time.now()
+                    pos.header.frame_id = "global"
+                    pos.pose = pose
+                    result.append(PoseStamped)
+                path = Path()
+                path.header.stamp = rospy.Time.now()
+                path.path_backup.header.frame_id = "global"
+                path.poses = list(np_array[:5]) + result + list(np_array[17:])
+                self.traj_pub.publish(path)
+
+    def __calc_corner_points(self):
+        coords = self.convert_pose_to_array(np.array(self.__first_trajectory))
+        x_values = np.array([point[0] for point in coords])
+        y_values = np.array([point[1] for point in coords])
+
+        angles = np.arctan2(np.diff(y_values), np.diff(x_values))
+        angles = np.rad2deg(angles)
+        angles[angles > 0] -= 360  # Convert for angles between 0 - 360 degree
+
+        threshold = 1  # in degree
+        curve_change_indices = np.where(np.abs(np.diff(angles)) > threshold)[0]
+
+        sublist = self.create_sublists(curve_change_indices, proximity=5)
+
+        coords_of_curve = [coords[i] for i in sublist]
+
+        return coords_of_curve
+
+    def create_sublists(self, points, proximity=5):
+        sublists = []
+        current_sublist = []
+
+        for point in points:
+            if not current_sublist:
+                current_sublist.append(point)
+            else:
+                last_point = current_sublist[-1]
+                distance = abs(point - last_point)
+
+                if distance <= proximity:
+                    current_sublist.append(point)
+                else:
+                    sublists.append(current_sublist)
+                    current_sublist = [point]
+        if current_sublist:
+            sublists.append(current_sublist)
+
+        filtered_list = [in_list for in_list in sublists if len(in_list) > 1]
+
+        return filtered_list
+
+    def get_cornering_speed(self):
+        corner = self.__corners[0]
+        pos = self.current_pos
+
+        def euclid_dist(vector1, vector2):
+            point1 = np.array(vector1)
+            point2 = np.array(vector2)
+
+            diff = point2 - point1
+            sum_sqrt = np.dot(diff.T, diff)
+            return np.sqrt(sum_sqrt)
+
+        def map_corner(dist):
+            if dist < 8:  # lane_change
+                return 8
+            elif dist < 25:
+                return 6
+            elif dist < 50:
+                return 7
+            else:
+                8
+
+        distance_corner = 0
+        for i in range(len(corner) - 1):
+            distance_corner += euclid_dist(corner[i], corner[i + 1])
+        # self.logerr(distance_corner)
+
+        if self.__in_corner:
+            distance_end = euclid_dist(pos, corner[0])
+            if distance_end > distance_corner + 2:
+                self.__in_corner = False
+                self.__corners.pop(0)
+                self.loginfo("End Corner")
+                return self.__get_speed_cruise()
+            else:
+                return map_corner(distance_corner)
+
+        distance_start = euclid_dist(pos, corner[0])
+        if distance_start < 3:
+            self.__in_corner = True
+            self.loginfo("Start Corner")
+            return map_corner(distance_corner)
+        else:
+            return self.__get_speed_cruise()
+
+    def convert_pose_to_array(self, poses: np.array):
+        """convert pose array to numpy array
+
+        Args:
+            poses (np.array): pose array
+
+        Returns:
+            np.array: numpy array
+        """
+        result_array = np.empty((len(poses), 2))
+        for pose in range(len(poses)):
+            result_array[pose] = np.array([poses[pose].pose.position.x,
+                                           poses[pose].pose.position.y])
+        return result_array
 
     def __check_emergency(self, data: Bool):
         """If an emergency stop is needed first check if we are
@@ -103,7 +321,12 @@ def update_target_speed(self, acc_speed, behavior):
         else:
             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)
+        # self.target_speed = min(self.target_speed, 8)
         self.velocity_pub.publish(self.target_speed)
+        # self.logerr(f"Speed: {self.target_speed}")
+        # self.speed_list.append(self.target_speed)
 
     def __set_acc_speed(self, data: Float32):
         self.__acc_speed = data.data
@@ -123,9 +346,7 @@ def __set_change_point(self, data: LaneChange):
     def get_speed_by_behavior(self, behavior: str) -> float:
         speed = 0.0
         split = "_"
-        self.loginfo("get speed")
         short_behavior = behavior.partition(split)[0]
-        self.loginfo("short behavior: " + str(short_behavior))
         if short_behavior == "int":
             speed = self.__get_speed_intersection(behavior)
         elif short_behavior == "lc":
@@ -142,18 +363,14 @@ def __get_speed_intersection(self, behavior: str) -> float:
             speed = bs.int_app_init.speed
         elif behavior == bs.int_app_green.name:
             speed = bs.int_app_green.speed
-        elif behavior == bs.int_app_no_sign.name:
+        elif behavior == bs.int_app_to_stop.name:
             speed = self.__calc_speed_to_stop_intersection()
         elif behavior == bs.int_wait.name:
             speed == bs.int_wait.speed
-        elif behavior == bs.int_enter_no_light:
-            speed = bs.int_enter_no_light.speed
-        elif behavior == bs.int_enter_empty_str.name:
-            speed = bs.int_enter_empty_str.speed
-        elif behavior == bs.int_enter_light.name:
-            speed == bs.int_enter_light.speed
+        elif behavior == bs.int_enter.name:
+            speed = bs.int_enter.speed
         elif behavior == bs.int_exit:
-            speed = bs.int_exit.speed
+            speed = self.__get_speed_cruise()
 
         return speed
 
@@ -162,7 +379,11 @@ def __get_speed_lanechange(self, behavior: str) -> float:
         if behavior == bs.lc_app_init.name:
             speed = bs.lc_app_init.speed
         elif behavior == bs.lc_app_blocked.name:
-            speed = bs.lc_app_blocked.speed  # calc_speed_to_stop_lanechange()
+            speed = self.__calc_speed_to_stop_lanechange()
+        elif behavior == bs.lc_app_free.name:
+            speed = bs.lc_app_free.speed
+        elif behavior == bs.lc_wait.name:
+            speed = bs.lc_wait.speed
         elif behavior == bs.lc_enter_init.name:
             speed = bs.lc_enter_init.speed
         elif behavior == bs.lc_exit.name:
@@ -174,36 +395,40 @@ def __get_speed_cruise(self) -> float:
         return self.__acc_speed
 
     def __calc_speed_to_stop_intersection(self) -> float:
-        target_distance = 3.0
+        target_distance = 5.0
         virtual_stopline_distance = self.__calc_virtual_stopline()
         # calculate speed needed for stopping
         v_stop = max(convert_to_ms(10.),
                      convert_to_ms((virtual_stopline_distance / 30)
                                    * 50))
-        if v_stop > convert_to_ms(50.0):
-            v_stop = convert_to_ms(50.0)
+        if v_stop > bs.int_app_init.speed:
+            v_stop = bs.int_app_init.speed
         if virtual_stopline_distance < target_distance:
             v_stop = 0.0
+        return v_stop
 
     # TODO: Find out purpose
     def __calc_speed_to_stop_lanechange(self) -> float:
-        if self.__change_point[0] != np.inf and self.__change_point[1]:
-            stopline = self.__change_point[0]
-        else:
-            return 100
+        stopline = self.__calc_virtual_change_point()
 
-        v_stop = max(convert_to_ms(5.),
-                     convert_to_ms((stopline / 30) ** 1.5
+        v_stop = max(convert_to_ms(10.),
+                     convert_to_ms((stopline / 30)
                                    * 50))
-        if v_stop > convert_to_ms(50.0):
-            v_stop = convert_to_ms(30.0)
+        if v_stop > bs.lc_app_init.speed:
+            v_stop = bs.lc_app_init.speed
+        if stopline < 5.0:
+            v_stop = 0.0
         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]
+        else:
+            return 0.0
+
     def __calc_virtual_stopline(self) -> float:
         if self.__stopline[0] != np.inf and self.__stopline[1]:
             return self.__stopline[0]
-        elif self.traffic_light_detected:
-            return self.traffic_light_distance
         else:
             return 0.0
 
@@ -214,7 +439,11 @@ def run(self):
         """
 
         def loop(timer_event=None):
-            self.update_target_speed(self.__acc_speed, self.__curr_behavior)
+            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.new_timer(self.control_loop_rate, loop)
         self.spin()
diff --git a/code/planning/src/local_planner/test_traj.py b/code/planning/src/local_planner/test_traj.py
new file mode 100644
index 00000000..05a00c73
--- /dev/null
+++ b/code/planning/src/local_planner/test_traj.py
@@ -0,0 +1,68 @@
+import numpy as np
+
+from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \
+    import run_fot
+
+import matplotlib.pyplot as plt
+
+
+wp = np.r_[[np.full((50), 983.5889666959667)],
+           [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T
+obs = np.array([[983.568124548765, 5384.0219828457075,
+                 983.628124548765, 5386.0219828457075]])
+initial_conditions = {
+    'ps': 0,
+    'target_speed': 6,
+    'pos': np.array([983.5807552562393, 5370.014637890163]),
+    'vel': np.array([5, 1]),
+    'wp': wp,
+    'obs': obs
+}
+
+hyperparameters = {
+    "max_speed": 25.0,
+    "max_accel": 15.0,
+    "max_curvature": 20.0,
+    "max_road_width_l": 3.0,
+    "max_road_width_r": 0,
+    "d_road_w": 0.5,
+    "dt": 0.2,
+    "maxt": 20.0,
+    "mint": 6.0,
+    "d_t_s": 0.5,
+    "n_s_sample": 2.0,
+    "obstacle_clearance": 2,
+    "kd": 1.0,
+    "kv": 0.1,
+    "ka": 0.1,
+    "kj": 0.1,
+    "kt": 0.1,
+    "ko": 0.1,
+    "klat": 1.0,
+    "klon": 1.0,
+    "num_threads": 0,  # set 0 to avoid using threaded algorithm
+}
+
+result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \
+            speeds_y, misc, costs, success = run_fot(initial_conditions,
+                                                     hyperparameters)
+
+if success:
+    print("Success!")
+    print("result_x: ", result_x)
+    print("result_y: ", result_y)
+    print("yaw!", iyaw)
+    fig, ax = plt.subplots(1, 2)
+
+    ax[0].scatter(wp[:, 0], wp[:, 1], label="original")
+    ax[0].scatter([obs[0, 0], obs[0, 2]],
+                  [obs[0, 1], obs[0, 3]], label="object")
+    ax[0].set_xticks([obs[0, 0], obs[0, 2]])
+    ax[1].scatter(result_x, result_y, label="frenet")
+    ax[1].scatter([obs[0, 0], obs[0, 2]],
+                  [obs[0, 1], obs[0, 3]], label="object")
+    ax[1].set_xticks([obs[0, 0], obs[0, 2]])
+    plt.legend()
+    plt.show()
+else:
+    print("Failure!")
diff --git a/doc/00_assets/planning/Flow_app_enter.png b/doc/00_assets/planning/Flow_app_enter.png
new file mode 100644
index 00000000..a80cb136
Binary files /dev/null and b/doc/00_assets/planning/Flow_app_enter.png differ
diff --git a/doc/00_assets/planning/Flow_app_forwardwait.png b/doc/00_assets/planning/Flow_app_forwardwait.png
new file mode 100644
index 00000000..541847a9
Binary files /dev/null and b/doc/00_assets/planning/Flow_app_forwardwait.png differ
diff --git a/doc/00_assets/planning/Flow_app_intersection.png b/doc/00_assets/planning/Flow_app_intersection.png
new file mode 100644
index 00000000..14fd1a3a
Binary files /dev/null and b/doc/00_assets/planning/Flow_app_intersection.png differ
diff --git a/doc/00_assets/planning/Flow_app_wait.png b/doc/00_assets/planning/Flow_app_wait.png
new file mode 100644
index 00000000..305018e8
Binary files /dev/null and b/doc/00_assets/planning/Flow_app_wait.png differ
diff --git a/doc/06_perception/13_traffic_light_detection.md b/doc/06_perception/13_traffic_light_detection.md
index c627e3cf..68a061c6 100644
--- a/doc/06_perception/13_traffic_light_detection.md
+++ b/doc/06_perception/13_traffic_light_detection.md
@@ -35,3 +35,22 @@ This method sets up a publisher for the traffic light state. It publishes to the
 
 This method is called whenever a new image message is received. It performs traffic light detection by using `traffic_light_inference.py` on the image and publishes the result.
 The result is a `TrafficLightState` message where the state is set to the detected traffic light state (1 for green, 2 for red, 4 for yellow, 0 for unknown).
+
+## Filtering of images
+
+### Vision Node
+
+Objects, which are detected as traffic light by the RTDETR-X model (or others), must fulfill the following criterias to be published:
+
+- At least a 30% (0.30) certainty/probablity of the classification model
+- More than 1.5x as tall (height) as it is wide (width)
+- Above 360px (upper half of the 1280x720 image)
+
+### Traffic Light Node
+
+Objects, which are published by the Vision Node, are further filtered by the following criterias:
+
+- Classification probabilities of "Unknown" and "Side" are either both below 1e-10 or one of both are below 1e-15
+- "Side" is treated as "Unknown"
+- Valid states (Red, Green, Yellow) must be present at least twice in a row to be actually published
+- A state decays (state=0; "Unknown") after 2 seconds if there is no new info in the meantime
diff --git a/doc/07_planning/Behavior_detailed.md b/doc/07_planning/Behavior_detailed.md
new file mode 100644
index 00000000..fc40d449
--- /dev/null
+++ b/doc/07_planning/Behavior_detailed.md
@@ -0,0 +1,88 @@
+# Behaviors detailed
+
+**Disclaimer**: The original behavior and decision tree are based on [PAF22](https://github.com/ll7/paf22)  and a previous [PAF project](https://github.com/ll7/psaf2).
+
+**Summary:**
+The behaviors are responsible for the higher level actions, like driving through an intersection or performing a lane change. This file describes the detailed process of performing the behavior.
+
+---
+
+## Author
+
+Julius Miller
+
+## Date
+
+14.01.2023
+
+---
+<!-- TOC -->
+- [Behaviors detailed](#behaviors-detailed)
+  - [Author](#author)
+  - [Date](#date)
+  - [Intersection](#intersection)
+    - [Detect Intersection](#detect-intersection)
+    - [Approach Intersection](#approach-intersection)
+    - [Wait Intersection](#wait-intersection)
+    - [Enter Intersection](#enter-intersection)
+    - [Leave Intersection](#leave-intersection)
+  - [Lane Change](#lane-change)
+  - [Cruise](#cruise)
+<!-- TOC -->
+
+---
+
+## Intersection
+
+### Detect Intersection
+
+Subscriber: Waypoint
+
+A Intersection is detected if the distance from the current position to the waypoint
+ is less than 30 meters and the waypoint is at an intersection.
+
+### Approach Intersection
+
+Subscribers: Traffic Light, Waypoint (Stopline), Speed
+
+![ApproachIntersection](../00_assets/planning/Flow_app_intersection.png)
+
+If there is a green traffic light the car will drive towards the intersection with a speed of 30 km/h.
+
+In every other case the car stops at the intersection and reduces dynamically the speed calculated by [motion_plannnig.py](../code/planning/src/local_planner/motion_planning.py)
+
+The target_distance is currently 5 meters.
+
+![ForwardWait](../00_assets/planning/Flow_app_forwardwait.png)
+
+Conditions:
+
+1. Describes if the car is stopped at the intersection.
+2. Drive through intersection even if the traffic light turns yellow
+3. Running over line
+
+### Wait Intersection
+
+Subscriber: Traffic Light
+
+![Wait](../00_assets/planning/Flow_app_wait.png)
+
+### Enter Intersection
+
+Subscriber: Waypoint
+
+![Enter](../00_assets/planning/Flow_app_enter.png)
+
+New Waypoint is selected in [global_plan_distance_publisher.py](../code/perception/src/global_plan_distance_publisher.py) if the current position is closer than 2.5 meters to the waypoint or the next waypoint is closer than the currrent waypoint
+
+### Leave Intersection
+
+Signals behavior is over.
+
+## Lane Change
+
+WIP
+
+## Cruise
+
+Default behavior. Motion Planning uses acc speed in this case.