Calculate path curvature only once

[maxime-clem]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I've agreed with the maintainers that I can plan this task.

Description

Path curvature is an important information when planning and generating control commands.
Currently, this curvature is calculated in multiple different nodes, sometimes using different methods.

This task aims at calculating the curvature only once.

Purpose

Calculating the curvature only once will help avoid errors and will ensure all nodes use the same reference curvature in their calculations.

Possible approaches

• Add a curvature field to the TrajectoryPoint message (https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl).
• Add a recommended way to calculate curvatures in the documentation.
  • We may want to add common functions for curvature calculation and interpolation.
• Add curvature calculation in the obstacle_avoidance_planner to populate the curvatures of the output Trajectory message.
• Update the motion_velocity_smoother and the controllers to directly use the curvature of the Trajectory message.

Definition of done

• Add a curvature field to the TrajectoryPoint message (https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl).
• Add a recommended way to calculate curvatures in the documentation.
• Add curvature calculation in the obstacle_avoidance_planner to populate the curvatures of the output Trajectory message.
• Update the motion_velocity_smoother and the controllers to directly use the curvature of the Trajectory message.

[TakaHoribe]

@maxime-clem @brkay54 @boyali
Thank you for raising a nice discussion.

Currently curvature is calculated in several packages independently. The reason is as follows

First, the method of curvature calculation differs depending on the application of each module.

For example, motion_velocity_smother calculates the lateral acceleration from the curvature and do deceleration planning to prevent large lateral acceleration.
However, the motion_velocity_smoother is not interested in an exact curvature.
Say, in a long straight line, if there is very small noise at the path points due to calculation errors or insufficient algorithm performance, the exact curvature will be a very large value in a small interval. The motion_velocity_smother is not interested in such exact curvature. The motion_velocity_smoother is more interested in how much the path bends at roughly wheel_base intervals, so what the module needs is not exact curvature, but something like a curvature that is convenient for "deceleration on curve" planning.

On the other hand, the obstacle_avoidance_planner has a role to smooth the path. Such a module would require accurate curvature calculations since the smoothing is done with even very small curvature noises in mind.

Second, the curvature is redundant information since it can be recalculated from path shape (not perfectly, but enough accurate in practice).

This is a background for the current implementation. But this is defined more than 2 years ago, and we can discuss a new approach for the curvature calculation.
IMO, it is nice to have curvature information in trajectory if we can solve the first point.

---

At least, there are several methods proposed for curvature calculation, some of them are more accurate then the current implementation. I hope these methods are implemented in utility library (such as tier4_autoware_utils and available for all nodes.
Actually, there already is a problem due to an inaccurate curvature calculation in the control module (MPC controller).

[boyali]

The B-spline library was previously a part of the nonlinear model predictive control package. I based these libraries mainly on fast matrix computations and some numpy-like matrix concatenation functions. I tried to write helper functions by combining Numpy and Matlab-like semantics.

After starting to work on the Autoware control toolbox, I used the same helper functions in the body of control functions. If we put BSplines in autoware_util, there will be duplicate helper functions. Separating them would probably require redesign.

Here is a list of what is in these libraries:
BSpline library: (based on the books The Elements of Statistical Learning Chapter 5 Semiparametric Regression David Ruppert )

• Smoothing BSplines that can smooth any given path with a chosen smoothing parameter,
• Interpolating BSpline that does not smooth but interpolates.
• Pre-conditioned conjugate gradient-based cubic, or linear spline interpolator (exactly same as the Autoware version but uses Eigen matrix and vectors).
• Templated BSpline smoother, which is a template version of BSpline smoother. It uses a pre-determined Eigen matrix instead of dynamic versions to make the computations faster. It computes a coefficient matrix only once and uses the same matrix to smooth and interpolate the paths or curves.

** helper : act_utils**

• std::vector and scalar multiplication template
• absolute_difference for non-negative variables
• wrapToPi --> Matlab like using an exponential of complex numbers
• print_container variadic template that prints any std::vector and array
• Eigen matrix print variadic templates.
• Eigen matrix vstack, hstack methods
• Eigen matrix block_diag --> Matlab method,
• Strip leading zeros from a standard vector (for transfer functions)
• Join std vectors,
• Zero padding std vectors,
• Floating number comparison templates (isEqual, >0, <0)
• Cross product helper for curvature computations,
• Curvature method that uses differential geometric definitions,
• Eigen sparse triplet helper that converts a dense matrix to sparse by using triplet construction,
• Cholesky update for backstitution-based fast least-square solution.

Autoware Control Toolbox

• Transfer function classes, that can be multiplied, summed, or taken the power of.
• State-space classes that can be constructed from the transfer function with Bilinear discretization and matrix balancing algorithms.
• Signal builders that generate triangular, square, and sinusoidal signals for testing purposes.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.