Merge branch 'main' into 370-localization-with-gps

code/planning/src/local_planner/motion_planning.py

@@ -3,6 +3,7 @@
 import math
 import os
 import sys
+from typing import List
 
 import numpy as np
 import ros_compatibility as roscomp
@@ -329,7 +330,7 @@ def __set_trajectory(self, data: Path):
 
         self.__corners = self.__calc_corner_points()
 
-    def __calc_corner_points(self) -> list[list[np.ndarray]]:
+    def __calc_corner_points(self) -> List[List[np.ndarray]]:
         """
         Calculate the corner points of the trajectory.
 

doc/research/paf24/general/leaderboard_summary.md

@@ -0,0 +1,131 @@
+# Leaderboard summary
+
+**Summary:** This document depicts general informations regarding the leaderboard as a quick overview in a more condensed form.
+
+- [General Information](#general-information)
+- [Traffic Scenarios](#traffic-scenarios)
+  - [Generic](#generic)
+  - [Control Loss](#control-loss)
+  - [Traffic negotiation](#traffic-negotiation)
+  - [Highway](#highway)
+  - [Obstacle avoidance](#obstacle-avoidance)
+  - [Braking and lane changing](#braking-and-lane-changing)
+  - [Parking](#parking)
+- [Available Sensors](#available-sensors)
+
+## General Information
+
+The CARLA Leaderboard includes a variety of scenarios to test autonomous driving models in realistic urban environments. This document provides a quick overview of all possible scenarios and available sensors.
+The leaderboard offers a driving score metric based on infractions happening during scenarios.
+
+[#366](https://github.com/una-auxme/paf/issues/366) provides a more indepth look at how this score is calculated.
+
+Each time an infraction takes place several details are recorded and appended to a list specific to the infraction type in question. Besides scenario and score handling, the leaderboard also provides a plethora of sensors to get data from.
+
+The leaderboard provides predefined routes with a starting and destination point. The route description can contain GPS style coordinates, map coordinates or route instructions.
+
+Two participation modalities are offered, "Sensors" and "Map". "Sensors" only offers the sensors listed down below while "Map" offers HD map data in addition to all the sensors.
+
+## Traffic Scenarios
+
+- ### **Generic:**
+
+  - Traffic lights
+  - Signs (stop, speed limit, yield)
+
+- ### **Control Loss:**
+
+  - The ego-vehicle loses control due to bad conditions on the road and it must recover, coming back to its original lane.
+    ![image](https://leaderboard.carla.org/assets/images/TR01.png)
+
+- ### **Traffic negotiation:**
+
+  - The ego-vehicle is performing an unprotected left turn at an intersection, yielding to oncoming traffic.
+    ![image](https://leaderboard.carla.org/assets/images/TR08.png)
+  - The ego-vehicle is performing a right turn at an intersection, yielding to crossing traffic.
+    ![image](https://leaderboard.carla.org/assets/images/TR09.png)
+  - The ego-vehicle needs to negotiate with other vehicles to cross an unsignalized intersection.
+    ![image](https://leaderboard.carla.org/assets/images/TR10.png)
+  - The ego-vehicle is going straight at an intersection but a crossing vehicle runs a red light, forcing the ego-vehicle to avoid the collision.
+    ![image](https://leaderboard.carla.org/assets/images/TR07.png)
+  - The ego-vehicle needs to perform a turn at an intersection yielding to bicycles crossing from either the left or right.
+    ![image](https://leaderboard.carla.org/assets/images/TR13.png)
+
+- ### **Highway:**
+
+  - The ego-vehicle merges into moving highway traffic from a highway on-ramp.
+    ![image](https://leaderboard.carla.org/assets/images/TR18.png)
+  - The ego-vehicle encounters a vehicle merging into its lane from a highway on-ramp.
+    ![image](https://leaderboard.carla.org/assets/images/TR19.png)
+  - The ego-vehicle encounters a vehicle cutting into its lane from a lane of static traffic.
+    ![image](https://leaderboard.carla.org/assets/images/TR20.png)
+  - The ego-vehicle must cross a lane of moving traffic to exit the highway at an off-ramp.
+    ![image](https://leaderboard.carla.org/assets/images/TR21.png)
+  - The ego-vehicle is approached by an emergency vehicle coming from behind.
+    ![image](https://leaderboard.carla.org/assets/images/TR23.png)
+
+- ### **Obstacle avoidance:**
+
+  - The ego-vehicle encounters an obstacle blocking the lane and must perform a lane change into traffic moving in the same or opposite direction to avoid it. The obstacle may be a construction site, an accident or a parked vehicle.
+    ![image](https://leaderboard.carla.org/assets/images/TR14.png)
+  - The ego-vehicle encounters a parked vehicle opening a door into its lane and must maneuver to avoid it.
+    ![image](https://leaderboard.carla.org/assets/images/TR15.png)
+  - The ego-vehicle encounters a slow moving hazard (e.g. bicycles) blocking part of the lane. The ego-vehicle must brake or maneuver next to a lane of traffic moving in the same or opposite direction to avoid it.
+    ![image](https://leaderboard.carla.org/assets/images/TR16.png)
+  - The ego-vehicle encounters an oncoming vehicles invading its lane on a bend due to an obstacle.
+    ![image](https://leaderboard.carla.org/assets/images/TR22.png)
+
+- ### **Braking and lane changing:**
+
+  - The leading vehicle decelerates suddenly due to an obstacle and the ego-vehicle must perform an emergency brake or an avoidance maneuver.
+    ![image](https://leaderboard.carla.org/assets/images/TR02.png)
+  - The ego-vehicle encounters an obstacle / unexpected entity on the road and must perform an emergency brake or an avoidance maneuver.
+    ![image](https://leaderboard.carla.org/assets/images/TR03.png)
+  - The ego-vehicle encounters an pedestrian emerging from behind a parked vehicle and advancing into the lane.
+    ![image](https://leaderboard.carla.org/assets/images/TR17.png)
+  - While performing a maneuver, the ego-vehicle encounters an obstacle in the road, either a pedestrian or a bicycle, and must perform an emergency brake or an avoidance maneuver.
+    ![image](https://leaderboard.carla.org/assets/images/TR04.png)
+  - While performing a maneuver, the ego-vehicle encounters a stopped vehicle in the road and must perform an emergency brake or an avoidance maneuver.
+    ![image](https://leaderboard.carla.org/assets/images/TR19a.png)
+  - The ego-vehicle must slow down or brake to allow a parked vehicle exiting a parallel parking bay to cut in front.
+    ![image](https://leaderboard.carla.org/assets/images/TR12.png)
+
+- ### **Parking:**
+
+  - The ego-vehicle must exit a parallel parking bay into a flow of traffic.
+    ![image](https://leaderboard.carla.org/assets/images/TR11.png)
+
+## Available Sensors
+
+- **[GNSS](https://carla.readthedocs.io/en/latest/ref_sensors/#gnss-sensor)**
+  - GPS sensor returning geo location data.
+- **[IMU](https://carla.readthedocs.io/en/latest/ref_sensors/#imu-sensor)**
+  - 6-axis inertial measurement unit.
+- **[LIDAR](https://carla.readthedocs.io/en/latest/ref_sensors/#lidar-sensor)**
+  - Laser to detect obstacles.
+- **[RADAR](https://carla.readthedocs.io/en/latest/ref_sensors/#radar-sensor)**
+  - Long-range RADAR (up to 100 meters).
+- **[RGB CAMERA](https://carla.readthedocs.io/en/latest/ref_sensors/#rgb-camera)**
+  - Regular camera for image capture
+- **[COLLISION DETECTOR](https://carla.readthedocs.io/en/latest/ref_sensors/#collision-detector)**
+  - Used to detect collisions with other actors.
+- **[DEPTH CAMERA](https://carla.readthedocs.io/en/latest/ref_sensors/#depth-camera)**
+  - Provides a distance-coded picture to create a depth map of the scene.
+- **[LANE INVASION DETECTOR](https://carla.readthedocs.io/en/latest/ref_sensors/#lane-invasion-detector)**
+  - Uses road data to detect when the vehicle crosses a lane marking.
+- **[OBSTACLE DETECTOR](https://carla.readthedocs.io/en/latest/ref_sensors/#obstacle-detector)**
+  - Detects obstacles in front of the vehicle within a capsular shape. Works geometry based so requires obstacles to exist as geometry in the scene. (seems like a pure simulation sensor)
+- **[RSS SENSOR](https://carla.readthedocs.io/en/latest/ref_sensors/#rss-sensor)**
+  - Integrates the responisbility sensitive safety model in CARLA. Basically a framwork for mathematical guidelines on how to react in various scenarios. Disabled by default.
+- **[SEMANTIC LIDAR](https://carla.readthedocs.io/en/latest/ref_sensors/#semantic-lidar-sensor)**
+  - Similiar to LIDAR with a different data structure/focus.
+- **[SEMANTIC SEGMENTATION CAMERA](https://carla.readthedocs.io/en/latest/ref_sensors/#semantic-segmentation-camera)**
+  - Classifies objects in sight by tag (seems like a pure simulation sensor).
+- **[INSTANCE SEGMENTATION CAMERA](https://carla.readthedocs.io/en/latest/ref_sensors/#instance-segmentation-camera)**
+  - Classifies every object by class and instance ID (seems like a pure simulation sensor).
+- **[DVS CAMERA](https://carla.readthedocs.io/en/latest/ref_sensors/#dvs-camera)**
+  - Different kind of camera that works in high-speed scenarios. Pixels asynchronously respond to local changes in brightness instead of globally by shutter.
+- **[OPTICAL FLOW CAMERA](https://carla.readthedocs.io/en/latest/ref_sensors/#optical-flow-camera)**
+  - Captures motion(velocity per pixel) perceived from the point of view of the camera.
+- **[V2X SENSOR](https://carla.readthedocs.io/en/latest/ref_sensors/#v2x-sensor)**
+  - Vehicle to everything sensor, allows vehicle to communicate with other vehicles and elements in the environment. More like a concept for the future, not widespread implemented yet.

doc/research/paf24/planning/Unstuck_Overtake Behavior.md

@@ -0,0 +1,156 @@
+# Unstuck/Overtake Behavior
+
+**Summary:** This document analyzes the current unstuck/overtake behavior and reveals several critical issues with the existing implementation.
+
+- [Unstuck behavior](#unstuck-behavior)
+  - [Key components](#key-components)
+  - [How unstuck is triggered](#how-unstuck-is-triggered)
+- [Overtake behavior](#overtake-behavior)
+  - [Key components](#key-components)
+  - [How overtake is triggered](#how-overtake-is-triggered)
+- [Relevant rostopics](#relevant-rostopics)
+- [Methods overview](#methods-overview)
+  - [def \_\_set_curr_behavior](#def-set_curr_behavior)
+  - [def change_trajectory](#def-change_trajectory)
+  - [def overtake_fallback](#def-overtake_fallback)
+- [Overtake behavior issues](#overtake-behavior-issues)
+  - [Aggressive lane changes](#aggressive-lane-changes)
+  - [Inadequate collision detection](#inadequate-collision-detection)
+  - [Improper trajectory planning](#improper-trajectory-planning)
+- [Unstuck behavior issues](#unstuck-behavior-issues)
+- [Potential improvements](#potential-improvements)
+
+---
+
+## Unstuck behavior
+
+The "unstuck" behavior is designed to address situations where the vehicle becomes stuck, perhaps due to obstacles or other vehicles blocking its path.
+
+### Key components
+
+- `self.unstuck_distance`: Tracks the distance to the object causing the blockage, helping the algorithm to determine if an alternative route (overtaking) might be required.
+- `self.unstuck_overtake_flag`: Prevents repeated or "spam" overtakes by ensuring that the unstuck overtake only happens once within a specified distance (`UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE`).
+- `self.init_overtake_pos`: Saves the position where the unstuck behavior is initiated to prevent excessive overtaking within a short range.
+- **Overtaking Logic**: The method `overtake_fallback` is called within `__get_speed_unstuck`, where a new trajectory is created by offsetting the path to one side, allowing the vehicle to bypass the obstacle and get "unstuck."
+
+### How unstuck is triggered
+
+The method `__get_speed_unstuck` checks the current behavior (e.g., `us_unstuck`), adjusting speed accordingly and invoking `overtake_fallback` if an obstacle is detected and the vehicle is in an "unstuck" situation.
+
+---
+
+`UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE` ensures that once an unstuck maneuver is initiated, it won't be repeated until a certain distance is cleared.
+
+---
+
+## Overtake behavior
+
+The overtake behavior allows the vehicle to safely overtake a slower-moving or stationary vehicle ahead by modifying its trajectory.
+
+### Key components
+
+- `self.__overtake_status`: Manages the status of overtaking, where `1` indicates an active overtake, while `-1` signals that overtaking isn't required.
+- `change_trajectory`: Initiates the overtake by calling `overtake_fallback` when it detects a collision point or slow-moving vehicle within a certain range. This method also publishes the updated `__overtake_status` to signal other systems of the change.
+- `overtake_fallback`: Calculates a new trajectory path that offsets the vehicle by a certain distance (`normal_x_offset`) to safely pass the obstacle. The adjusted path uses the Rotation library to align the offset with the vehicle’s heading, creating a smooth path around the obstacle.
+
+### How overtake is triggered
+
+The method `__set_curr_behavior` monitors the vehicle’s behavior, triggering an overtake when `ot_enter_init` behavior is detected and if an obstacle or collision point is near. It then calls `change_trajectory` to modify the route.
+
+---
+
+Speed adjustments specific to overtaking are handled in `__get_speed_overtake`, where the vehicle might slow down (e.g., `ot_enter_slow`) or proceed at normal speed after overtaking (e.g., `ot_leave`).
+
+---
+
+## Relevant rostopics
+
+- `overtake_success`
+- `unstuck_distance`
+- _#TODO: need to continue_
+
+---
+
+## Methods overview
+
+### `def __set_curr_behavior(self, data: String)`
+
+**How it works:**
+
+- **If beginning an overtake maneuver:**
+  - If no obstacle to overtake:
+    - Unsuccessful or canceled overtake
+    - Publish abandonment of the overtake attempt.
+  - Otherwise:
+    - Change trajectory
+
+- **Infinity as "No Collision Detected"**: Setting `self.__collision_point` to infinity (`np.inf`) means "no collision detected." So, checking if `np.isinf(self.__collision_point)` effectively asks, "Is there no obstacle or point that needs an overtake?"
+
+  - `self.__overtake_status = -1`: Unsuccessful or canceled overtake
+  - `self.__overtake_status = 1`: Overtake should take place
+
+**Questions:**
+
+1. Who tells the function the behavior status?
+2. If the function has already become an overtake status, why should we check if there is actually an object to overtake?
+
+---
+
+### `def change_trajectory(self, distance_obj)`
+
+**Description**: Updates trajectory for overtaking and converts it to a new `Path` message.
+
+- **Args**: `distance_obj` (float): Distance to overtake object
+- **Outcome**: Publishes overtake success status
+
+**Questions:**
+
+1. What does the success status for overtake mean? It seems like it publishes that an overtake takes place but does not notify if the maneuver was successful.
+
+---
+
+### `def overtake_fallback(self, distance, pose_list, unstuck=False)`
+
+**Description**: Constructs a temporary path around an obstacle based on the current location, intended distance to overtake, and a choice between two different lateral offsets depending on whether the vehicle is in an "unstuck" situation.
+
+1. **Pick Path Points Around the Obstacle**: Selects a section of the path around the vehicle’s current position and the obstacle it needs to overtake. If the vehicle is stuck, it picks a larger section to give it more room to maneuver.
+2. **Shift Path to the Side**: Moves this section of the path slightly to the side (left or right, depending on the vehicle's heading) to avoid the obstacle. If the vehicle is in a “stuck” situation, it shifts it a bit more to give it extra clearance.
+3. **Create the New Path**: Converts the shifted points into a path format that the vehicle’s navigation system can understand and follow.
+4. **Combine with Original Path**: Merges this temporary bypass with the original path, so the vehicle can return to its route after it passes the obstacle.
+
+---
+
+## Overtake behavior issues
+
+### Aggressive lane changes
+
+The vehicle exhibits aggressive lane changes, leading to emergency stops to avoid oncoming traffic. This behavior suggests that the decision-making logic for overtaking does not adequately assess the surrounding traffic conditions before executing maneuvers.
+
+### Inadequate collision detection
+
+The vehicle fails to avoid obstacles, such as open car doors or stationary vehicles, indicating that the collision detection mechanisms are either insufficient or not effectively integrated into the overtaking logic.
+
+### Improper trajectory planning
+
+The trajectory generated for overtaking often leads to incorrect paths, such as attempting to overtake trees or parked cars without considering oncoming traffic. A better filtering and validation of potential overtaking targets is needed.
+
+---
+
+## Unstuck behavior issues
+
+- The vehicle gets stuck multiple times and exhibits erratic behavior when trying to get unstuck.
+- The vehicle's aggressive lane-holding behavior after being unstuck.
+- After collisions, the vehicle often fails to resume movement, indicating a lack of recovery logic post-collision.
+
+---
+
+## Potential improvements
+
+- `def change_trajectory`: Consider implementing a more sophisticated trajectory planning algorithm that considers dynamic obstacles and traffic conditions rather than relying on a fallback method. Verify that the area into which the vehicle is moving is clear of obstacles and oncoming traffic.
+- `def overtake_fallback`: Instead of fixed offsets for unstuck and normal situations, consider dynamically calculating offsets based on current speed, vehicle dimensions, and surrounding traffic conditions.
+- `__get_speed_unstuck`: Include checks for nearby vehicles and their speeds. Make `UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE` dynamic.
+- `__check_emergency`: Currently, it only considers whether the vehicle is in a parking behavior state. Expand this method to evaluate various emergency conditions (e.g., obstacles detected by sensors) and initiate appropriate responses (e.g., stopping or rerouting).
+- `get_speed_by_behavior`: Consider feedback from sensors regarding current traffic conditions.
+- `__calc_corner_points`: Consider a more intelligent approach rather than relying on simple angle thresholds.
+
+---

doc/research/paf24/planning/planning_acting_relation.md

@@ -0,0 +1,31 @@
+# Relation between planning and acting
+
+This diagram shows the basics of the communication between the planning (green) and acting (blue) components. The information is gained using the rqt-graph. There are three main topics which represent the communication:
+
+- `/paf/hero/trajectory`
+- `/paf/hero/emergency`
+- `/paf/hero/curr_behavior`
+
+![Diagram showing the involved nodes and topics and their relation](../../../assets/research_assets/planning_acting_communication.svg)
+
+## Useful commands
+
+- The types of the messages published on the mentioned topics can be found out with the command `rostopic type <topic>`.
+- To have a deeper look on the message types, you can use the command `rosmsg show <messageType>`.
+- `rostopic echo <topic>` shows all the messages published on the specified topic.
+
+## Messages
+
+The topic `/paf/hero/trajectory` uses the message type `nav_msgs/Path`. This type looks as follows:
+
+![Format of Path messages](../../../assets/research_assets/nav_msgs_Path_type.png)
+
+Every message that is published consists of several parts of this structure. One concrete example of a part of such a message can be seen here:
+
+![Example of a Path message](../../../assets/research_assets/trajectory_example.png)
+
+The topic `/paf/hero/emergency` uses the message type `std_msgs/Bool`, so the emergency case can either be true or false.
+
+The topic `/paf/hero/curr_behavior` uses the message type `std_msgs/String`. Examples of these messages can be seen here:
+
+![Examples of messages posted on the topic curr_behavior](../../../assets/research_assets/curr_behavior_example.png)