Merge branch 'main' into 373-unstuck-behavior

code/planning/src/global_planner/global_planner.py

@@ -1,17 +1,15 @@
 #!/usr/bin/env python
-import rospy
-import tf.transformations
-import ros_compatibility as roscomp
-from ros_compatibility.node import CompatibleNode
 from xml.etree import ElementTree as eTree
 
-from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
+import ros_compatibility as roscomp
+import rospy
+import tf.transformations
 from carla_msgs.msg import CarlaRoute  # , CarlaWorldInfo
+from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
 from nav_msgs.msg import Path
-from std_msgs.msg import String
-from std_msgs.msg import Float32MultiArray
-
 from preplanning_trajectory import OpenDriveConverter
+from ros_compatibility.node import CompatibleNode
+from std_msgs.msg import Float32MultiArray, String
 
 RIGHT = 1
 LEFT = 2
@@ -21,15 +19,15 @@
 class PrePlanner(CompatibleNode):
     """
     This node is responsible for collecting all data needed for the
-    preplanning and calculate a trajectory based on the OpenDriveConverter
+    preplanning and calculating a trajectory based on the OpenDriveConverter
     from preplanning_trajectory.py.
     Subscribed/needed topics:
     - OpenDrive Map:          /carla/{role_name}/OpenDRIVE
                  or:          /carla/world_info
     - global Plan:            /carla/{role_name}/global_plan
     - current agent position: /paf/{role_name}/current_pos
     Published topics:
-    - preplanned trajectory:  /paf/{role_name}/trajectory
+    - preplanned trajectory:  /paf/{role_name}/trajectory_global
     - prevailing speed limits:/paf/{role_name}/speed_limits_OpenDrive
     """
 
@@ -90,7 +88,7 @@ def global_route_callback(self, data: CarlaRoute) -> None:
         """
         when the global route gets updated a new trajectory is calculated with
         the help of OpenDriveConverter and published into
-        '/paf/ self.role_name /trajectory'
+        '/paf/ self.role_name /trajectory_global'
         :param data: global Route
         """
         if data is None:
@@ -213,7 +211,7 @@ def global_route_callback(self, data: CarlaRoute) -> None:
 
     def world_info_callback(self, opendrive: String) -> None:
         """
-        when the map gets updated a mew OpenDriveConverter instance is created
+        when the map gets updated a new OpenDriveConverter instance is created
         (needed for the trajectory preplanning)
         :param opendrive: updated CarlaWorldInformation
         """

code/planning/src/global_planner/preplanning_trajectory.py

@@ -1,9 +1,9 @@
 import copy
-from xml.etree import ElementTree as eTree
-import help_functions
+from math import cos, degrees, sin
 from typing import Tuple
-from math import sin, cos, degrees
+from xml.etree import ElementTree as eTree
 
+import help_functions
 
 # Check small distance between two points
 SMALL_DIST = 0.001
@@ -220,7 +220,7 @@ def calculate_intervalls_id(
     ):
         """
         The function assumes, that the current chosen road is not the only
-        one that is possible. The current raad is calculated based on all
+        one that is possible. The current road is calculated based on all
         possible scenarios not assuming that the succ and pred road are
         both junctions.
         :param agent: current position of the agent with x and y coordinate
@@ -287,7 +287,7 @@ def calculate_intervalls_id(
 
     def get_special_case_id(self, road: int, current: int, agent: Tuple[float, float]):
         """When the function get_min_dist() returns two solutions with the
-        same distance, this function calculated the distance based on the
+        same distance, this function calculates the distance based on the
         interpolation of the two possible roads.
         :param road: id value of the successor or predecessor road
         :param current: id value of the current road

code/planning/src/local_planner/ACC.py

@@ -1,19 +1,18 @@
 #!/usr/bin/env python
+import numpy as np
 import ros_compatibility as roscomp
-from ros_compatibility.node import CompatibleNode
-from rospy import Subscriber, Publisher
-from geometry_msgs.msg import PoseStamped
 from carla_msgs.msg import CarlaSpeedometer  # , CarlaWorldInfo
+from geometry_msgs.msg import PoseStamped
 from nav_msgs.msg import Path
-from std_msgs.msg import Float32MultiArray, Float32, Bool
-import numpy as np
-from utils import interpolate_speed, calculate_rule_of_thumb
+from ros_compatibility.node import CompatibleNode
+from rospy import Publisher, Subscriber
+from std_msgs.msg import Bool, Float32, Float32MultiArray
+from utils import calculate_rule_of_thumb, interpolate_speed
 
 
 class ACC(CompatibleNode):
-    """
-    This node recieves a possible collision and
-    """
+    """ACC (Adaptive Cruise Control) calculates and publishes the desired speed based on
+    possible collisions, the current speed, the trajectory, and the speed limits."""
 
     def __init__(self):
         super(ACC, self).__init__("ACC")

doc/planning/Global_Planner.md

@@ -1,11 +1,10 @@
 # Global Planner
 
 **Summary:** [global_planner.py](.../code/planning/global_planner/src/global_planner.py):
-The global planner is responsible for collecting and preparing all data from the leaderboard and other intern
+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.
 After finishing that this node initiates the calculation of a trajectory based on the OpenDriveConverter
-from preplanning_trajectory.py. In the end the computed trajectory and prevailing speed limits are published
-to the other components of this project (acting, decision making,...).
+from preplanning_trajectory.py. In the end the computed trajectory and prevailing speed limits are published.
 
 This component and so most of the documentation was taken from the previous project PAF22 (Authors: Simon Erlbacher, Niklas Vogel)
 
@@ -25,8 +24,7 @@ No extra installation needed.
 
 ## Description
 
-First the global planner is responsible for collecting and preparing all data from the leaderboard and other intern
-components that is needed for the preplanning component.
+First 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.
 
 To get an instance of the OpenDriveConverter (ODC) the received OpenDrive Map prevailing in String format
 has to be converted. In our case we use the
@@ -56,8 +54,8 @@ The received agent spawn position is valid if it´s closer to the first waypoint
 parameter expresses. This is necessary to prevent unwanted behaviour in the startup phase where the
 current agent position is faulty.
 
-When the ODC got initialised, the current agent position is received and the global plan is obtained from
-the leaderboard the trajectory can be calculated by iterating through the global route and passing it to the ODC.
+When the ODC is initialised, the current agent position is received and the global plan is obtained from
+the leaderboard. The trajectory can be calculated by iterating through the global route and passing it to the ODC.
 After smaller outliners are removed the x and y coordinates as well as the yaw-orientation and the prevailing
 speed limits can be acquired from the ODC in the following form:
 

doc/planning/README.md

@@ -12,11 +12,10 @@ from the leaderboard with the belonging actions. For example action number 3 mea
 
 ### [Global Planning (PrePlanner)](./Global_Planner.md)
 
-The global planner is responsible for collecting and preparing all data from the leaderboard and other intern
+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.
 After finishing that this node initiates the calculation of a trajectory based on the OpenDriveConverter
-from preplanning_trajectory.py. In the end the computed trajectory and prevailing speed limits are published
-to the other components of this project (acting, decision making,...).
+from preplanning_trajectory.py. In the end the computed trajectory and prevailing speed limits are published.
 
 ![img.png](../assets/Global_Plan.png)
 

doc/research/paf24/planning/local_v_global_planning.md

@@ -0,0 +1,33 @@
+# Relation beteween local and global planning
+
+Communication between Global and Local Planning is done via ros topics. Here is a graph showing the relation between the nodes.
+
+- Global Planning: PrePlanner
+- Local Planning: ACC (Adaptive Cruise Control), CollisionCheck, MotionPlanning
+
+![Graph relation](/doc/assets/research_assets/planning_internal.png)
+
+## Global Planning
+
+Global planning consists of the PrePlanner Node and the OpenDriveConverter.
+
+- OpenDriveConverter: The class is primarily used to process OpenDrive maps and extract relevant information needed for trajectory planning.
+- PrePlanner Node: The PrePlanner node is responsible for creating a trajectory out of an OpenDrive Map with the belonging road options.
+  - the `/paf/hero/trajectory_global` topic published is used internally for the preplanned trajectory.
+
+## Local Planning
+
+Local planning consists of the ACC, CollisionCheck and MotionPlanning nodes. It uses information from the Global Planning to make decisions in the local environment of the ego vehicle.
+It creates a trajectory based on the global trajectory and the current position of the vehicle that is used by the acting component.
+
+- ACC: The Adaptive Cruise Control node is responsible for calculating the desired speed of the vehicle based on the current speed, trajectory, and speed limits.
+  - receives Speed from `/carla/hero/Speed`
+  - receives speed limits from `/paf/hero/speed_limits_OpenDrive`
+  - receives global trajectory from `/paf/hero/trajectory_global`
+- CollisionCheck: The Collision Check node is responsible for detecting collisions and reporting them.
+  - receives current speed from `/carla/hero/Speed`
+  - receives distances to objects detected by a LIDAR sensor from `/paf/hero/Center/object_distance`
+- MotionPlanning: The Motion Planning node is responsible for calculating the desired trajectory based on the current position of the vehicle and the global trajectory.
+  - receives speed information from ACC
+  - receives global trajectory from `/paf/hero/trajectory_global`
+