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 378d94bd..806ac81e 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 intern
components that is needed for the preplanning component.
@@ -28,7 +30,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.