[Proposal] Move some velocity planning modules from behavior to motion planning

[maxime-clem]

New motion planning module to modify the velocity profile of the ego trajectory

Main motivation: use the final Trajectory shape for calculation rather than the approximate Path.

• Inputs: trajectory, dynamic objects, additional constraints.
• Output: trajectory with an updated velocity profile.

Desired features

• Can modify the trajectory velocity for safer driving.
• Modular (can easily add/remove modules, similar to the behavior_velocity modules).
• No dependency on the lanelet map.
  • This is hard to achieve so I am planning an initial prototype where some modules will still rely on the lanelet map.

Updated motion planning architecture

Running after the obstacle_avoidance_planner allows using the final shape of the Trajectory for more accurate collision calculations.

Internal Interface

Very similar to the behavior_velocity_planner. Each module is a plugin that can easily be enabled or disabled. These modules follow a same interface and uses the same input planning data (ego state, dynamic objects, map, etc).

Target Modules

Here are modules that I propose to initially move to the motion_velocity_planner.

• out_of_lane
• dynamic_obstacle_stop
• obstacle_velocity_limiter

Open questions

I think this new node makes sense to enable more accurate collision detection, but it blurs the difference between behavior and motion velocity planning. Thus the questions are:

• what prevents us from moving all behavior_velocity_planner modules to the motion planning side ?
• what differentiate behavior and motion planning ?

Previous considerations

Fixing the path shape before the behavior_velocity_planner

We could keep the behavior_velocity_planner as it is but improve its input Path to be more accurate.
This would require doing some potentially heavy optimization in the behavior_path_planner so this approach was skipped.

Directly moving the behavior_velocity_planner to the motion planning

Requires heavy interface changes and would completely break the current architecture. Since this is too big of a change this idea was skipped.

Forbid using the vector map in the proposed motion_velocity_planner

Instead of directly using the vector map, we could use an intermediate node that provide necessary information to the motion_velocity_planner. For example, the out_of_lane only cares about the current ego lane, and the obstacle_velocity_limiter only cares about static obstacles encoded in the vector map. An intermediate node could extract these geometries from the map and send it to the motion_velocity_planner.
Then the node can still be used without having a vector map and the geometries can be provided by other nodes (e.g., perception).
This idea was skipped. Instead, the motion_velocity_planner directly uses the vector map but will still be able to function without it. Only some modules will require the vector map to run.

[hzkkk1997]

@maxime-clem Hello, I am very sorry to bother you. After updating the autoware version, I found that a new motion_velocity_planner node was added, and this node also has an optional configuration parameter smooth_velocity_before_planning for velocity smoothing. I feel that the velocity smoothing is repeated, may I ask if there is any better effect? I noticed that this time the speed smoothing method used the analytical method.

[maxime-clem]

Velocity smoothing in the motion_velocity_planner is done in order to calculate "accurate" time_from_start fields (https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_planning_msgs/msg/TrajectoryPoint.msg#L1).
This field is used by some modules (e.g., out_of_lane), to calculate time to collision values along the trajectory.
Please note that the smoothed velocities are not part of the output of the motion_velocity_planner and are only used internally by the different modules.

You are right that the velocity smoothing will be done again by the velocity_smoother node, but this time the smoothed velocities will be output by the node and used to control the vehicle.

[hzkkk1997]

@maxime-clem Thank you very much for your reply. May I ask whether the speed smoothing method in motion_velocity_planner can also choose to use the method of quadratic optimization?
In addition, I think if you put this smooth speed in the topic also sent out. Is it possible to avoid the subsequent calculation of the speed smoothing module to reduce the calculation consumption?

[maxime-clem]

motion_velocity_planner can also choose to use the method of quadratic optimization?

It would be nice to add a parameter to choose the smoothing method. I will add this feature when I find the time.

Is it possible to avoid the subsequent calculation of the speed smoothing module to reduce the calculation consumption?

Since the motion_velocity_planner adds stop or slowdown points to the trajectory, adding more constraints on the velocity profile, another step of smoothing is required to adhere to these new constraints.
Therefore it is not possible to avoid doing the smoothing an extra time.

[hzkkk1997]

Thank you very much, this is very helpful to me！