diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 291e161b..deeb0f32 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/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,7 +19,7 @@ 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 @@ -29,7 +27,7 @@ class PrePlanner(CompatibleNode): - 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 """ diff --git a/code/planning/src/global_planner/preplanning_trajectory.py b/code/planning/src/global_planner/preplanning_trajectory.py index 478addd7..2300627e 100755 --- a/code/planning/src/global_planner/preplanning_trajectory.py +++ b/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 diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 74b681b2..2e3fa8c8 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/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") diff --git a/doc/assets/research_assets/planning_internal.png b/doc/assets/research_assets/planning_internal.png new file mode 100644 index 00000000..ed9c9d1c Binary files /dev/null and b/doc/assets/research_assets/planning_internal.png differ diff --git a/doc/planning/Global_Planner.md b/doc/planning/Global_Planner.md index 2d3e4ea9..94a66612 100644 --- a/doc/planning/Global_Planner.md +++ b/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: diff --git a/doc/planning/README.md b/doc/planning/README.md index 806ac81e..aff116ab 100644 --- a/doc/planning/README.md +++ b/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) diff --git a/doc/research/paf24/planning/local_v_global_planning.md b/doc/research/paf24/planning/local_v_global_planning.md new file mode 100644 index 00000000..73383fdf --- /dev/null +++ b/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` + \ No newline at end of file