diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 2d39ab33..deeb0f32 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -19,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 @@ -88,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: @@ -211,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..a4e56537 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 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/planning/Collision_Check.md b/doc/planning/Collision_Check.md index bbf6032c..d3e88572 100644 --- a/doc/planning/Collision_Check.md +++ b/doc/planning/Collision_Check.md @@ -3,11 +3,12 @@ **Summary:** This module is responsible for detecting collisions and reporting them. It subscribes to topics that provide information about the current speed of the vehicle and the distances to objects detected by a LIDAR sensor. It publishes topics that provide information about emergency stops, the distance to collisions, the distance to oncoming traffic, and the approximated speed of the obstacle in front. -- [Component](#component) -- [ROS Data Interface](#ros-data-interface) - - [Published Topics](#published-topics) - - [Subscribed Topics](#subscribed-topics) -- [Node Creation + Running Tests](#node-creation--running-tests) +- [Collision Check](#collision-check) + - [Component](#component) + - [ROS Data Interface](#ros-data-interface) + - [Published Topics](#published-topics) + - [Subscribed Topics](#subscribed-topics) + - [Node Creation + Running Tests](#node-creation--running-tests) ## Component diff --git a/doc/planning/Global_Planner.md b/doc/planning/Global_Planner.md index 08a00760..94a66612 100644 --- a/doc/planning/Global_Planner.md +++ b/doc/planning/Global_Planner.md @@ -8,11 +8,12 @@ from preplanning_trajectory.py. In the end the computed trajectory and prevailin This component and so most of the documentation was taken from the previous project PAF22 (Authors: Simon Erlbacher, Niklas Vogel) -- [Getting started](#getting-started) -- [Description](#description) - - [Inputs](#inputs) - - [Outputs](#outputs) -- [Testing](#testing) +- [Global Planner](#global-planner) + - [Getting started](#getting-started) + - [Description](#description) + - [Inputs](#inputs) + - [Outputs](#outputs) + - [Testing](#testing) ## Getting started diff --git a/doc/research/paf24/planning/local_v_global_planning.md b/doc/research/paf24/planning/local_v_global_planning.md index 56042fe1..1add15e9 100644 --- a/doc/research/paf24/planning/local_v_global_planning.md +++ b/doc/research/paf24/planning/local_v_global_planning.md @@ -28,4 +28,5 @@ Local planning consists of the ACC, CollisionCheck and MotionPlanning nodes. It - 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 + - receives global trajectory from `/paf/hero/trajectory_global` + \ No newline at end of file