diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py
index 48abf69b..8c1514cb 100755
--- a/code/planning/src/local_planner/motion_planning.py
+++ b/code/planning/src/local_planner/motion_planning.py
@@ -1,23 +1,21 @@
#!/usr/bin/env python
# import tf.transformations
-import ros_compatibility as roscomp
-import rospy
-import sys
+import math
import os
+import sys
+import numpy as np
+import ros_compatibility as roscomp
+import rospy
+from carla_msgs.msg import CarlaSpeedometer
+from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
+from nav_msgs.msg import Path
+from perception.msg import LaneChange, Waypoint
from ros_compatibility.node import CompatibleNode
from rospy import Publisher, Subscriber
-from std_msgs.msg import String, Float32, Bool, Float32MultiArray, Int16
-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 perception.msg import Waypoint, LaneChange
-
-from utils import convert_to_ms, spawn_car, NUM_WAYPOINTS, TARGET_DISTANCE_TO_STOP
+from std_msgs.msg import Bool, Float32, Float32MultiArray, Int16, String
+from utils import NUM_WAYPOINTS, TARGET_DISTANCE_TO_STOP, convert_to_ms, spawn_car
sys.path.append(os.path.abspath(sys.path[0] + "/../../planning/src/behavior_agent"))
from behaviours import behavior_speed as bs # type: ignore # noqa: E402
@@ -30,10 +28,9 @@
class MotionPlanning(CompatibleNode):
"""
- This node selects speeds according to the behavior in the Decision Tree
- and the ACC.
- Later this Node should compute a local Trajectory and forward
- it to the Acting.
+ This node selects speeds based on the current behaviour and ACC to forward to the
+ acting components. It also handles the generation of trajectories for overtaking
+ maneuvers.
"""
def __init__(self):
@@ -41,12 +38,14 @@ def __init__(self):
self.role_name = self.get_param("role_name", "hero")
self.control_loop_rate = self.get_param("control_loop_rate", 0.05)
+ # TODO: add type hints
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.__collision_point = None
+ # TODO: clarify what the overtake_status values mean (by using an enum or ...)
self.__overtake_status = -1
self.published = False
self.current_pos = None
@@ -162,9 +161,11 @@ def __init__(self):
Float32, f"/paf/{self.role_name}/target_velocity", qos_profile=1
)
+ # TODO move up to subscribers
self.wp_subs = self.new_subscription(
Float32, f"/paf/{self.role_name}/current_wp", self.__set_wp, qos_profile=1
)
+
self.overtake_success_pub = self.new_publisher(
Float32, f"/paf/{self.role_name}/overtake_success", qos_profile=1
)
@@ -235,12 +236,31 @@ def change_trajectory(self, distance_obj):
pose_list = self.trajectory.poses
# Only use fallback
- self.overtake_fallback(distance_obj, pose_list)
+ self.generate_overtake_trajectory(distance_obj, pose_list)
self.__overtake_status = 1
self.overtake_success_pub.publish(self.__overtake_status)
return
- def overtake_fallback(self, distance, pose_list, unstuck=False):
+ def generate_overtake_trajectory(self, distance, pose_list, unstuck=False):
+ """
+ Generates a trajectory for overtaking maneuvers.
+
+ This method creates a new trajectory for the vehicle to follow when an
+ overtaking maneuver is required. It adjusts the waypoints based on the current
+ waypoint and distance, and applies an offset to the waypoints to create a path
+ that avoids obstacles or gets the vehicle unstuck.
+
+ Args:
+ distance (int): The distance over which the overtaking maneuver should be
+ planned.
+ pose_list (list): A list of PoseStamped objects representing the current
+ planned path.
+ unstuck (bool, optional): A flag indicating whether the vehicle is stuck
+ and requires a larger offset to get unstuck. Defaults to False.
+
+ Returns:
+ None: The method updates the self.trajectory attribute with the new path.
+ """
currentwp = self.current_wp
normal_x_offset = 2
unstuck_x_offset = 3 # could need adjustment with better steering
@@ -306,10 +326,25 @@ def __set_trajectory(self, data: Path):
"""
self.trajectory = data
self.loginfo("Trajectory received")
+
self.__corners = self.__calc_corner_points()
- def __calc_corner_points(self):
+ def __calc_corner_points(self) -> list[list[np.ndarray]]:
+ """
+ Calculate the corner points of the trajectory.
+
+ This method converts the poses in the trajectory to an array of coordinates,
+ calculates the angles between consecutive points, and identifies the points
+ where there is a significant change in the angle, indicating a corner or curve.
+ The corner points are then grouped by proximity and returned.
+
+ Returns:
+ list: A list of lists, where each sublist contains 2D points that form a
+ corner.
+ """
coords = self.convert_pose_to_array(np.array(self.trajectory.poses))
+
+ # TODO: refactor by using numpy functions
x_values = np.array([point[0] for point in coords])
y_values = np.array([point[1] for point in coords])
@@ -320,13 +355,25 @@ def __calc_corner_points(self):
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)
+ sublist = self.group_points_by_proximity(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):
+ def group_points_by_proximity(self, points, proximity=5):
+ """
+ Groups a list of points into sublists based on their proximity to each other.
+
+ Args:
+ points (list): A list of points to be grouped.
+ proximity (int, optional): The maximum distance between points in a sublist
+
+ Returns:
+ list: A list of sublists, where each sublist contains points that are within
+ the specified proximity of each other. Sublists with only one point are
+ filtered out.
+ """
sublists = []
current_sublist = []
@@ -345,6 +392,7 @@ def create_sublists(self, points, proximity=5):
if current_sublist:
sublists.append(current_sublist)
+ # TODO: Check if it is intended to filter out sublists with only one point
filtered_list = [in_list for in_list in sublists if len(in_list) > 1]
return filtered_list
@@ -354,6 +402,7 @@ def get_cornering_speed(self):
pos = self.current_pos[:2]
def euclid_dist(vector1, vector2):
+ # TODO replace with numpy function
point1 = np.array(vector1)
point2 = np.array(vector2)
@@ -369,7 +418,7 @@ def map_corner(dist):
elif dist < 50:
return 7
else:
- 8
+ 8 # TODO add return
distance_corner = 0
for i in range(len(corner) - 1):
@@ -395,14 +444,14 @@ def map_corner(dist):
return self.__get_speed_cruise()
@staticmethod
- def convert_pose_to_array(poses: np.array):
- """convert pose array to numpy array
+ def convert_pose_to_array(poses: np.ndarray) -> np.ndarray:
+ """Convert an array of PoseStamped objects to a numpy array of positions.
Args:
- poses (np.array): pose array
+ poses (np.ndarray): Array of PoseStamped objects.
Returns:
- np.array: numpy array
+ np.ndarray: Numpy array of shape (n, 2) containing the x and y positions.
"""
result_array = np.empty((len(poses), 2))
for pose in range(len(poses)):
@@ -424,6 +473,11 @@ def __check_emergency(self, data: Bool):
self.emergency_pub.publish(data)
def update_target_speed(self, acc_speed, behavior):
+ """
+ Updates the target velocity based on the current behavior and ACC velocity and
+ overtake status and publishes it. The unit of the velocity is m/s.
+ """
+
be_speed = self.get_speed_by_behavior(behavior)
if behavior == bs.parking.name or self.__overtake_status == 1:
self.target_speed = be_speed
@@ -439,6 +493,10 @@ def __set_acc_speed(self, data: Float32):
self.__acc_speed = data.data
def __set_curr_behavior(self, data: String):
+ """
+ Sets the received current behavior of the vehicle.
+ If the behavior is an overtake behavior, a trajectory change is triggered.
+ """
self.__curr_behavior = data.data
if data.data == bs.ot_enter_init.name:
if np.isinf(self.__collision_point):
@@ -480,6 +538,7 @@ def get_speed_by_behavior(self, behavior: str) -> float:
return speed
def __get_speed_unstuck(self, behavior: str) -> float:
+ # TODO check if this 'global' is necessary
global UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE
speed = 0.0
if behavior == bs.us_unstuck.name:
@@ -507,7 +566,9 @@ def __get_speed_unstuck(self, behavior: str) -> float:
# create overtake trajectory starting 6 meteres before
# the obstacle
# 6 worked well in tests, but can be adjusted
- self.overtake_fallback(self.unstuck_distance, pose_list, unstuck=True)
+ self.generate_overtake_trajectory(
+ self.unstuck_distance, pose_list, unstuck=True
+ )
self.logfatal("Overtake Trajectory while unstuck!")
self.unstuck_overtake_flag = True
self.init_overtake_pos = self.current_pos[:2]
@@ -628,8 +689,8 @@ def __calc_virtual_overtake(self) -> float:
def run(self):
"""
- Control loop
- :return:
+ Control loop that updates the target speed and publishes the target trajectory
+ and speed over ROS topics.
"""
def loop(timer_event=None):
@@ -649,10 +710,6 @@ def loop(timer_event=None):
if __name__ == "__main__":
- """
- main function starts the MotionPlanning node
- :param args:
- """
roscomp.init("MotionPlanning")
try:
node = MotionPlanning()
diff --git a/doc/assets/planning/planning_structure.drawio b/doc/assets/planning/planning_structure.drawio
new file mode 100644
index 00000000..cb63bdef
--- /dev/null
+++ b/doc/assets/planning/planning_structure.drawio
@@ -0,0 +1,340 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/doc/assets/planning/planning_structure.png b/doc/assets/planning/planning_structure.png
new file mode 100644
index 00000000..568fa982
Binary files /dev/null and b/doc/assets/planning/planning_structure.png differ
diff --git a/doc/general/architecture.md b/doc/general/architecture.md
index 72eacdce..f8eaf839 100644
--- a/doc/general/architecture.md
+++ b/doc/general/architecture.md
@@ -198,12 +198,12 @@ Subscriptions:
- ```collision``` ([std_msgs/Float32MultiArray](https://docs.ros.org/en/api/std_msgs/html/msg/Float32MultiArray.html))
- ```traffic_light_y_distance``` ([std_msgs/Int16](https://docs.ros.org/en/api/std_msgs/html/msg/Int16.html))
- ```unstuck_distance``` ([std_msgs/Float32](https://docs.ros.org/en/api/std_msgs/html/msg/Float32.html))
+- ```current_wp``` ([std_msgs/Float32](https://docs.ros.org/en/api/std_msgs/html/msg/Float32.html))
Publishes:
- ```trajectory``` ([nav_msgs/Path Message](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Path.html))
- ```target_velocity``` ([std_msgs/Float32](https://docs.ros.org/en/api/std_msgs/html/msg/Float32.html))
-- ```current_wp``` ([std_msgs/Float32](https://docs.ros.org/en/api/std_msgs/html/msg/Float32.html))
- ```overtake_success``` ([std_msgs/Float32](https://docs.ros.org/en/api/std_msgs/html/msg/Float32.html))
## Acting
diff --git a/doc/planning/README.md b/doc/planning/README.md
index 6a70faf9..aff116ab 100644
--- a/doc/planning/README.md
+++ b/doc/planning/README.md
@@ -1,14 +1,16 @@
# Planning Wiki
+
+
## Overview
-### [Preplanning](./Preplanning.md)
+### [OpenDrive Converter (preplanning_trajectory.py)](./Preplanning.md)
-Preplanning is very close to the global plan. The challenge of the preplanning focuses on creating a trajectory out of
+This module focuses on creating a trajectory out of
an OpenDrive map (ASAM OpenDrive). As input it receives an xodr file (OpenDrive format) and the target points
from the leaderboard with the belonging actions. For example action number 3 means, drive through the intersection.
-### [Global plan](./Global_Planner.md)
+### [Global Planning (PrePlanner)](./Global_Planner.md)
The global planner is responsible for collecting and preparing all data from the leaderboard and other internal
components that is needed for the preplanning component.
@@ -27,7 +29,9 @@ decision tree, which is easy to adapt and to expand.
### [Local Planning](./Local_Planning.md)
-The Local Planning component is responsible for evaluating short term decisions in the local environment of the ego vehicle. It containes components responsible for detecting collisions and reacting e. g. lowering speed.
-The local planning also executes behaviors e. g. changes the trajectory for an overtake.
+This module includes the Nodes: ACC, CollisionCheck, MotionPlanner
+
+The Local Planning package is responsible for evaluating short term decisions in the local environment of the ego vehicle. It containes components responsible for detecting collisions and reacting e. g. lowering speed.
+The local planning also executes behaviors e.g. changes the trajectory for an overtake.
diff --git a/doc/planning/motion_planning.md b/doc/planning/motion_planning.md
index dd05ed42..83a6960b 100644
--- a/doc/planning/motion_planning.md
+++ b/doc/planning/motion_planning.md
@@ -1,65 +1,65 @@
-# Motion Planning
-
-**Summary:** [motion_planning.py](.../code/planning/local_planner/src/motion_planning.py):
-The motion planning is responsible for collecting all the speeds from the different components and choosing the optimal one to be fowarded into the acting. It also is capabale to change the trajectory for a overtaking maneuver.
-
-- [Motion Planning](#motion-planning)
- - [Overview](#overview)
- - [Component](#component)
- - [ROS Data Interface](#ros-data-interface)
- - [Subscribed Topics](#subscribed-topics)
- - [Published Topics](#published-topics)
- - [Node Creation + Running Tests](#node-creation--running-tests)
-
-## Overview
-
-This module is responsible for adjusting the current speed and the current trajectory according to the traffic situation. It subscribes to topics that provide information about the current speed of the vehicle, the current heading and many more to navigate safely in the simulation.
-It publishes topics that provide information about the target speed, trajectoy changes, current waypoint and if an overtake was successful.
-
-This file is also responsible for providing a ```target_speed of -3``` for acting, whenever we need to use the Unstuck Behavior. -3 is the only case we can drive backwards right now,
-since we only need it for the unstuck routine. It also creates an overtake trajectory, whenever the unstuck behavior calls for it.
-
-## Component
-
-The Motion Planning only consists of one node that contains all subscriper and publishers. It uses some utility functions from [utils.py](../../code/planning/src/local_planner/utils.py).
-
-## ROS Data Interface
-
-### Subscribed Topics
-
-This node subscribes to the following topics:
-
-- `/paf/hero/Spawn_car`: Can spawn a car on the first straight in the dev environment, if this message is manually published.
-- `/paf/hero/speed_limit`: Subscribes to the speed Limit.
-- `/carla/hero/Speed`: Subscribes to the current speed.
-- `/paf/hero/current_heading`: Subscribes to the filtered heading of the ego vehicle.
-- `/paf/hero/trajectory_global`: Subscribes to the global trajectory, which is calculated at the start of the simulation.
-- `/paf/hero/current_pos`: Subscribes to the filtered position of our car.
-- `/paf/hero/curr_behavior`: Subscribes to Current Behavior pubished by the Decision Making.
-- `/paf/hero/unchecked_emergency`: Subscribes to check if the emergency brake is not triggered.
-- `/paf/hero/acc_velocity`: Subscribes to the speed published by the acc.
-- `/paf/hero/waypoint_distance`: Subscribes to the Carla Waypoint to get the new road option.
-- `/paf/hero/lane_change_distance`: Subscribes to the Carla Waypoint to check if the next Waypoint is a lane change.
-- `/paf/hero/collision`: Subscribes to the collision published by the Collision Check.
-- `/paf/hero//Center/traffic_light_y_distance`: Subscribes to the distance the traffic light has to the upper camera bound in pixels.
-- `/paf/hero/unstuck_distance`: Subscribes to the distance travelled by the unstuck maneuver.
-
-### Published Topics
-
-This node publishes the following topics:
-
-- `/paf/hero/trajectory`: Publishes the new adjusted trajectory.
-- `/paf/hero/target_velocity`: Publishes the new calcualted Speed.
-- `/paf/hero/current_wp`: Publishes according to our position the index of the current point on the trajectory.
-- `/paf/hero/overtake_success`: Publishes if an overtake was successful.
-
-## Node Creation + Running Tests
-
-To run this node insert the following statement in the [planning.launch](../../code/planning/launch/planning.launch) file:
-
-```xml
-
-
-
-
-```
+# Motion Planning
+
+**Summary:** [motion_planning.py](.../code/planning/local_planner/src/motion_planning.py):
+The motion planning is responsible for collecting all the velocity recommendations from the different components and choosing the optimal one to be fowarded into the acting. It also is capable to change the trajectory for an overtaking maneuver.
+
+- [Overview](#overview)
+- [ROS Data Interface](#ros-data-interface)
+ - [Subscribed Topics](#subscribed-topics)
+ - [Published Topics](#published-topics)
+- [Node Creation + Running Tests](#node-creation--running-tests)
+
+## Overview
+
+This module contains one ROS node and is responsible for adjusting and publishing the target velocity and the target trajectory according to the traffic situation.
+It subscribes to topics that provide information about the current velocity of the vehicle, the current heading and many more to navigate safely in the simulation.
+It also publishes a topic that indicates wether an overtake maneuver was successful or not.
+
+This component is also responsible for providing a target_speed of `-3` for acting, whenever we need to use the Unstuck Behavior. This is currently the only behavior that allows the car to drive backwards.
+
+The trajectory is calculated by the global planner and is adjusted by the motion planning node.
+When the decision making node decides that an overtake maneuver is necessary, the motion planning node will adjust the trajectory accordingly.
+Otherwise the received trajectory is published without any changes.
+
+## ROS Data Interface
+
+### Subscribed Topics
+
+This node subscribes to the following topics:
+
+- `/carla/hero/Speed`: Subscribes to the current speed.
+- `/paf/hero/acc_velocity`: Subscribes to the speed published by the acc.
+- `/paf/hero/Center/traffic_light_y_distance`: Subscribes to the distance the traffic light has to the upper camera bound in pixels.
+- `/paf/hero/collision`: Subscribes to the collision published by the Collision Check.
+- `/paf/hero/curr_behavior`: Subscribes to Current Behavior pubished by the Decision Making.
+- `/paf/hero/current_heading`: Subscribes to the filtered heading of the ego vehicle.
+- `/paf/hero/current_pos`: Subscribes to the filtered position of our car.
+- `/paf/hero/current_wp`: Subscribes to the current waypoint.
+- `/paf/hero/lane_change_distance`: Subscribes to the Carla Waypoint to check if the next Waypoint is a lane change.
+- `/paf/hero/speed_limit`: Subscribes to the speed limit.
+- `/paf/hero/trajectory_global`: Subscribes to the global trajectory, which is calculated at the start of the simulation.
+- `/paf/hero/unstuck_distance`: Subscribes to the distance travelled by the unstuck maneuver.
+- `/paf/hero/waypoint_distance`: Subscribes to the Carla Waypoint to get the new road option.
+
+### Published Topics
+
+This node publishes the following topics:
+
+- `/paf/hero/overtake_success`: Publishes if an overtake was successful. ([std_msgs/Float32](http://docs.ros.org/en/api/std_msgs/html/msg/Float32.html))
+- `/paf/hero/target_velocity`: Publishes the target velocity in m/s. ([std_msgs/Float32](http://docs.ros.org/en/api/std_msgs/html/msg/Float32.html))
+- `/paf/hero/trajectory`: Publishes the new adjusted trajectory. ([nav_msgs/Path](https://docs.ros.org/en/lunar/api/nav_msgs/html/msg/Path.html))
+
+## Node Creation + Running Tests
+
+To run this node insert the following statement in the [planning.launch](../../code/planning/launch/planning.launch) file:
+
+```xml
+
+
+
+
+```
+
+The motion planning node listens to the following debugging topics:
+
+- `/paf/hero/Spawn_car`: Can spawn a car on the first straight in the dev environment, if this message is manually published.
diff --git a/doc/research/paf24/general/driving_score.md b/doc/research/paf24/general/driving_score.md
new file mode 100644
index 00000000..4eaeea7e
--- /dev/null
+++ b/doc/research/paf24/general/driving_score.md
@@ -0,0 +1,82 @@
+# Driving Score Computation
+
+**Summary:** The Driving score is the main performance metric of the agent and therefore has to be examined.
+
+- [A Starting Point for Metrics and Measurements](#a-starting-point-for-metrics-and-measurements)
+- [Driving score](#driving-score)
+- [Route completion](#route-completion)
+- [Infraction penalty](#infraction-penalty)
+- [Infractions](#infractions)
+- [Off-road driving](#off-road-driving)
+- [Additional Events](#additional-events)
+- [Sources](#sources)
+
+## A Starting Point for Metrics and Measurements
+
+The CARLA Leaderboard sets a public example for evaluation and comparison.
+The driving proficiency of an agent can be characterized by multiple metrics. For this leaderboard, the CARLA team selected a set of metrics that help understand different aspects of driving.
+
+## Driving score
+
+$\frac{1}{N}\sum^i_N R_i P_i$
+
+- The main metric of the leaderboard, serving as an aggregate of the average route completion and the number of traffic infractions. Here $N$ stands for the number of routes, $R_i$ is the percentage of completion of the $i$-th route, and $P_i$ is the infraction penalty of the $i$-th route.
+
+## Route completion
+
+$\frac{1}{N}\sum^i_N R_i$
+
+- Percentage of route distance completed by an agent, averaged across $N$ routes.
+
+## Infraction penalty
+
+$\prod_j^{ped, veh, ... stop} (p_j^i)^{n_{infractions}}$
+
+- Aggregates the number of infractions triggered by an agent as a geometric series. Agents start with an ideal 1.0 base score, which is reduced by a penalty coefficient for every instance of these.
+
+## Infractions
+
+The CARLA leaderboard offers individual metrics for a series of infractions. Each of these has a penalty coefficient that will be applied every time it happens. Ordered by severity, the infractions are the following.
+
+- Collisions with pedestrians $0.50$
+- Collisions with other vehicles $0.60$
+- Collisions with static elements $0.65$
+- Running a red light $0.70$
+- Running a stop sign $0.80$
+
+Some scenarios feature behaviors that can block the ego-vehicle indefinitely. These scenarios will have a timeout of 4 minutes after which the ego-vehicle will be released to continue the route. However, a penalty is applied when the time limit is breached
+
+- Scenario Timeout $0.70$
+
+The agent is expected to maintain a minimum speed in keeping with nearby traffic. The agent’s speed will be compared with the speed of nearby vehicles. Failure to maintain a suitable speed will result in a penalty.
+The penalty applied is dependent on the magnitude of the speed difference, up to the following value:
+
+- Failure to maintain speed $0.70$
+
+The agent should yield to emergency vehicles coming from behind. Failure to allow the emergency vehicle to pass will incur a penalty:
+
+- Failure to yield to emergency vehicle $0.70$
+
+Besides these, there is one additional infraction which has no coefficient, and instead affects the computation of route completion $(R_i)$.
+
+## Off-road driving
+
+If an agent drives off-road, that percentage of the route will not be considered towards the computation of the route completion score.
+
+## Additional Events
+
+Some events will interrupt the simulation, preventing the agent to continue.
+
+- Route deviation
+If an agent deviates more than $30$ meters from the assigned route.
+- Agent blocked
+If an agent is blocked in traffic without taking any actions for $180$ simulation seconds.
+- Simulation timeout
+If no client-server communication can be established in $60$ seconds.
+- Route timeout
+This timeout is triggered if the simulation of a route takes more than the allowed time. This allowed time is computed by multiplying the route distance in meters by a factor of $0.8$.
+
+## Sources
+
+- [Alpha Drive Carla Leaderboard Case Study](https://alphadrive.ai/industries/automotive/carla-leaderboard-case-study/)
+- [Carla Leaderboard](https://leaderboard.carla.org/)
diff --git a/doc/research/paf24/general/old_research_overview.md b/doc/research/paf24/general/old_research_overview.md
new file mode 100644
index 00000000..db2627b9
--- /dev/null
+++ b/doc/research/paf24/general/old_research_overview.md
@@ -0,0 +1,57 @@
+# Research Summary
+
+**Summary:** The research of the previous groups is condensed into this file to make it an entry point for this year's project.
+
+- [Research and Resources](#research-and-resources)
+- [Acting and Control Modules](#acting-and-control-modules)
+- [Planning and Trajectory Generation](#planning-and-trajectory-generation)
+- [State Machine for Decision-Making](#state-machine-for-decision-making)
+- [OpenDrive Integration and Navigation Data](#opendrive-integration-and-navigation-data)
+
+## Research and Resources
+
+- This section provides an extensive foundation for the autonomous vehicle project by consolidating previous research from **PAF22** and **PAF23**.
+- **PAF22**: Established core methods for autonomous vehicle control and perception, including traffic light detection and emergency braking features. It also set up the base components of the CARLA simulator integration, essential sensor configurations, and data processing pipelines.
+- **PAF23**: Enhanced lane-change algorithms and expanded intersection-handling strategies. This project introduced a more robust approach to decision-making at intersections, factoring in pedestrian presence, oncoming traffic, and improved signal detection. Additionally, **PAF23** refined vehicle
+- behavior for more fluid lane-change maneuvers, optimizing control responses to avoid obstacles and maintain lane positioning during highway merging and overtaking scenarios.
+- The resources also include CARLA-specific tools such as the CARLA Leaderboard and ROS Bridge integration, which link CARLA’s simulation environment to the Robot Operating System (ROS). Detailed references to CARLA’s sensor suite are provided, covering RGB cameras, LIDAR, radar, GNSS, and IMU
+sensors essential for perception and control.
+
+## Acting and Control Modules
+
+- The **acting module** focuses on the vehicle’s control actions, including throttle, steering, and braking.
+- **Core Controllers**: Contains controllers like the **PID controller** for longitudinal (speed) control and **Pure Pursuit** and **Stanley controllers** for lateral (steering) control. These controllers work in unison to achieve precise vehicle handling, especially on turns and at varying speeds.
+- **PAF22 Contributions**: Implemented the PID-based longitudinal controller and integrated the Pure Pursuit controller for basic trajectory following, setting a solid foundation for steering and throttle control. PAF22 also laid out the preliminary **emergency braking** logic, designed to override
+other controls in hazardous situations.
+- **PAF23 Contributions**: Introduced refinements in lateral control, including the adaptive Stanley controller, which adjusts steering sensitivity based on vehicle speed to maintain a smooth trajectory. **PAF23** also optimized the emergency braking logic to respond more quickly to obstacles, with
+improvements in lane-changing safety.
+- **Sensor Integration**: The acting module subscribes to **navigation and sensor data** topics to remain updated on vehicle position and velocity, integrating sensor feedback for real-time control adjustments.
+
+## Planning and Trajectory Generation
+
+- The **planning module** is critical for determining safe, efficient routes by combining **global and local path planning** techniques.
+- **Global Planning**: Uses the **CommonRoad route planner** from TUM, creating a high-level path based on predefined waypoints. **PAF22** initially set up this planner, while **PAF23** added finer adjustments for lane selection and obstacle navigation.
+- **Local Planning**: Tailored for dynamic obstacles, this component focuses on immediate adjustments to the vehicle’s path, particularly useful in urban environments with unpredictable elements.
+Local planning includes **trajectory tracking** using Pure Pursuit and Stanley controllers to maintain a steady path.
+- **PAF23 Enhancements**: Improved **collision avoidance** algorithms and added real-time updates to the trajectory based on sensor data. The local planner now adapts quickly to lane-change requests or route deviations due to traffic, creating a seamless flow between global and local path planning.
+Furthermore, the integration of **behavior trees** has been researched, offering some advantages in computing power and explainability. Its drawbacks in uncertain situations and complex environments have been presented.
+
+## State Machine for Decision-Making
+
+- This modular state machine handles various driving behaviors, including **lane changes**, **intersections**, and **traffic light responses**.
+- **Core State Machines**: The **driving state machine** manages normal vehicle navigation, controlling target speed and ensuring lane compliance. The **lane-change state machine** makes safe decisions based on lane availability and traffic,
+while the **intersection state machine** manages vehicle approach, stop, and turn behaviors at intersections.
+
+- **PAF22 Contributions**: Developed the base decision-making states, enabling lane following, simple lane-change maneuvers, and basic intersection handling.
+- **PAF23 Contributions**: Significantly enhanced the state machine by adding specialized states for complex maneuvers, like responding to oncoming traffic at intersections, merging onto highways, and making priority-based decisions at roundabouts.
+The **intersection state machine** now incorporates detailed behaviors for handling left turns, straight passes, and right turns, considering pedestrian zones and cross-traffic. **PAF23** also introduced an adaptive lane-change state,
+which calculates safety based on vehicle speed, distance to adjacent vehicles, and road type.
+
+## OpenDrive Integration and Navigation Data
+
+- **OpenDrive** files provide a structured road network description, detailing lanes, road segments, intersections, and traffic signals. The navigation data is published in CARLA as ROS topics containing GPS/world coordinates and route instructions.
+- **PAF22 Setup**: Established OpenDrive as the core format for map data, integrating it with the CARLA simulator. Initial work involved parsing road and lane data to create accurate trajectories.
+- **PAF23 Enhancements**: Improved the parsing of OpenDrive files, focusing on high-level map data relevant to the vehicle's route, such as signal placements and lane restrictions. This project also optimized the navigation data integration with ROS,
+ensuring the vehicle receives consistent updates on its location relative to the route, intersections, and nearby obstacles.
+- **Navigation Data Structure**: The system uses navigation data points, including **GPS coordinates, world coordinates, and high-level route instructions** (e.g., turn left, change lanes) to guide the vehicle. Each point is matched with a **road option command**,
+instructing the vehicle on how to proceed at specific waypoints.