2.2.2.3. motion_planning_node module¶

Motion Planning ROS node to plan a trajectory (path + velocity) based on centerpoints and vehicle pose.

class motion_planning_node.AutonomousMissionEnum(value)[source]¶

Bases: Enum

Enum to describe which Autonomous Mission is currently select.

ACCELERATION = 1¶

AUTOCROSS = 6¶

BRAKETEST = 4¶

INSPECTION = 5¶

MANUAL_DRIVING = 0¶

SKIDPAD = 2¶

TRACKDRIVE = 3¶

class motion_planning_node.MotionPlanning[source]¶

Bases: object

Motion Planning ROS node to plan a trajectory (path + velocity) based on centerpoints and vehicle pose.

mission_completed¶

Indicates whether the mission is already completed.

Type:: bool

vehicle_pose¶

Current vehicle pose estimated by SLAM.

Type:: VehiclePose

vehicle_pose_lock¶

Mutual exclusion lock for current vehicle pose estimated by SLAM.

Type:: threading.Lock

centerpoints_msg¶

Latest centerpoints filtered by track filtering.

Type:: CenterPoints

centerpoints_lock¶

Mutual exclusion lock for latest centerpoints filtered by track filtering.

Type:: threading.Lock

send_trajectory_lock¶

Mutual exclusion lock for publishing trajectory. Used to ensure that not a local trajectory is send after a global trajectory by mistake.

Type:: threading.Lock

optimizer_lock¶

Mutual exclusion lock for optimizing a global track. Check what it is used for.

Type:: threading.Lock

plan_locally_event¶

Threading event to signalize that a new centerpoints have been received for which should be planned locally.

Type:: threading.Event

plan_globally_event¶

Threading event to signalize that a new centerpoints have been received for which should be planned globally.

Type:: threading.Event

local_thread¶

Thread for planning locally.

Type:: threading.Thread

global_thread¶

Thread for planning globally.

Type:: threading.Thread

optimizer¶

Global Optimizer which has been used for optimizing a global trajectory successfully.

Type:: Optional[global_optimization.GlobalOptimizer]

send_debug_informations¶

Indicates whether debug informations should be send via ROS.

Type:: bool

last_planning_mode¶

Planning mode of last Motion Planning.

Type:: PlanningMode

mission¶

Mission which the vehicle currently drives.

Type:: AutonomousMissionEnum

optimize_skidpad_path¶

Whether to only calculate the velocity profile for skidpad or also optimize the path for skidpad.

Type:: bool

is_skidpad¶

Indicates whether the current autonomous mission is skidpad.

Type:: bool

max_local_track_length¶

Maximal track length for local paths for when they are limited.

Type:: float

min_local_track_gap¶

Mnimum gap between the nearest centerpoint and the vehicle for when centerpoints are limited.

Type:: float

lr_cog¶

Offset between rear axle and center of gravity (cog) of vehicle. Used for moving vehicle position to cog.

Type:: float

trajectory_publisher¶

ROS Publisher to publish trajectories.

Type:: rospy.Publisher

local_trajectory_path_slices_publisher¶

ROS Publisher to publish debug informations for local motion planner

Type:: rospy.Publisher

local_motion_planner_config¶

Configuration for local motion planner.

Type:: local_motion_planner.LocalMotionPlannerConfig

global_optimizer_config¶

Configuration for global optimizer.

Type:: global_optimization.GlobalOptimizerConfig

global_velocity_config¶

Configuration for velocity profiler for global planning.

Type:: global_velocity.VelocityConfig

local_velocity_config¶

Configuration for velocity profiler for local planning.

Type:: local_velocity_profile.VelocityProfileConfig

Initialize motion planning ROS node.

centerpoints_callback(centerpoints_msg: CenterPoints)[source]¶

Buffer centerpoints calculated by track filtering and received via ROS.

Parameters:: centerpoints_msg (CenterPoints) – Received centerpoints message.

check_msg_changed(centerpoints_msg_old, centerpoints_msg)[source]¶

Check if the centerpoints message has changed.

Parameters:

centerpoints_msg_old (CenterPoints) – Recently received centerpoints message.
centerpoints_msg (CenterPoints) – Received centerpoints message.

Returns:

msg_changed – Indicates whether the centerpoints message has changed.

Return type:

bool

find_nearest_centerpoint(centerpoints: ndarray[Any, dtype[ScalarType]], vehicle_position: ndarray[Any, dtype[ScalarType]]) → int[source]¶

Find the nearest centerpoint to the current vehicle position in a list of centerpoint which.

Centerpoint must be atleast self.min_local_track_gap away.

Parameters:

centerpoints (npt.NDArray) – Centerpoints for which the nearest should be found, shape: (n, 2) with n equal number of centerpoints.
vehicle_position (npt.NDArray) – Current vehicle position, shape: (2, ).

Returns:

Index oif nearest centerpoint.

Return type:

int

limit_global_centerpoints_by_cutting(centerpoints: ndarray[Any, dtype[ScalarType]], vehicle_position: ndarray[Any, dtype[ScalarType]], max_length: bool = True) → Tuple[int, int | None][source]¶

Limit global and open centerpoints by cutting the centerpoints.

First find the nearest centerpoint. All centerpoints forward are kept, except when max_length is defined as centerline would then be longer than self.max_local_track_length. In this case they get cut too.

Parameters:

centerpoints (npt.NDArray) – Centerpoints which should be limited, shape, (n, 2) with n equal number of centerpoints.
vehicle_position (npt.NDArray) – Current vehicle position, shape: (2, ).
max_length (bool, optional) – Indicates whether centerline should not be longer than self.max_local_track_length, by default True

Returns:

Index of nearest centerpoint and Index of last centerpoint

Index of last centerpoint is None if max_length is not defined or centerline is not longer self.max_local_track_length.

Return type:

Tuple[int, Optional[int]]

limit_global_centerpoints_by_rolling(centerpoints: ndarray[Any, dtype[ScalarType]], vehicle_position: ndarray[Any, dtype[ScalarType]]) → Tuple[int, int][source]¶

Limit global and closed centerpoints by rolling the centerpoints.

First find the nearest centerpoint. Roll centerpoints so that nearest centerpoints is the first one. Calculate the cumulative distance for that an find the index where the cumulative distance is greater than self.max_local_track_length.

Parameters:

centerpoints (npt.NDArray) – Centerpoints which should be limited, shape: (n, 2) with n equal number of centerpoints.
vehicle_position (npt.NDArray) – Current vehicle position, shape: (2, ).

Returns:

Index of nearest centerpoint of original centerpoints and Index of last centerpoint of rolled centerpoints.

Return type:

Tuple[int, int]

mission_completed_callback(msg: MissionFinished)[source]¶

Buffer whether the mission is completed calculated by finish line detection and received via ROS.

A completed mission signals that we should brake as fast as possible and allowed.

Parameters:: msg (MissionFinished) – Message indicating whether mission is completed.

static optimize_globally(pipe: Connection, global_motion_planner_config: GlobalMotionPlannerConfig, gg_handler: GGManager)[source]¶

Optimize closed path globally.

Parameters:

pipe (mpc.Connection) – Pipe where to receive the centerpoints message from.
global_motion_planner_config (global_motion_planner.GlobalMotionPlannerConfig) – Configuration for global motion planner config.
gg_handler (gg_manager.GGManager) – Class for managing for ggV constraints.

plan_globally()[source]¶: Plan motion globally as soon as event is set.

plan_locally()[source]¶: Plan motion locally as soon as event is set.

plan_motion_globally() → Tuple[GlobalMotionPlanner, ndarray[Any, dtype[ScalarType]], Header][source]¶

Plan and calculate trajectory for a global and closed track based on centerpoints.

Returns:: GlobalOptimizer used for Optimization, global optimized trajectory (shape: (n, 3) with n equal number of centerpoints) and header of the centerpoints for which the trajectory has been calculated for.
Return type:: Tuple[global_optimization.GlobalOptimizer, npt.NDArray, Header]

plan_motion_locally() → Tuple[LocalMotionPlanner, Header, Header][source]¶

Plan and calculate trajectory for a local track based on centerpoints.

Returns:: LocalMotionPlanner object used for planning, Header of Vehicle Pose used for planning and Header of Centerpoints used for planning
Return type:: Tuple[local_motion_planner.LocalMotionPlanner, Header, Header]

plan_skidpad(centerpoints: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]¶

Calculate trajectory for skidpad based on centerpoints and maximum lateral acceleration.

Parameters:: centerpoints (npt.NDArray) – Centerpoints for which the velocity should be calculated, shape: (n, 2) with n equal number of centerpoints.
Returns:: Trajectory contianing centerpoints and velocity, shape: (n, 3) with n equal number of centerpoints.
Return type:: npt.NDArray

send_local_trajectory_debug_informations(centerpoints_image_header: Header, vehicle_pose_header: Header, local_motion_planner_obj: LocalMotionPlanner)[source]¶

Publish debug informations for local motion planning.

Parameters:

centerpoints_image_header (Header) – Header of centerpoints for which the trajectory has been planned.
vehicle_pose_header (Header) – Header of vehicle pose for which the trajectory has been planned.
local_motion_planner_obj (local_motion_planner.LocalMotionPlanner) – Local Motion Planner object which was used to plan the trajectory.

send_trajectory(trajectory: ndarray[Any, dtype[ScalarType]], closed: bool, centerpoints_image_header: Header, vehicle_pose_header: Header | None = None) → Time[source]¶

Publish planned trajectory on ROS.

Parameters:

trajectory (npt.NDArray) – Trajectory to be published, shape: (n, 3) with n equal number of points of trajectory.
closed (bool) – Indicates whether the trajectory is closed.
centerpoints_image_header (Header) – Header of the centerpoints for which the trajectory has been planned.
vehicle_pose_header (Optional[Header], optional) – Header of the vehicle pose for which the trajectory has been planned, by default None

Returns:

Time of trajectory header.

Return type:

rospy.Time

set_planning_mode(closed_track: bool)[source]¶

Set planning mode according to current state.

Todo

Enable planning finishing while driving skidpad. Control Node needs to support it first.

Parameters:: closed_track (bool) – Indicates whether the track represented by the centerpoints is closed.

vehicle_pose_callback(msg: VehiclePose)[source]¶

Buffer vehicle pose estimated by SLAM and received via SLAM.

Also moves vehicle position from rear axle to center of gravity.

Todo

Get offset between rear axle and center of gravity from transform tree.

Parameters:: msg (VehiclePose) – Current vehicle position estimated by SLAM and received via SLAM.

class motion_planning_node.PlanningMode(value)[source]¶

Bases: Enum

Enum to describe which Planning Mode is currently used.

Table 2.1 Planning Modes¶
Value	Name	Meaning
0	NO	Nothing is planned yet
1	LOCAL	Currently only local tracks are planned
2	PARALLEL	Currently a global track is optimized and parallel local tracks are still planned
3	GLOBAL	A global, closed track is already been optimized, either optimizing another or waiting for one
4	SKIDPAD	Global skidpad tracks are being planned
5	FINISH	Motion is planned to brake as fast as possible and allowed

FINISH = 5¶

GLOBAL = 3¶

LOCAL = 1¶

NO = 0¶

PARALLEL = 2¶

SKIDPAD = 4¶