diff --git a/planning/autoware_velocity_smoother/CMakeLists.txt b/planning/autoware_velocity_smoother/CMakeLists.txt new file mode 100644 index 0000000000..9dd0c62c45 --- /dev/null +++ b/planning/autoware_velocity_smoother/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_velocity_smoother) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Boost REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +set(MOTION_VELOCITY_SMOOTHER_SRC + src/node.cpp +) + +set(SMOOTHER_SRC + src/smoother/smoother_base.cpp + src/smoother/l2_pseudo_jerk_smoother.cpp + src/smoother/linf_pseudo_jerk_smoother.cpp + src/smoother/jerk_filtered_smoother.cpp + src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp + src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp + src/trajectory_utils.cpp + src/resample.cpp +) + +ament_auto_add_library(smoother SHARED + ${SMOOTHER_SRC} +) + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + ${MOTION_VELOCITY_SMOOTHER_SRC} +) + +target_link_libraries(${PROJECT_NAME}_node + smoother +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware::velocity_smoother::VelocitySmootherNode" + EXECUTABLE velocity_smoother_node +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_smoother_functions + test/test_smoother_functions.cpp + ) + target_link_libraries(test_smoother_functions + smoother + ) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_velocity_smoother_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME}_node + ) +endif() + + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/planning/autoware_velocity_smoother/README.md b/planning/autoware_velocity_smoother/README.md new file mode 100644 index 0000000000..1a506d8612 --- /dev/null +++ b/planning/autoware_velocity_smoother/README.md @@ -0,0 +1,275 @@ +# Velocity Smoother + +## Purpose + +`autoware_velocity_smoother` outputs a desired velocity profile on a reference trajectory. +This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality. +We call this module `autoware_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile. + +## Inner-workings / Algorithms + +### Flow chart + +![motion_velocity_smoother_flow](./media/motion_velocity_smoother_flow.drawio.svg) + +#### Extract trajectory + +For the point on the reference trajectory closest to the center of the rear wheel axle of the vehicle, it extracts the reference path between `extract_behind_dist` behind and `extract_ahead_dist` ahead. + +#### Apply external velocity limit + +It applies the velocity limit input from the external of `autoware_velocity_smoother`. +Remark that the external velocity limit is different from the velocity limit already set on the map and the reference trajectory. +The external velocity is applied at the position that it is able to reach the velocity limit with the deceleration and the jerk constraints set as the parameter. + +#### Apply stop approaching velocity + +It applies the velocity limit near the stopping point. +This function is used to approach near the obstacle or improve the accuracy of stopping. + +#### Apply lateral acceleration limit + +It applies the velocity limit to decelerate at the curve. +It calculates the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`. +The velocity limit is set as not to fall under `min_curve_velocity`. + +Note: velocity limit that requests larger than `nominal.jerk` is not applied. In other words, even if a sharp curve is planned just in front of the ego, no deceleration is performed. + +#### Apply steering rate limit + +It calculates the desired steering angles of trajectory points. and it applies the steering rate limit. If the (`steering_angle_rate` > `max_steering_angle_rate`), it decreases the velocity of the trajectory point to acceptable velocity. + +#### Resample trajectory + +It resamples the points on the reference trajectory with designated time interval. +Note that the range of the length of the trajectory is set between `min_trajectory_length` and `max_trajectory_length`, and the distance between two points is longer than `min_trajectory_interval_distance`. +It samples densely up to the distance traveled between `resample_time` with the current velocity, then samples sparsely after that. +By sampling according to the velocity, both calculation load and accuracy are achieved since it samples finely at low velocity and coarsely at high velocity. + +#### Calculate initial state + +Calculate initial values for velocity planning. +The initial values are calculated according to the situation as shown in the following table. + +| Situation | Initial velocity | Initial acceleration | +| ------------------------------------------------------------- | ---------------------- | ---------------------- | +| First calculation | Current velocity | 0.0 | +| Engaging | `engage_velocity` | `engage_acceleration` | +| Deviate between the planned velocity and the current velocity | Current velocity | Previous planned value | +| Normal | Previous planned value | Previous planned value | + +#### Smooth velocity + +It plans the velocity. +The algorithm of velocity planning is chosen from `JerkFiltered`, `L2` and `Linf`, and it is set in the launch file. +In these algorithms, they use OSQP[1] as the solver of the optimization. + +##### JerkFiltered + +It minimizes the sum of the minus of the square of the velocity and the square of the violation of the velocity limit, the acceleration limit and the jerk limit. + +##### L2 + +It minimizes the sum of the minus of the square of the velocity, the square of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit. + +##### Linf + +It minimizes the sum of the minus of the square of the velocity, the maximum absolute value of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit. + +#### Post process + +It performs the post-process of the planned velocity. + +- Set zero velocity ahead of the stopping point +- Set maximum velocity given in the config named `max_velocity` +- Set velocity behind the current pose +- Resample trajectory (`post resampling`) +- Output debug data + +After the optimization, a resampling called `post resampling` is performed before passing the optimized trajectory to the next node. Since the required path interval from optimization may be different from the one for the next module, `post resampling` helps to fill this gap. Therefore, in `post resampling`, it is necessary to check the path specification of the following module to determine the parameters. Note that if the computational load of the optimization algorithm is high and the path interval is sparser than the path specification of the following module in the first resampling, `post resampling` would resample the trajectory densely. On the other hand, if the computational load of the optimization algorithm is small and the path interval is denser than the path specification of the following module in the first resampling, the path is sparsely resampled according to the specification of the following module. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | + +### Output + +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_steering_rate_limited` | `autoware_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | + +## Parameters + +### Constraint parameters + +| Name | Type | Description | Default value | +| :------------- | :------- | :--------------------------------------------- | :------------ | +| `max_velocity` | `double` | Max velocity limit [m/s] | 20.0 | +| `max_accel` | `double` | Max acceleration limit [m/ss] | 1.0 | +| `min_decel` | `double` | Min deceleration limit [m/ss] | -0.5 | +| `stop_decel` | `double` | Stop deceleration value at a stop point [m/ss] | 0.0 | +| `max_jerk` | `double` | Max jerk limit [m/sss] | 1.0 | +| `min_jerk` | `double` | Min jerk limit [m/sss] | -0.5 | + +### External velocity limit parameter + +| Name | Type | Description | Default value | +| :----------------------------------------- | :------- | :---------------------------------------------------- | :------------ | +| `margin_to_insert_external_velocity_limit` | `double` | margin distance to insert external velocity limit [m] | 0.3 | + +### Curve parameters + +| Name | Type | Description | Default value | +| :------------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `enable_lateral_acc_limit` | `bool` | To toggle the lateral acceleration filter on and off. You can switch it dynamically at runtime. | true | +| `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 0.5 | +| `min_curve_velocity` | `double` | Min velocity at lateral acceleration limit [m/ss] | 2.74 | +| `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5 | +| `decel_distance_after_curve` | `double` | Distance to slowdown after a curve for lateral acceleration limit [m] | 2.0 | +| `min_decel_for_lateral_acc_lim_filter` | `double` | Deceleration limit to avoid sudden braking by the lateral acceleration filter [m/ss]. Strong limitation degrades the deceleration response to the appearance of sharp curves due to obstacle avoidance, etc. | -2.5 | + +### Engage & replan parameters + +| Name | Type | Description | Default value | +| :----------------------------- | :------- | :--------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `replan_vel_deviation` | `double` | Velocity deviation to replan initial velocity [m/s] | 5.53 | +| `engage_velocity` | `double` | Engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) | 0.25 | +| `engage_acceleration` | `double` | Engage acceleration [m/ss] (use this acceleration when engagement) | 0.1 | +| `engage_exit_ratio` | `double` | Exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. | 0.5 | +| `stop_dist_to_prohibit_engage` | `double` | If the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] | 0.5 | + +### Stopping velocity parameters + +| Name | Type | Description | Default value | +| :------------------ | :------- | :------------------------------------------------------------------------------------ | :------------ | +| `stopping_velocity` | `double` | change target velocity to this value before v=0 point [m/s] | 2.778 | +| `stopping_distance` | `double` | distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. | 0.0 | + +### Extraction parameters + +| Name | Type | Description | Default value | +| :-------------------- | :------- | :-------------------------------------------------------------- | :------------ | +| `extract_ahead_dist` | `double` | Forward trajectory distance used for planning [m] | 200.0 | +| `extract_behind_dist` | `double` | backward trajectory distance used for planning [m] | 5.0 | +| `delta_yaw_threshold` | `double` | Allowed delta yaw between ego pose and trajectory pose [radian] | 1.0472 | + +### Resampling parameters + +| Name | Type | Description | Default value | +| :----------------------------- | :------- | :----------------------------------------------------- | :------------ | +| `max_trajectory_length` | `double` | Max trajectory length for resampling [m] | 200.0 | +| `min_trajectory_length` | `double` | Min trajectory length for resampling [m] | 30.0 | +| `resample_time` | `double` | Resample total time [s] | 10.0 | +| `dense_dt` | `double` | resample time interval for dense sampling [s] | 0.1 | +| `dense_min_interval_distance` | `double` | minimum points-interval length for dense sampling [m] | 0.1 | +| `sparse_dt` | `double` | resample time interval for sparse sampling [s] | 0.5 | +| `sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 4.0 | + +### Resampling parameters for post process + +| Name | Type | Description | Default value | +| :---------------------------------- | :------- | :----------------------------------------------------- | :------------ | +| `post_max_trajectory_length` | `double` | max trajectory length for resampling [m] | 300.0 | +| `post_min_trajectory_length` | `double` | min trajectory length for resampling [m] | 30.0 | +| `post_resample_time` | `double` | resample total time for dense sampling [s] | 10.0 | +| `post_dense_dt` | `double` | resample time interval for dense sampling [s] | 0.1 | +| `post_dense_min_interval_distance` | `double` | minimum points-interval length for dense sampling [m] | 0.1 | +| `post_sparse_dt` | `double` | resample time interval for sparse sampling [s] | 0.1 | +| `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0 | + +### Limit steering angle rate parameters + +| Name | Type | Description | Default value | +| :------------------------------- | :------- | :------------------------------------------------------------------------------------ | :------------ | +| `enable_steering_rate_limit` | `bool` | To toggle the steer rate filter on and off. You can switch it dynamically at runtime. | true | +| `max_steering_angle_rate` | `double` | Maximum steering angle rate [degree/s] | 40.0 | +| `resample_ds` | `double` | Distance between trajectory points [m] | 0.1 | +| `curvature_threshold` | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02 | +| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m] | 1.0 | + +### Weights for optimization + +#### JerkFiltered + +| Name | Type | Description | Default value | +| :-------------- | :------- | :------------------------------------ | :------------ | +| `jerk_weight` | `double` | Weight for "smoothness" cost for jerk | 10.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel limit" cost | 5000.0 | +| `over_j_weight` | `double` | Weight for "over jerk limit" cost | 1000.0 | + +#### L2 + +| Name | Type | Description | Default value | +| :------------------- | :------- | :--------------------------------- | :------------ | +| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost | 100.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel limit" cost | 1000.0 | + +#### Linf + +| Name | Type | Description | Default value | +| :------------------- | :------- | :--------------------------------- | :------------ | +| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost | 100.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel limit" cost | 1000.0 | + +### Others + +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | +| `over_stop_velocity_warn_thr` | `double` | Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] | 1.389 | + + + +## Assumptions / Known limits + +- Assume that the velocity limit or the stopping point is properly set at the point on the reference trajectory +- If the velocity limit set in the reference path cannot be achieved by the designated constraints of the deceleration and the jerk, decelerate while suppressing the velocity, the acceleration and the jerk deviation as much as possible +- The importance of the deviations is set in the config file + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +[1] B. Stellato, et al., "OSQP: an operator splitting solver for quadratic programs", Mathematical Programming Computation, 2020, [10.1007/s12532-020-00179-2](https://link.springer.com/article/10.1007/s12532-020-00179-2). + +[2] Y. Zhang, et al., "Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization", Sensors, vol. 18, no. 7, p. 2185, 2018, [10.3390/s18072185](https://doi.org/10.3390/s18072185) + +## (Optional) Future extensions / Unimplemented parts diff --git a/planning/autoware_velocity_smoother/config/Analytical.param.yaml b/planning/autoware_velocity_smoother/config/Analytical.param.yaml new file mode 100644 index 0000000000..329714e3d3 --- /dev/null +++ b/planning/autoware_velocity_smoother/config/Analytical.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + resample: + ds_resample: 0.1 + num_resample: 1 + delta_yaw_threshold: 0.785 + + latacc: + enable_constant_velocity_while_turning: false + constant_velocity_dist_threshold: 2.0 + + forward: + max_acc: 1.0 + min_acc: -1.0 + max_jerk: 0.3 + min_jerk: -0.3 + kp: 0.3 + + backward: + start_jerk: -0.1 + min_jerk_mild_stop: -0.3 + min_jerk: -1.5 + min_acc_mild_stop: -1.0 + min_acc: -2.5 + span_jerk: -0.01 diff --git a/planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml b/planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml new file mode 100644 index 0000000000..78c088b084 --- /dev/null +++ b/planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + jerk_weight: 10.0 # weight for "smoothness" cost for jerk + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 5000.0 # weight for "over accel limit" cost + over_j_weight: 2000.0 # weight for "over jerk limit" cost + jerk_filter_ds: 0.1 # resampling ds for jerk filter diff --git a/planning/autoware_velocity_smoother/config/L2.param.yaml b/planning/autoware_velocity_smoother/config/L2.param.yaml new file mode 100644 index 0000000000..c993978204 --- /dev/null +++ b/planning/autoware_velocity_smoother/config/L2.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 100.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 1000.0 # weight for "over accel limit" cost diff --git a/planning/autoware_velocity_smoother/config/Linf.param.yaml b/planning/autoware_velocity_smoother/config/Linf.param.yaml new file mode 100644 index 0000000000..ec38010621 --- /dev/null +++ b/planning/autoware_velocity_smoother/config/Linf.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 200.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 5000.0 # weight for "over accel limit" cost diff --git a/planning/autoware_velocity_smoother/config/default_common.param.yaml b/planning/autoware_velocity_smoother/config/default_common.param.yaml new file mode 100644 index 0000000000..be0667c158 --- /dev/null +++ b/planning/autoware_velocity_smoother/config/default_common.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + + normal: + min_acc: -0.5 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml b/planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml new file mode 100644 index 0000000000..21ad85137e --- /dev/null +++ b/planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml @@ -0,0 +1,63 @@ +/**: + ros__parameters: + # motion state constraints + max_velocity: 20.0 # max velocity limit [m/s] + stop_decel: 0.0 # deceleration at a stop point[m/ss] + + # external velocity limit parameter + margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] + + # ---- curve parameters ---- + # - common parameters - + curvature_calculation_distance: 5.0 # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m] + # - lateral acceleration limit parameters - + enable_lateral_acc_limit: true # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime. + max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] + decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit + decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit + min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] + # - steering angle rate limit parameters - + enable_steering_rate_limit: true # To toggle the steer rate filter on and off. You can switch it dynamically at runtime. + max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] + resample_ds: 0.1 # distance between trajectory points [m] + curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] + + # engage & replan parameters + replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s] + engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) + engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. + stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] + + # stop velocity + stopping_velocity: 2.778 # change target velocity to this value before v=0 point [m/s] + stopping_distance: 0.0 # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. + + # path extraction parameters + extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m] + extract_behind_dist: 5.0 # backward trajectory distance used for planning [m] + delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajectory pose [radian] + + # resampling parameters for optimization + max_trajectory_length: 200.0 # max trajectory length for resampling [m] + min_trajectory_length: 150.0 # min trajectory length for resampling [m] + resample_time: 2.0 # resample total time for dense sampling [s] + dense_resample_dt: 0.2 # resample time interval for dense sampling [s] + dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + sparse_resample_dt: 0.5 # resample time interval for sparse sampling [s] + sparse_min_interval_distance: 4.0 # minimum points-interval length for sparse sampling [m] + + # resampling parameters for post process + post_max_trajectory_length: 300.0 # max trajectory length for resampling [m] + post_min_trajectory_length: 30.0 # min trajectory length for resampling [m] + post_resample_time: 10.0 # resample total time for dense sampling [s] + post_dense_resample_dt: 0.1 # resample time interval for dense sampling [s] + post_dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] + post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] + + # system + over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point + + plan_from_ego_speed_on_manual_mode: true # planning is done from ego velocity/acceleration on MANUAL mode. This should be true for smooth transition from MANUAL to AUTONOMOUS, but could be false for debugging. diff --git a/planning/autoware_velocity_smoother/config/rqt_multiplot.xml b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml new file mode 100644 index 0000000000..694f18c0d9 --- /dev/null +++ b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml @@ -0,0 +1,317 @@ + + + + #babdb6 + #000000 + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_velocity + std_msgs/Float32 + + + + #000000 + 1 + + + 1000 + 10 + 0 + + + 100 + Optimum Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_merged_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_merged_velocity + std_msgs/Float32 + + + + #73d216 + 1 + + + 1000 + 10 + 0 + + + 100 + Jerk Filtered Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_max_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_max_velocity + std_msgs/Float32 + + + + #75507b + 1 + + + 1000 + 10 + 0 + + + 100 + Maximum Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_acceleration + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_acceleration + std_msgs/Float32 + + + + #ef2929 + 1 + + + 1000 + 10 + 0 + + + 100 + Acceleration + + + + true + + 30 + Velocity + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/calculation_time + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/calculation_time + std_msgs/Float32 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Calculation Time + + + + true + + 30 + Calculation Time + + + + false +
+ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp new file mode 100644 index 0000000000..7380f6a09b --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -0,0 +1,296 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ + +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "autoware/velocity_smoother/resample.hpp" +#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" +#include "autoware_utils/ros/diagnostics_interface.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/self_pose_listener.hpp" +#include "autoware_utils/system/stop_watch.hpp" +#include "autoware_utils/system/time_keeper.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_ros/transform_listener.h" + +#include + +#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_internal_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_internal_planning_msgs::msg::VelocityLimit; // temporary +using autoware_utils::DiagnosticsInterface; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::Odometry; +using visualization_msgs::msg::MarkerArray; + +struct Motion +{ + double vel = 0.0; + double acc = 0.0; + + Motion() {} + Motion(const double v, const double a) : vel(v), acc(a) {} +}; + +class VelocitySmootherNode : public rclcpp::Node +{ +public: + explicit VelocitySmootherNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; + rclcpp::Subscription::SharedPtr sub_current_trajectory_; + autoware_utils::InterProcessPollingSubscriber sub_current_odometry_{ + this, "/localization/kinematic_state"}; + autoware_utils::InterProcessPollingSubscriber + sub_current_acceleration_{this, "~/input/acceleration"}; + autoware_utils::InterProcessPollingSubscriber< + VelocityLimit, autoware_utils::polling_policy::Newest> + sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"}; + autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/operation_mode_state"}; + + Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry + AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_; + VelocityLimit::ConstSharedPtr external_velocity_limit_ptr_{ + nullptr}; // external velocity limit message + Trajectory::ConstSharedPtr base_traj_raw_ptr_; // current base_waypoints + double max_velocity_with_deceleration_; // maximum velocity with deceleration + // for external velocity limit + double wheelbase_; // wheelbase + double base_link2front_; // base_link to front + + TrajectoryPoints prev_output_; // previously published trajectory + + // previous trajectory point closest to ego vehicle + boost::optional prev_closest_point_{}; + boost::optional current_closest_point_from_prev_output_{}; + + bool is_reverse_; + + // check if the vehicle is under control of the planning module + OperationModeState operation_mode_; + + enum class AlgorithmType { + INVALID = 0, + JERK_FILTERED = 1, + L2 = 2, + LINF = 3, + ANALYTICAL = 4, + }; + + enum class InitializeType { + EGO_VELOCITY = 0, + LARGE_DEVIATION_REPLAN = 1, + ENGAGING = 2, + NORMAL = 3, + }; + + struct Param + { + bool enable_lateral_acc_limit; + bool enable_steering_rate_limit; + + double max_velocity; // max velocity [m/s] + double margin_to_insert_external_velocity_limit; // for external velocity limit [m] + double replan_vel_deviation; // if speed error exceeds this [m/s], + // replan from current velocity + double engage_velocity; // use this speed when start moving [m/s] + double engage_acceleration; // use this acceleration when start moving [m/ss] + double engage_exit_ratio; // exit engage sequence + // when the speed exceeds ratio x engage_vel. + double stopping_velocity; // change target velocity to this value before v=0 point. + double stopping_distance; // distance for the stopping_velocity + double extract_ahead_dist; // forward waypoints distance from current position [m] + double extract_behind_dist; // backward waypoints distance from current position [m] + double stop_dist_to_prohibit_engage; // prevent to move toward close stop point + double ego_nearest_dist_threshold; // for ego's closest index calculation + double ego_nearest_yaw_threshold; // for ego's closest index calculation + + resampling::ResampleParam post_resample_param; + AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2 + + bool plan_from_ego_speed_on_manual_mode = true; + } node_param_{}; + + struct AccelerationRequest + { + bool request{false}; + double max_acceleration{0.0}; + double max_jerk{0.0}; + }; + struct ExternalVelocityLimit + { + double velocity{0.0}; // current external_velocity_limit + double dist{0.0}; // distance to set external velocity limit + AccelerationRequest acceleration_request; + std::string sender{""}; + }; + ExternalVelocityLimit + external_velocity_limit_; // velocity and distance constraint of external velocity limit + + std::shared_ptr smoother_; + + bool publish_debug_trajs_; // publish planned trajectories + + double over_stop_velocity_warn_thr_; // threshold to publish over velocity warn + + mutable rclcpp::Clock::SharedPtr clock_; + + void setupSmoother(const double wheelbase); + + // parameter update + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + // topic callback + void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg); + + void calcExternalVelocityLimit(); + + // publish methods + void publishTrajectory(const TrajectoryPoints & traj) const; + + void publishStopDistance(const TrajectoryPoints & trajectory) const; + + // non-const methods + void publishClosestState(const TrajectoryPoints & trajectory); + + void updatePrevValues(const TrajectoryPoints & final_result); + + // const methods + bool checkData() const; + + void updateDataForExternalVelocityLimit(); + + AlgorithmType getAlgorithmType(const std::string & algorithm_name) const; + + TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & traj_input) const; + + bool smoothVelocity( + const TrajectoryPoints & input, const size_t input_closest, + TrajectoryPoints & traj_smoothed) const; + + std::pair calcInitialMotion( + const TrajectoryPoints & input_traj, const size_t input_closest) const; + + void applyExternalVelocityLimit(TrajectoryPoints & traj) const; + + void insertBehindVelocity( + const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const; + + void applyStopApproachingVelocity(TrajectoryPoints & traj) const; + + void overwriteStopPoint(const TrajectoryPoints & input, TrajectoryPoints & output) const; + + double calcTravelDistance() const; + + bool isEngageStatus(const double target_vel) const; + + void publishDebugTrajectories(const std::vector & debug_trajectories) const; + + void publishClosestVelocity( + const TrajectoryPoints & trajectory, const Pose & current_pose, + const rclcpp::Publisher::SharedPtr pub) const; + + Trajectory toTrajectoryMsg( + const TrajectoryPoints & points, const std_msgs::msg::Header * header = nullptr) const; + + TrajectoryPoint calcProjectedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & pose) const; + TrajectoryPoint calcProjectedTrajectoryPointFromEgo(const TrajectoryPoints & trajectory) const; + + // parameter handling + void initCommonParam(); + + // debug + autoware_utils::StopWatch stop_watch_; + std::shared_ptr prev_time_; + double prev_acc_; + rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; + rclcpp::Publisher::SharedPtr pub_trajectory_raw_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_trajectory_vel_lim_; + rclcpp::Publisher::SharedPtr pub_trajectory_latacc_filtered_; + rclcpp::Publisher::SharedPtr pub_trajectory_steering_rate_limited_; + rclcpp::Publisher::SharedPtr pub_trajectory_resampled_; + rclcpp::Publisher::SharedPtr debug_closest_velocity_; + rclcpp::Publisher::SharedPtr debug_closest_acc_; + rclcpp::Publisher::SharedPtr debug_closest_jerk_; + rclcpp::Publisher::SharedPtr debug_calculation_time_; + rclcpp::Publisher::SharedPtr debug_closest_max_velocity_; + rclcpp::Publisher::SharedPtr debug_processing_time_detail_; + + // For Jerk Filtered Algorithm Debug + rclcpp::Publisher::SharedPtr pub_forward_filtered_trajectory_; + rclcpp::Publisher::SharedPtr pub_backward_filtered_trajectory_; + rclcpp::Publisher::SharedPtr pub_merged_filtered_trajectory_; + rclcpp::Publisher::SharedPtr pub_closest_merged_velocity_; + + // helper functions + size_t findNearestIndexFromEgo(const TrajectoryPoints & points) const; + bool isReverse(const TrajectoryPoints & points) const; + void flipVelocity(TrajectoryPoints & points) const; + void publishStopWatchTime(); + + std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; + + mutable std::shared_ptr time_keeper_{nullptr}; + + std::unique_ptr diagnostics_interface_{nullptr}; +}; +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp new file mode 100644 index 0000000000..40081e1c79 --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include + +#include + +namespace autoware::velocity_smoother +{ +namespace resampling +{ +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +struct ResampleParam +{ + double max_trajectory_length; // max length of the objective trajectory for resample + double min_trajectory_length; // min length of the objective trajectory for resample + double resample_time; // max time to calculate trajectory length + double dense_resample_dt; // resample time interval for dense sampling [s] + double dense_min_interval_distance; // minimum points-interval length for dense sampling [m] + double sparse_resample_dt; // resample time interval for sparse sampling [s] + double sparse_min_interval_distance; // minimum points-interval length for sparse sampling [m] +}; + +TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const double v_current, + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v = true); + +TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); +} // namespace resampling +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp new file mode 100644 index 0000000000..7a74351b47 --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -0,0 +1,125 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_utils/system/time_keeper.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ +class AnalyticalJerkConstrainedSmoother : public SmootherBase +{ +public: + struct Param + { + struct + { + double ds_resample; + int num_resample; + double delta_yaw_threshold; + } resample; + struct + { + bool enable_constant_velocity_while_turning; + double constant_velocity_dist_threshold; + } latacc; + struct + { + double max_acc; + double min_acc; + double max_jerk; + double min_jerk; + double kp; + } forward; + struct + { + double start_jerk; + double min_jerk_mild_stop; + double min_jerk; + double min_acc_mild_stop; + double min_acc; + double span_jerk; + } backward; + }; + + explicit AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper = + std::make_shared()); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; + + TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose, + [[maybe_unused]] const double nearest_dist_threshold, + [[maybe_unused]] const double nearest_yaw_threshold) const override; + + TrajectoryPoints applyLateralAccelerationFilter( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling = true, const double input_points_interval = 1.0) const override; + + void setParam(const Param & param); + Param getParam() const; + +private: + Param smoother_param_; + rclcpp::Logger logger_{ + rclcpp::get_logger("smoother").get_child("analytical_jerk_constrained_smoother")}; + + bool searchDecelTargetIndices( + const TrajectoryPoints & trajectory, const size_t closest_index, + std::vector> & decel_target_indices) const; + bool applyForwardJerkFilter( + const TrajectoryPoints & base_trajectory, const size_t start_index, const double initial_vel, + const double initial_acc, const Param & params, TrajectoryPoints & output_trajectory) const; + bool applyBackwardDecelFilter( + const std::vector & start_indices, const size_t decel_target_index, + const double decel_target_vel, const Param & params, + TrajectoryPoints & output_trajectory) const; + bool calcEnoughDistForDecel( + const TrajectoryPoints & trajectory, const size_t start_index, const double decel_target_vel, + const double planning_jerk, const Param & params, const std::vector & dist_to_target, + bool & is_enough_dist, int & type, std::vector & times, double & stop_dist) const; + bool applyDecelVelocityFilter( + const size_t decel_start_index, const double decel_target_vel, const double planning_jerk, + const Param & params, const int type, const std::vector & times, + TrajectoryPoints & output_trajectory) const; + + // debug + std::string strTimes(const std::vector & times) const; + std::string strStartIndices(const std::vector & start_indices) const; +}; +} // namespace autoware::velocity_smoother + +// clang-format off +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +// clang-format on diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp new file mode 100644 index 0000000000..0640bc950f --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ +namespace analytical_velocity_planning_utils +{ +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +bool calcStopDistWithJerkAndAccConstraints( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double target_vel, int & type, std::vector & times, + double & stop_dist); +bool validCheckCalcStopDist( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin); +bool calcStopVelocityWithConstantJerkAccLimit( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double decel_target_vel, const int type, + const std::vector & times, const size_t start_index, + TrajectoryPoints & output_trajectory); +void updateStopVelocityStatus( + double v0, double a0, double jerk_acc, double jerk_dec, int type, + const std::vector & times, double t, double & x, double & v, double & a, double & j); +double integ_x(double x0, double v0, double a0, double j0, double t); +double integ_v(double v0, double a0, double j0, double t); +double integ_a(double a0, double j0, double t); +} // namespace analytical_velocity_planning_utils +} // namespace autoware::velocity_smoother + +// clang-format off +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +// clang-format on diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp new file mode 100644 index 0000000000..96f8645f9d --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -0,0 +1,78 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/qp_interface/qp_interface.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/system/time_keeper.hpp" + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include +#include + +namespace autoware::velocity_smoother +{ +class JerkFilteredSmoother : public SmootherBase +{ +public: + struct Param + { + double jerk_weight; + double over_v_weight; + double over_a_weight; + double over_j_weight; + double jerk_filter_ds; + }; + + explicit JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; + + TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const override; + + void setParam(const Param & param); + Param getParam() const; + +private: + Param smoother_param_; + std::shared_ptr qp_interface_; + rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("jerk_filtered_smoother")}; + + TrajectoryPoints forwardJerkFilter( + const double v0, const double a0, const double a_max, const double a_stop, const double j_max, + const TrajectoryPoints & input) const; + TrajectoryPoints backwardJerkFilter( + const double v0, const double a0, const double a_min, const double a_stop, const double j_min, + const TrajectoryPoints & input) const; + TrajectoryPoints mergeFilteredTrajectory( + const double v0, const double a0, const double a_min, const double j_min, + const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const; +}; +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp new file mode 100644 index 0000000000..cb41a72225 --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -0,0 +1,65 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/system/time_keeper.hpp" + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include +#include + +namespace autoware::velocity_smoother +{ +class L2PseudoJerkSmoother : public SmootherBase +{ +public: + struct Param + { + double pseudo_jerk_weight; + double over_v_weight; + double over_a_weight; + }; + + explicit L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; + + TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const override; + + void setParam(const Param & smoother_param); + Param getParam() const; + +private: + Param smoother_param_; + autoware::osqp_interface::OSQPInterface qp_solver_; + rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; +}; +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp new file mode 100644 index 0000000000..3a80304202 --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -0,0 +1,65 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/system/time_keeper.hpp" + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include +#include + +namespace autoware::velocity_smoother +{ +class LinfPseudoJerkSmoother : public SmootherBase +{ +public: + struct Param + { + double pseudo_jerk_weight; + double over_v_weight; + double over_a_weight; + }; + + explicit LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; + + TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const override; + + void setParam(const Param & smoother_param); + Param getParam() const; + +private: + Param smoother_param_; + autoware::osqp_interface::OSQPInterface qp_solver_; + rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; +}; +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp new file mode 100644 index 0000000000..d7b6422711 --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -0,0 +1,98 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ + +#include "autoware/velocity_smoother/resample.hpp" +#include "autoware_utils/system/time_keeper.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include +#include +#include + +namespace autoware::velocity_smoother +{ +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +class SmootherBase +{ +public: + struct BaseParam + { + double max_accel; // max acceleration in planning [m/s2] > 0 + double min_decel; // min deceleration in planning [m/s2] < 0 + double stop_decel; // deceleration at a stop point [m/s2] <= 0 + double max_jerk; + double min_jerk; + double max_lateral_accel; // max lateral acceleration [m/ss] > 0 + double min_decel_for_lateral_acc_lim_filter; // deceleration limit applied in the lateral + // acceleration filter to avoid sudden braking. + double min_curve_velocity; // min velocity at curve [m/s] + double decel_distance_before_curve; // distance before slow down for lateral acc at a curve + double decel_distance_after_curve; // distance after slow down for lateral acc at a curve + double max_steering_angle_rate; // max steering angle rate [degree/s] + double wheel_base; // wheel base [m] + double sample_ds; // distance between trajectory points [m] + double curvature_threshold; // look-up distance of Trajectory point for calculation of steering + // angle limit [m] + double curvature_calculation_distance; // threshold steering degree limit to trigger + // steeringRateLimit [degree] + resampling::ResampleParam resample_param; + }; + + explicit SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper); + virtual ~SmootherBase() = default; + virtual bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) = 0; + + virtual TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const = 0; + + virtual TrajectoryPoints applyLateralAccelerationFilter( + const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, + [[maybe_unused]] const double a0 = 0.0, [[maybe_unused]] const bool enable_smooth_limit = false, + const bool use_resampling = true, const double input_points_interval = 1.0) const; + + TrajectoryPoints applySteeringRateLimit( + const TrajectoryPoints & input, const bool use_resampling = true, + const double input_points_interval = 1.0) const; + + double getMaxAccel() const; + double getMinDecel() const; + double getMaxJerk() const; + double getMinJerk() const; + + void setWheelBase(const double wheel_base); + void setMaxAccel(const double max_acceleration); + void setMaxJerk(const double max_jerk); + + void setParam(const BaseParam & param); + BaseParam getBaseParam() const; + +protected: + BaseParam base_param_; + mutable std::shared_ptr time_keeper_{nullptr}; +}; +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp new file mode 100644 index 0000000000..d20f82538e --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include +#include + +namespace autoware::velocity_smoother::trajectory_utils +{ +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Pose; + +TrajectoryPoint calcInterpolatedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx); + +TrajectoryPoints extractPathAroundIndex( + const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length, + const double & behind_length); + +std::vector calcArclengthArray(const TrajectoryPoints & trajectory); + +std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory); + +std::vector calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, size_t idx_dist); + +void applyMaximumVelocityLimit( + const size_t from, const size_t to, const double max_vel, TrajectoryPoints & trajectory); + +std::optional searchZeroVelocityIdx(const TrajectoryPoints & trajectory); + +bool calcStopDistWithJerkConstraints( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double target_vel, std::map & jerk_profile, + double & stop_dist); + +bool isValidStopDist( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin); + +std::optional> updateStateWithJerkConstraint( + const double v0, const double a0, const std::map & jerk_profile, const double t); + +std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( + const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk, + const double acc_max, const double acc_min); + +double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); + +} // namespace autoware::velocity_smoother::trajectory_utils + +#endif // AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml new file mode 100644 index 0000000000..11932ad125 --- /dev/null +++ b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg new file mode 100644 index 0000000000..529de678b1 --- /dev/null +++ b/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg @@ -0,0 +1,1622 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Extract trajectory + + + + + Extract trajectory + + + + + + + + + + + + Apply external velocity limit + + + + + Apply external velocity limit + + + + + + + + + + + + Apply lateral acceleration limit + + + + + Apply lateral acceleration limit + + + + + + + + + + + + Resample trajectory + + + + + Resample trajectory + + + + + + + + + + + + Calculate initial state + + + + + Calculate initial state + + + + + + + + + + + + Smooth velocity + + + + + Smooth velocity + + + + + + + + + + + + Postprocess + + + + + Postprocess + + + + + Reference trajectory + + + + + + Output trajectory + + + + + + + + + onCurrentTrajectory + + + + + onCurrentTrajectory + + + + + + + + + + + + Calculate travel distance + + + + + Calculate travel distance + + + + + + + Apply steering rate limit + + + + + + + + + + + Apply stop approaching velocity + + + + + Apply stop approaching velocity + + + + + + + + + + + + + + + + + + + + extract_behind_dist + + + + + extract_behind_dist + + + + + + + + + + extract_ahead_dist + + + + + extract_ahead_dist + + + + + + + + + + + Extract trajectory + + + + + Extract trajecto... + + + + + + + + + + Apply external velocity limit + + + + + Apply external velocity li... + + + + + + + + + + + + + + default velocity limit + + + + + default velocity lim... + + + + + + + + + + external velocity limit + + + + + external velocity li... + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + distance + + + + + distance + + + + + + + + + + + + + applied velocity limit + + + + + applied velocity lim... + + + + + + + + + + + + distance required for deceleration under jerk constraints + + + + + distance required for decelera... + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + Apply stopping velocity limit + + + + + Apply stopping velocity li... + + + + + + + + + + + + + + + + + + + stopping velocity + + + + + stopping velocity + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + + + + applied velocity limit + + + + + applied velocity lim... + + + + + + + + + + + stopping + + distance + + + + + stopping... + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + + + + + velocity limit with external velocity limit + + + + + velocity limit with external veloci... + + + + + + + + + + + + + + + + + + Apply lateral acceleration limit + + + + + Apply lateral acceleration lim... + + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + + + applied velocity limit + + + + + applied velocity lim... + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + velocity limit with stopping velocity + + + + + velocity limit with stopping veloci... + + + + + + + + + + + + + + + ego car + + + + + ego car + + + + + + + + + + + + + + + + + + + distance + + + + + distance + + + + + + + + + + distance + + + + + distance + + + + + + + + + + curvature + + + + + curvature + + + + + + + + + + distance + + + + + distance + + + + + + + + + + + + trajectory + + + + + trajectory + + + + + + + + + + + + Resample trajectory + + + + + Resample trajectory + + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + distance + + + + + distance + + + + + + + + + + + + + + + + + + + + + + + + + + + resample time * current velocity + + + + + resample time * current veloc... + + + + + + + + + + + + dense sampling + + + + + dense sampling + + + + + + + + + + sparse sampling + + + + + sparse sampling + + + + + + + + + + + + + + + + Smooth velocity + + + + + Smooth velocity + + + + + + + + + + + + + + + + + + + + + + + + Apply steering rate limit + + + + + Apply steering rate limit + + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + + + applied velocity limit + + + + + applied velocity lim... + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + velocity limit with lateral acceleration limit + + + + + + velocity with lateral acceleration limit + + + + + + + + + + + + + + + + ego car + + + + + ego car + + + + + + + + + + + + + + + + + + + distance + + + + + distance + + + + + + + + + + curvature + + + + + curvature + + + + + + + + + + distance + + + + + distance + + + + + + + + + + + + trajectory + + + + + trajectory + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + smoothed velocity + + + + + smoothed velocity + + + + + + + + + + velocity limit + + + + + velocity limit + + + + + + Viewer does not support full SVG 1.1 + + + + + + + + + + + + + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + distance + + + + + distance + + + diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml new file mode 100644 index 0000000000..2d99e34809 --- /dev/null +++ b/planning/autoware_velocity_smoother/package.xml @@ -0,0 +1,47 @@ + + + + autoware_velocity_smoother + 0.0.0 + The autoware_velocity_smoother package + + Fumiya Watanabe + Takamasa Horibe + Makoto Kurihara + Satoshi Ota + Go Sakayori + Apache License 2.0 + + Takamasa Horibe + Fumiya Watanabe + Yutaka Shimizu + Makoto Kurihara + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_internal_debug_msgs + autoware_internal_planning_msgs + autoware_interpolation + autoware_motion_utils + autoware_osqp_interface + autoware_planning_msgs + autoware_planning_test_manager + autoware_qp_interface + autoware_utils + autoware_vehicle_info_utils + geometry_msgs + nav_msgs + rclcpp + tf2 + tf2_ros + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp new file mode 100644 index 0000000000..c6f08ea910 --- /dev/null +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -0,0 +1,1146 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/node.hpp" + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// clang-format on +namespace autoware::velocity_smoother +{ +VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) +: Node("velocity_smoother", node_options), + diagnostics_interface_(std::make_unique(this, "velocity_smoother")) +{ + using std::placeholders::_1; + + // set common params + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + wheelbase_ = vehicle_info.wheel_base_m; + base_link2front_ = vehicle_info.max_longitudinal_offset_m; + initCommonParam(); + over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); + + // create time_keeper and its publisher + // NOTE: This has to be called before setupSmoother to pass the time_keeper to the smoother. + debug_processing_time_detail_ = + create_publisher("~/debug/processing_time_detail_ms", 1); + time_keeper_ = std::make_shared(debug_processing_time_detail_); + + // create smoother + setupSmoother(wheelbase_); + + // publishers, subscribers + pub_trajectory_ = create_publisher("~/output/trajectory", 1); + pub_virtual_wall_ = create_publisher("~/virtual_wall", 1); + pub_velocity_limit_ = create_publisher( + "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); + pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); + sub_current_trajectory_ = create_subscription( + "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); + + // parameter update + set_param_res_ = + this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1)); + + // debug + publish_debug_trajs_ = declare_parameter("publish_debug_trajs"); + debug_closest_velocity_ = create_publisher("~/closest_velocity", 1); + debug_closest_acc_ = create_publisher("~/closest_acceleration", 1); + debug_closest_jerk_ = create_publisher("~/closest_jerk", 1); + debug_closest_max_velocity_ = create_publisher("~/closest_max_velocity", 1); + debug_calculation_time_ = create_publisher("~/debug/processing_time_ms", 1); + pub_trajectory_raw_ = create_publisher("~/debug/trajectory_raw", 1); + pub_trajectory_vel_lim_ = + create_publisher("~/debug/trajectory_external_velocity_limited", 1); + pub_trajectory_latacc_filtered_ = + create_publisher("~/debug/trajectory_lateral_acc_filtered", 1); + pub_trajectory_steering_rate_limited_ = + create_publisher("~/debug/trajectory_steering_rate_limited", 1); + pub_trajectory_resampled_ = create_publisher("~/debug/trajectory_time_resampled", 1); + + external_velocity_limit_.velocity = node_param_.max_velocity; + max_velocity_with_deceleration_ = node_param_.max_velocity; + + // publish default max velocity + VelocityLimit max_vel_msg{}; + max_vel_msg.stamp = this->now(); + max_vel_msg.max_velocity = node_param_.max_velocity; + pub_velocity_limit_->publish(max_vel_msg); + + clock_ = get_clock(); + + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); +} + +void VelocitySmootherNode::setupSmoother(const double wheelbase) +{ + switch (node_param_.algorithm_type) { + case AlgorithmType::JERK_FILTERED: { + smoother_ = std::make_shared(*this, time_keeper_); + + // Set Publisher for jerk filtered algorithm + pub_forward_filtered_trajectory_ = + create_publisher("~/debug/forward_filtered_trajectory", 1); + pub_backward_filtered_trajectory_ = + create_publisher("~/debug/backward_filtered_trajectory", 1); + pub_merged_filtered_trajectory_ = + create_publisher("~/debug/merged_filtered_trajectory", 1); + pub_closest_merged_velocity_ = + create_publisher("~/closest_merged_velocity", 1); + break; + } + case AlgorithmType::L2: { + smoother_ = std::make_shared(*this, time_keeper_); + break; + } + case AlgorithmType::LINF: { + smoother_ = std::make_shared(*this, time_keeper_); + break; + } + case AlgorithmType::ANALYTICAL: { + smoother_ = std::make_shared(*this, time_keeper_); + break; + } + default: + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); + } + + smoother_->setWheelBase(wheelbase); +} + +rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter( + const std::vector & parameters) +{ + auto update_param = [&](const std::string & name, double & v) { + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + v = it->as_double(); + return true; + } + return false; + }; + + // TODO(Horibe): temporally. replace with template. + auto update_param_bool = [&](const std::string & name, bool & v) { + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + v = it->as_bool(); + return true; + } + return false; + }; + + { + auto & p = node_param_; + update_param_bool("enable_lateral_acc_limit", p.enable_lateral_acc_limit); + update_param_bool("enable_steering_rate_limit", p.enable_steering_rate_limit); + + update_param("max_vel", p.max_velocity); + update_param( + "margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit); + update_param("replan_vel_deviation", p.replan_vel_deviation); + update_param("engage_velocity", p.engage_velocity); + update_param("engage_acceleration", p.engage_acceleration); + update_param("engage_exit_ratio", p.engage_exit_ratio); + update_param("stopping_velocity", p.stopping_velocity); + update_param("stopping_distance", p.stopping_distance); + update_param("extract_ahead_dist", p.extract_ahead_dist); + update_param("extract_behind_dist", p.extract_behind_dist); + update_param("stop_dist_to_prohibit_engage", p.stop_dist_to_prohibit_engage); + update_param("ego_nearest_dist_threshold", p.ego_nearest_dist_threshold); + update_param("ego_nearest_yaw_threshold", p.ego_nearest_yaw_threshold); + update_param_bool("plan_from_ego_speed_on_manual_mode", p.plan_from_ego_speed_on_manual_mode); + } + + { + auto p = smoother_->getBaseParam(); + update_param("normal.max_acc", p.max_accel); + update_param("normal.min_acc", p.min_decel); + update_param("stop_decel", p.stop_decel); + update_param("normal.max_jerk", p.max_jerk); + update_param("normal.min_jerk", p.min_jerk); + update_param("max_lateral_accel", p.max_lateral_accel); + update_param("min_curve_velocity", p.min_curve_velocity); + update_param("decel_distance_before_curve", p.decel_distance_before_curve); + update_param("decel_distance_after_curve", p.decel_distance_after_curve); + update_param("max_trajectory_length", p.resample_param.max_trajectory_length); + update_param("min_trajectory_length", p.resample_param.min_trajectory_length); + update_param("resample_time", p.resample_param.resample_time); + update_param("dense_resample_dt", p.resample_param.dense_resample_dt); + update_param("min_interval_distance", p.resample_param.dense_min_interval_distance); + update_param("sparse_resample_dt", p.resample_param.sparse_resample_dt); + update_param("sparse_min_interval_distance", p.resample_param.sparse_min_interval_distance); + update_param("resample_ds", p.sample_ds); + update_param("curvature_threshold", p.curvature_threshold); + update_param("max_steering_angle_rate", p.max_steering_angle_rate); + update_param("curvature_calculation_distance", p.curvature_calculation_distance); + smoother_->setParam(p); + } + + switch (node_param_.algorithm_type) { + case AlgorithmType::JERK_FILTERED: { + auto p = std::dynamic_pointer_cast(smoother_)->getParam(); + update_param("jerk_weight", p.jerk_weight); + update_param("over_v_weight", p.over_v_weight); + update_param("over_a_weight", p.over_a_weight); + update_param("over_j_weight", p.over_j_weight); + update_param("jerk_filter_ds", p.jerk_filter_ds); + std::dynamic_pointer_cast(smoother_)->setParam(p); + break; + } + case AlgorithmType::L2: { + auto p = std::dynamic_pointer_cast(smoother_)->getParam(); + update_param("pseudo_jerk_weight", p.pseudo_jerk_weight); + update_param("over_v_weight", p.over_v_weight); + update_param("over_a_weight", p.over_a_weight); + std::dynamic_pointer_cast(smoother_)->setParam(p); + break; + } + case AlgorithmType::LINF: { + auto p = std::dynamic_pointer_cast(smoother_)->getParam(); + update_param("pseudo_jerk_weight", p.pseudo_jerk_weight); + update_param("over_v_weight", p.over_v_weight); + update_param("over_a_weight", p.over_a_weight); + std::dynamic_pointer_cast(smoother_)->setParam(p); + break; + } + case AlgorithmType::ANALYTICAL: { + auto p = std::dynamic_pointer_cast(smoother_)->getParam(); + update_param("resample.delta_yaw_threshold", p.resample.delta_yaw_threshold); + update_param( + "latacc.constant_velocity_dist_threshold", p.latacc.constant_velocity_dist_threshold); + update_param("forward.max_acc", p.forward.max_acc); + update_param("forward.min_acc", p.forward.min_acc); + update_param("forward.max_jerk", p.forward.max_jerk); + update_param("forward.min_jerk", p.forward.min_jerk); + update_param("forward.kp", p.forward.kp); + update_param("backward.start_jerk", p.backward.start_jerk); + update_param("backward.min_jerk_mild_stop", p.backward.min_jerk_mild_stop); + update_param("backward.min_jerk", p.backward.min_jerk); + update_param("backward.min_acc_mild_stop", p.backward.min_acc_mild_stop); + update_param("backward.min_acc", p.backward.min_acc); + update_param("backward.span_jerk", p.backward.span_jerk); + std::dynamic_pointer_cast(smoother_)->setParam(p); + break; + } + default: + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); + } + + rcl_interfaces::msg::SetParametersResult result{}; + result.successful = true; + result.reason = "success"; + return result; +} + +void VelocitySmootherNode::initCommonParam() +{ + auto & p = node_param_; + p.enable_lateral_acc_limit = declare_parameter("enable_lateral_acc_limit"); + p.enable_steering_rate_limit = declare_parameter("enable_steering_rate_limit"); + + p.max_velocity = declare_parameter("max_vel"); + p.margin_to_insert_external_velocity_limit = + declare_parameter("margin_to_insert_external_velocity_limit"); + p.replan_vel_deviation = declare_parameter("replan_vel_deviation"); + p.engage_velocity = declare_parameter("engage_velocity"); + p.engage_acceleration = declare_parameter("engage_acceleration"); + p.engage_exit_ratio = declare_parameter("engage_exit_ratio"); + p.engage_exit_ratio = std::min(std::max(p.engage_exit_ratio, 0.0), 1.0); + p.stopping_velocity = declare_parameter("stopping_velocity"); + p.stopping_distance = declare_parameter("stopping_distance"); + p.extract_ahead_dist = declare_parameter("extract_ahead_dist"); + p.extract_behind_dist = declare_parameter("extract_behind_dist"); + p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage"); + p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + p.post_resample_param.max_trajectory_length = + declare_parameter("post_max_trajectory_length"); + p.post_resample_param.min_trajectory_length = + declare_parameter("post_min_trajectory_length"); + p.post_resample_param.resample_time = declare_parameter("post_resample_time"); + p.post_resample_param.dense_resample_dt = declare_parameter("post_dense_resample_dt"); + p.post_resample_param.dense_min_interval_distance = + declare_parameter("post_dense_min_interval_distance"); + p.post_resample_param.sparse_resample_dt = declare_parameter("post_sparse_resample_dt"); + p.post_resample_param.sparse_min_interval_distance = + declare_parameter("post_sparse_min_interval_distance"); + p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type")); + + p.plan_from_ego_speed_on_manual_mode = + declare_parameter("plan_from_ego_speed_on_manual_mode"); +} + +void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const +{ + Trajectory publishing_trajectory = autoware::motion_utils::convertToTrajectory(trajectory); + publishing_trajectory.header = base_traj_raw_ptr_->header; + pub_trajectory_->publish(publishing_trajectory); + published_time_publisher_->publish_if_subscribed( + pub_trajectory_, publishing_trajectory.header.stamp); +} + +void VelocitySmootherNode::calcExternalVelocityLimit() +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (!external_velocity_limit_ptr_) { + external_velocity_limit_.acceleration_request.request = false; + return; + } + + // sender + external_velocity_limit_.sender = external_velocity_limit_ptr_->sender; + + // on the first time, apply directly + if (prev_output_.empty() || !current_closest_point_from_prev_output_) { + external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity; + pub_velocity_limit_->publish(*external_velocity_limit_ptr_); + return; + } + + constexpr double eps = 1.0E-04; + const double margin = node_param_.margin_to_insert_external_velocity_limit; + + // Set distance as zero if ego vehicle is stopped and external velocity limit is zero + if ( + std::fabs(current_odometry_ptr_->twist.twist.linear.x) < eps && + external_velocity_limit_.velocity < eps) { + external_velocity_limit_.dist = 0.0; + } + + const auto base_max_acceleration = get_parameter("normal.max_acc").as_double(); + const auto acceleration_request = + external_velocity_limit_ptr_->use_constraints && + base_max_acceleration < external_velocity_limit_ptr_->constraints.max_acceleration; + if ( + acceleration_request && + current_odometry_ptr_->twist.twist.linear.x < external_velocity_limit_ptr_->max_velocity) { + external_velocity_limit_.acceleration_request.request = true; + external_velocity_limit_.acceleration_request.max_acceleration = + external_velocity_limit_ptr_->constraints.max_acceleration; + external_velocity_limit_.acceleration_request.max_jerk = + external_velocity_limit_ptr_->constraints.max_jerk; + } + + // calculate distance and maximum velocity + // to decelerate to external velocity limit with jerk and acceleration + // constraints. + // if external velocity limit decreases + if ( + std::fabs((external_velocity_limit_.velocity - external_velocity_limit_ptr_->max_velocity)) > + eps) { + const double v0 = current_closest_point_from_prev_output_->longitudinal_velocity_mps; + const double a0 = current_closest_point_from_prev_output_->acceleration_mps2; + + if (isEngageStatus(v0)) { + max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity; + external_velocity_limit_.dist = 0.0; + } else { + const auto & cstr = external_velocity_limit_ptr_->constraints; + const auto a_min = external_velocity_limit_ptr_->use_constraints ? cstr.min_acceleration + : smoother_->getMinDecel(); + const auto j_max = + external_velocity_limit_ptr_->use_constraints ? cstr.max_jerk : smoother_->getMaxJerk(); + const auto j_min = + external_velocity_limit_ptr_->use_constraints ? cstr.min_jerk : smoother_->getMinJerk(); + + // If the closest acceleration is positive, velocity will increase + // until the acceleration becomes zero + // So we set the maximum increased velocity as the velocity limit + if (a0 > 0) { + max_velocity_with_deceleration_ = v0 - 0.5 * a0 * a0 / j_min; + } else { + max_velocity_with_deceleration_ = v0; + } + + if (external_velocity_limit_ptr_->max_velocity < max_velocity_with_deceleration_) { + // TODO(mkuri) If v0 < external_velocity_limit_ptr_->max_velocity < + // max_velocity_with_deceleration_ meets, stronger jerk than expected may be applied to + // external velocity limit. + if (v0 < external_velocity_limit_ptr_->max_velocity) { + RCLCPP_WARN( + get_logger(), + "Stronger jerk than expected may be applied to external velocity limit in this " + "condition."); + } + + double stop_dist = 0.0; + std::map jerk_profile; + if (!trajectory_utils::calcStopDistWithJerkConstraints( + v0, a0, j_max, j_min, a_min, external_velocity_limit_ptr_->max_velocity, jerk_profile, + stop_dist)) { + RCLCPP_WARN(get_logger(), "Stop distance calculation failed!"); + } + external_velocity_limit_.dist = stop_dist + margin; + } else { + max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity; + external_velocity_limit_.dist = 0.0; + } + } + } + + external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity; + pub_velocity_limit_->publish(*external_velocity_limit_ptr_); + + return; +} + +bool VelocitySmootherNode::checkData() const +{ + if (!current_odometry_ptr_ || !base_traj_raw_ptr_ || !current_acceleration_ptr_) { + RCLCPP_DEBUG( + get_logger(), "wait topics : current_vel = %d, base_traj = %d, acceleration = %d", + (bool)current_odometry_ptr_, (bool)base_traj_raw_ptr_, (bool)current_acceleration_ptr_); + return false; + } + if (base_traj_raw_ptr_->points.size() < 2) { + RCLCPP_ERROR(get_logger(), "input trajectory size must > 1. Skip computation."); + return false; + } + + return true; +} + +void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); + stop_watch_.tic(); + + diagnostics_interface_->clear(); + base_traj_raw_ptr_ = msg; + + // receive data + current_odometry_ptr_ = sub_current_odometry_.take_data(); + current_acceleration_ptr_ = sub_current_acceleration_.take_data(); + external_velocity_limit_ptr_ = sub_external_velocity_limit_.take_data(); + const auto operation_mode_ptr = sub_operation_mode_.take_data(); + if (operation_mode_ptr) { + operation_mode_ = *operation_mode_ptr; + } + + // guard + if (!checkData()) { + return; + } + + // calculate trajectory velocity + auto input_points = autoware::motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); + + // guard for invalid trajectory + input_points = autoware::motion_utils::removeOverlapPoints(input_points); + if (input_points.size() < 2) { + RCLCPP_ERROR(get_logger(), "No enough points in trajectory after overlap points removal"); + return; + } + + // Set 0 at the end of the trajectory + input_points.back().longitudinal_velocity_mps = 0.0; + + // calculate prev closest point + if (!prev_output_.empty()) { + current_closest_point_from_prev_output_ = calcProjectedTrajectoryPointFromEgo(prev_output_); + } + + // calculate distance to insert external velocity limit + calcExternalVelocityLimit(); + updateDataForExternalVelocityLimit(); + + // For negative velocity handling, multiple -1 to velocity if it is for reverse. + // NOTE: this process must be in the beginning of the process + is_reverse_ = isReverse(input_points); + if (is_reverse_) { + flipVelocity(input_points); + } + + const auto output = calcTrajectoryVelocity(input_points); + if (output.empty()) { + RCLCPP_WARN(get_logger(), "Output Point is empty"); + return; + } + + // Note that output velocity is resampled by linear interpolation + auto output_resampled = resampling::resampleTrajectory( + output, current_odometry_ptr_->twist.twist.linear.x, current_odometry_ptr_->pose.pose, + node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold, + node_param_.post_resample_param, false); + + // Set 0 at the end of the trajectory + if (!output_resampled.empty()) { + output_resampled.back().longitudinal_velocity_mps = 0.0; + } + + // update previous step infomation + updatePrevValues(output); + + // for reverse velocity + // NOTE: this process must be in the end of the process + if (is_reverse_) { + flipVelocity(output_resampled); + } + + // publish message + publishTrajectory(output_resampled); + + // publish debug message + publishStopDistance(output); + publishClosestState(output); + + // Publish Calculation Time + publishStopWatchTime(); + + // Publish diagnostics + diagnostics_interface_->publish(now()); + + RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); +} + +void VelocitySmootherNode::updateDataForExternalVelocityLimit() +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (prev_output_.empty()) { + return; + } + + // calculate distance to insert external velocity limit + const double travel_dist = calcTravelDistance(); + external_velocity_limit_.dist = std::max(external_velocity_limit_.dist - travel_dist, 0.0); + RCLCPP_DEBUG( + get_logger(), "run: travel_dist = %f, external_velocity_limit_dist_ = %f", travel_dist, + external_velocity_limit_.dist); + + return; +} + +TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( + const TrajectoryPoints & traj_input) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + TrajectoryPoints output{}; // velocity is optimized by qp solver + + // Extract trajectory around self-position with desired forward-backward length + const size_t input_closest = findNearestIndexFromEgo(traj_input); + + auto traj_extracted = trajectory_utils::extractPathAroundIndex( + traj_input, input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); + if (traj_extracted.empty()) { + RCLCPP_WARN(get_logger(), "Fail to extract the path from the input trajectory"); + return prev_output_; + } + + // Debug + if (publish_debug_trajs_) { + auto tmp = traj_extracted; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_raw_->publish(toTrajectoryMsg(tmp)); + } + + // Apply external velocity limit + applyExternalVelocityLimit(traj_extracted); + + // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close + const size_t traj_extracted_closest = findNearestIndexFromEgo(traj_extracted); + + // Apply velocity to approach stop point + applyStopApproachingVelocity(traj_extracted); + + // Debug + if (publish_debug_trajs_) { + auto tmp = traj_extracted; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_vel_lim_->publish(toTrajectoryMsg(traj_extracted)); + } + + // Smoothing velocity + if (!smoothVelocity(traj_extracted, traj_extracted_closest, output)) { + return prev_output_; + } + + return output; +} + +bool VelocitySmootherNode::smoothVelocity( + const TrajectoryPoints & input, const size_t input_closest, + TrajectoryPoints & traj_smoothed) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (input.empty()) { + return false; // cannot apply smoothing + } + + // Calculate initial motion for smoothing + const auto [initial_motion, type] = calcInitialMotion(input, input_closest); + + // Lateral acceleration limit + constexpr bool enable_smooth_limit = true; + constexpr bool use_resampling = true; + const auto traj_lateral_acc_filtered = + node_param_.enable_lateral_acc_limit + ? smoother_->applyLateralAccelerationFilter( + input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling) + : input; + + // Steering angle rate limit (Note: set use_resample = false since it is resampled above) + const auto traj_steering_rate_limited = + node_param_.enable_steering_rate_limit + ? smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false) + : traj_lateral_acc_filtered; + + // Resample trajectory with ego-velocity based interval distance + auto traj_resampled = smoother_->resampleTrajectory( + traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x, + current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + + const size_t traj_resampled_closest = findNearestIndexFromEgo(traj_resampled); + + // Set 0[m/s] in the terminal point + if (!traj_resampled.empty()) { + traj_resampled.back().longitudinal_velocity_mps = 0.0; + } + + // Publish Closest Resample Trajectory Velocity + publishClosestVelocity( + traj_resampled, current_odometry_ptr_->pose.pose, debug_closest_max_velocity_); + + // Clip trajectory from closest point + TrajectoryPoints clipped; + clipped.insert( + clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + + // Set maximum acceleration before applying smoother. Depends on acceleration request from + // external velocity limit + const double smoother_max_acceleration = + external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_acceleration + : get_parameter("normal.max_acc").as_double(); + const double smoother_max_jerk = external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_jerk + : get_parameter("normal.max_jerk").as_double(); + smoother_->setMaxAccel(smoother_max_acceleration); + smoother_->setMaxJerk(smoother_max_jerk); + + std::vector debug_trajectories; + if (!smoother_->apply( + initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories, + publish_debug_trajs_)) { + RCLCPP_WARN(get_logger(), "Fail to solve optimization."); + } + + // Set 0 velocity after input-stop-point + overwriteStopPoint(clipped, traj_smoothed); + + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + + // For the endpoint of the trajectory + if (!traj_smoothed.empty()) { + traj_smoothed.back().longitudinal_velocity_mps = 0.0; + } + + // Max velocity filter for safety + trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed); + + // Insert behind velocity for output's consistency + insertBehindVelocity(traj_resampled_closest, type, traj_smoothed); + + RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size()); + if (publish_debug_trajs_) { + { + auto tmp = traj_lateral_acc_filtered; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_latacc_filtered_->publish(toTrajectoryMsg(tmp)); + } + { + auto tmp = traj_resampled; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp)); + } + { + auto tmp = traj_steering_rate_limited; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); + } + + for (auto & debug_trajectory : debug_trajectories) { + debug_trajectory.insert( + debug_trajectory.begin(), traj_resampled.begin(), + traj_resampled.begin() + traj_resampled_closest); + for (size_t i = 0; i < traj_resampled_closest; ++i) { + debug_trajectory.at(i).longitudinal_velocity_mps = + debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; + } + } + publishDebugTrajectories(debug_trajectories); + } + + return true; +} + +void VelocitySmootherNode::insertBehindVelocity( + const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const bool keep_closest_vel_for_behind = + (type == InitializeType::EGO_VELOCITY || type == InitializeType::LARGE_DEVIATION_REPLAN || + type == InitializeType::ENGAGING); + + for (size_t i = output_closest - 1; i < output.size(); --i) { + if (keep_closest_vel_for_behind) { + output.at(i).longitudinal_velocity_mps = output.at(output_closest).longitudinal_velocity_mps; + output.at(i).acceleration_mps2 = output.at(output_closest).acceleration_mps2; + } else { + // TODO(planning/control team) deal with overlapped lanes with the same direction + const size_t seg_idx = [&]() { + // with distance and yaw thresholds + const auto opt_nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + if (opt_nearest_seg_idx) { + return opt_nearest_seg_idx.value(); + } + + // with distance threshold + const auto opt_second_nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold); + if (opt_second_nearest_seg_idx) { + return opt_second_nearest_seg_idx.value(); + } + + return autoware::motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose.position); + }(); + const auto prev_output_point = + trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose, seg_idx); + + // output should be always positive: TODO(Horibe) think better way + output.at(i).longitudinal_velocity_mps = + std::abs(prev_output_point.longitudinal_velocity_mps); + output.at(i).acceleration_mps2 = prev_output_point.acceleration_mps2; + } + } +} + +void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const +{ + const size_t closest = findNearestIndexFromEgo(trajectory); + + // stop distance calculation + const double stop_dist_lim{50.0}; + double stop_dist{stop_dist_lim}; + const auto stop_idx{autoware::motion_utils::searchZeroVelocityIndex(trajectory)}; + if (stop_idx) { + stop_dist = autoware::motion_utils::calcSignedArcLength(trajectory, closest, *stop_idx); + } else { + stop_dist = closest > 0 ? stop_dist : -stop_dist; + } + Float32Stamped dist_to_stopline{}; + dist_to_stopline.stamp = this->now(); + dist_to_stopline.data = std::clamp(stop_dist, -stop_dist_lim, stop_dist_lim); + pub_dist_to_stopline_->publish(dist_to_stopline); +} + +std::pair VelocitySmootherNode::calcInitialMotion( + const TrajectoryPoints & input_traj, const size_t input_closest) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); + const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x; + const double target_vel = std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps); + + // first time + if (!current_closest_point_from_prev_output_) { + Motion initial_motion = {vehicle_speed, 0.0}; + return {initial_motion, InitializeType::EGO_VELOCITY}; + } + + // when velocity tracking deviation is large + const double desired_vel = current_closest_point_from_prev_output_->longitudinal_velocity_mps; + const double desired_acc = current_closest_point_from_prev_output_->acceleration_mps2; + const double vel_error = vehicle_speed - std::fabs(desired_vel); + + if (std::fabs(vel_error) > node_param_.replan_vel_deviation) { + RCLCPP_DEBUG( + get_logger(), + "calcInitialMotion : Large deviation error for speed control. Use current speed for " + "initial value, desired_vel = %f, vehicle_speed = %f, vel_error = %f, error_thr = %f", + desired_vel, vehicle_speed, vel_error, node_param_.replan_vel_deviation); + Motion initial_motion = {vehicle_speed, desired_acc}; // TODO(Horibe): use current acc + return {initial_motion, InitializeType::LARGE_DEVIATION_REPLAN}; + } + + // if current vehicle velocity is low && base_desired speed is high, + // use engage_velocity for engage vehicle + const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; + if (vehicle_speed < engage_vel_thr) { + if (target_vel >= node_param_.engage_velocity) { + const double stop_dist = trajectory_utils::calcStopDistance(input_traj, input_closest); + if (stop_dist > node_param_.stop_dist_to_prohibit_engage) { + RCLCPP_DEBUG( + get_logger(), + "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " + "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", + vehicle_speed, target_vel, node_param_.engage_velocity, engage_vel_thr, stop_dist); + const double engage_acceleration = + external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_acceleration + : node_param_.engage_acceleration; + Motion initial_motion = {node_param_.engage_velocity, engage_acceleration}; + return {initial_motion, InitializeType::ENGAGING}; + } + RCLCPP_DEBUG( + get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); + } else if (target_vel > 0.0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *clock_, 3000, + "calcInitialMotion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ", + target_vel, node_param_.engage_velocity); + } + } + + // If the control mode is not AUTONOMOUS (vehicle is not under control of the planning module), + // use ego velocity/acceleration in the planning for smooth transition from MANUAL to AUTONOMOUS. + if (node_param_.plan_from_ego_speed_on_manual_mode) { // could be false for debug purpose + const bool is_in_autonomous_control = operation_mode_.is_autoware_control_enabled && + (operation_mode_.mode == OperationModeState::AUTONOMOUS || + operation_mode_.mode == OperationModeState::STOP); + if (!is_in_autonomous_control) { + RCLCPP_INFO_THROTTLE( + get_logger(), *clock_, 10000, "Not in autonomous control. Plan from ego velocity."); + // We should plan from the current vehicle speed, but if the initial value is greater than the + // velocity limit, the current planning algorithm decelerates with a very high deceleration. + // To avoid this, we set the initial value of the vehicle speed to be below the speed limit. + const auto p = smoother_->getBaseParam(); + const auto v0 = std::min(target_vel, vehicle_speed); + const auto a0 = std::clamp(vehicle_acceleration, p.min_decel, p.max_accel); + const Motion initial_motion = {v0, a0}; + return {initial_motion, InitializeType::EGO_VELOCITY}; + } + } + + // normal update: use closest in current_closest_point_from_prev_output + Motion initial_motion = {desired_vel, desired_acc}; + RCLCPP_DEBUG( + get_logger(), + "calcInitialMotion : normal update. v0 = %f, a0 = %f, vehicle_speed = %f, target_vel = %f", + initial_motion.vel, initial_motion.acc, vehicle_speed, target_vel); + return {initial_motion, InitializeType::NORMAL}; +} + +void VelocitySmootherNode::overwriteStopPoint( + const TrajectoryPoints & input, TrajectoryPoints & output) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input); + if (!stop_idx) { + return; + } + + // Get Closest Point from Output + // TODO(planning/control team) deal with overlapped lanes with the same directions + const auto nearest_output_point_idx = autoware::motion_utils::findNearestIndex( + output, input.at(*stop_idx).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + + // check over velocity + bool is_stop_velocity_exceeded{false}; + double input_stop_vel{}; + double output_stop_vel{}; + if (nearest_output_point_idx) { + double optimized_stop_point_vel = + output.at(*nearest_output_point_idx).longitudinal_velocity_mps; + is_stop_velocity_exceeded = (optimized_stop_point_vel > over_stop_velocity_warn_thr_); + input_stop_vel = input.at(*stop_idx).longitudinal_velocity_mps; + output_stop_vel = output.at(*nearest_output_point_idx).longitudinal_velocity_mps; + trajectory_utils::applyMaximumVelocityLimit( + *nearest_output_point_idx, output.size(), 0.0, output); + RCLCPP_DEBUG( + get_logger(), + "replan : input_stop_idx = %lu, stop velocity : input = %f, output = %f, thr = %f", + *nearest_output_point_idx, input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_); + } else { + input_stop_vel = -1.0; + output_stop_vel = -1.0; + RCLCPP_DEBUG( + get_logger(), + "replan : input_stop_idx = -1, stop velocity : input = %f, output = %f, thr = %f", + input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_); + } + + diagnostics_interface_->add_key_value( + "The velocity on the stop point is larger than 0.", is_stop_velocity_exceeded); +} + +void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (traj.size() < 1) { + return; + } + + trajectory_utils::applyMaximumVelocityLimit( + 0, traj.size(), max_velocity_with_deceleration_, traj); + + // insert the point at the distance of external velocity limit + const auto & current_pose = current_odometry_ptr_->pose.pose; + const size_t closest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj, current_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto inserted_index = + autoware::motion_utils::insertTargetPoint(closest_seg_idx, external_velocity_limit_.dist, traj); + if (!inserted_index) { + traj.back().longitudinal_velocity_mps = std::min( + traj.back().longitudinal_velocity_mps, static_cast(external_velocity_limit_.velocity)); + return; + } + + // apply external velocity limit from the inserted point + trajectory_utils::applyMaximumVelocityLimit( + *inserted_index, traj.size(), external_velocity_limit_.velocity, traj); + + // create virtual wall + if (std::abs(external_velocity_limit_.velocity) < 1e-3) { + const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( + traj.at(*inserted_index).pose, external_velocity_limit_.sender, this->now(), 0, + base_link2front_); + pub_virtual_wall_->publish(virtual_wall_marker); + } + + RCLCPP_DEBUG( + get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity); +} + +void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj); + if (!stop_idx) { + return; // no stop point. + } + double distance_sum = 0.0; + for (size_t i = *stop_idx - 1; i < traj.size(); --i) { // search backward + distance_sum += autoware_utils::calc_distance2d(traj.at(i), traj.at(i + 1)); + if (distance_sum > node_param_.stopping_distance) { + break; + } + if (traj.at(i).longitudinal_velocity_mps > node_param_.stopping_velocity) { + traj.at(i).longitudinal_velocity_mps = node_param_.stopping_velocity; + } + } +} + +void VelocitySmootherNode::publishDebugTrajectories( + const std::vector & debug_trajectories) const +{ + auto debug_trajectories_tmp = debug_trajectories; + if (node_param_.algorithm_type == AlgorithmType::JERK_FILTERED) { + if (debug_trajectories_tmp.size() != 3) { + RCLCPP_DEBUG(get_logger(), "Size of the debug trajectories is incorrect"); + return; + } + if (is_reverse_) { + flipVelocity(debug_trajectories_tmp.at(0)); + flipVelocity(debug_trajectories_tmp.at(1)); + flipVelocity(debug_trajectories_tmp.at(2)); + } + pub_forward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(0))); + pub_backward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(1))); + pub_merged_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(2))); + publishClosestVelocity( + debug_trajectories_tmp.at(2), current_odometry_ptr_->pose.pose, pub_closest_merged_velocity_); + } +} + +void VelocitySmootherNode::publishClosestVelocity( + const TrajectoryPoints & trajectory, const Pose & current_pose, + const rclcpp::Publisher::SharedPtr pub) const +{ + const auto closest_point = calcProjectedTrajectoryPoint(trajectory, current_pose); + + Float32Stamped vel_data{}; + vel_data.stamp = this->now(); + vel_data.data = std::max(closest_point.longitudinal_velocity_mps, static_cast(0.0)); + pub->publish(vel_data); +} + +void VelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) +{ + const auto closest_point = calcProjectedTrajectoryPointFromEgo(trajectory); + + auto publishFloat = [=](const double data, const auto pub) { + Float32Stamped msg{}; + msg.stamp = this->now(); + msg.data = data; + pub->publish(msg); + return; + }; + + const double curr_vel{closest_point.longitudinal_velocity_mps}; + const double curr_acc{closest_point.acceleration_mps2}; + if (!prev_time_) { + prev_time_ = std::make_shared(this->now()); + prev_acc_ = curr_acc; + return; + } + + // Calculate jerk + rclcpp::Time curr_time{this->now()}; + double dt = (curr_time - *prev_time_).seconds(); + double curr_jerk = (curr_acc - prev_acc_) / dt; + + // Publish data + publishFloat(curr_vel, debug_closest_velocity_); + publishFloat(curr_acc, debug_closest_acc_); + publishFloat(curr_jerk, debug_closest_jerk_); + + // Update + prev_acc_ = curr_acc; + *prev_time_ = curr_time; +} + +void VelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) +{ + prev_output_ = final_result; + prev_closest_point_ = calcProjectedTrajectoryPointFromEgo(final_result); +} + +VelocitySmootherNode::AlgorithmType VelocitySmootherNode::getAlgorithmType( + const std::string & algorithm_name) const +{ + if (algorithm_name == "JerkFiltered") { + return AlgorithmType::JERK_FILTERED; + } + if (algorithm_name == "L2") { + return AlgorithmType::L2; + } + if (algorithm_name == "Linf") { + return AlgorithmType::LINF; + } + if (algorithm_name == "Analytical") { + return AlgorithmType::ANALYTICAL; + } + + throw std::domain_error("[VelocitySmootherNode] undesired algorithm is selected."); +} + +double VelocitySmootherNode::calcTravelDistance() const +{ + const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_); + + if (prev_closest_point_) { + const double travel_dist = autoware_utils::calc_distance2d(*prev_closest_point_, closest_point); + return travel_dist; + } + + return 0.0; +} + +bool VelocitySmootherNode::isEngageStatus(const double target_vel) const +{ + const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); + const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; + return vehicle_speed < engage_vel_thr && target_vel >= node_param_.engage_velocity; +} + +Trajectory VelocitySmootherNode::toTrajectoryMsg( + const TrajectoryPoints & points, const std_msgs::msg::Header * header) const +{ + auto trajectory = autoware::motion_utils::convertToTrajectory(points); + trajectory.header = header ? *header : base_traj_raw_ptr_->header; + return trajectory; +} + +size_t VelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const +{ + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); +} + +bool VelocitySmootherNode::isReverse(const TrajectoryPoints & points) const +{ + if (points.empty()) return true; + + return std::any_of( + points.begin(), points.end(), [](const auto & pt) { return pt.longitudinal_velocity_mps < 0; }); +} +void VelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const +{ + for (auto & pt : points) { + pt.longitudinal_velocity_mps *= -1.0; + } +} + +void VelocitySmootherNode::publishStopWatchTime() +{ + Float64Stamped calculation_time_data{}; + calculation_time_data.stamp = this->now(); + calculation_time_data.data = stop_watch_.toc(); + debug_calculation_time_->publish(calculation_time_data); +} + +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & pose) const +{ + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); +} + +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( + const TrajectoryPoints & trajectory) const +{ + return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); +} + +} // namespace autoware::velocity_smoother + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::velocity_smoother::VelocitySmootherNode) diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp new file mode 100644 index 0000000000..cb8710c66a --- /dev/null +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -0,0 +1,270 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/resample.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" + +#include +#include + +namespace autoware::velocity_smoother +{ +namespace resampling +{ +TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const double v_current, + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v) +{ + // Arc length from the initial point to the closest point + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = autoware::motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, 0); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); + + const auto dist_to_closest_stop_point = + autoware::motion_utils::calcDistanceToForwardStopPoint(input, current_pose); + + // Get the resample size from the closest point + const double trajectory_length = autoware::motion_utils::calcArcLength(input); + const double Nt = param.resample_time / std::max(param.dense_resample_dt, 0.001); + const double ds_nominal = + std::max(v_current * param.dense_resample_dt, param.dense_min_interval_distance); + const double Ns = param.min_trajectory_length / std::max(ds_nominal, 0.001); + const double N = std::max(Nt, Ns); + std::vector out_arclength; + + // Step1. Resample front trajectory + constexpr double front_ds = 0.1; + for (double ds = 0.0; ds <= front_arclength_value; ds += front_ds) { + out_arclength.push_back(ds); + } + if (std::fabs(out_arclength.back() - front_arclength_value) < 1e-3) { + out_arclength.back() = front_arclength_value; + } else { + out_arclength.push_back(front_arclength_value); + } + + // Step2. Resample behind trajectory + double dist_i = 0.0; + bool is_zero_point_included = false; + bool is_endpoint_included = false; + for (size_t i = 1; static_cast(i) <= N; ++i) { + double ds = ds_nominal; + if (i > Nt) { + // if the planning time is not enough to see the desired distance, + // change the interval distance to see far. + ds = std::max(param.sparse_min_interval_distance, param.sparse_resample_dt * v_current); + } + + dist_i += ds; + + // Check if the distance is longer than max_trajectory_length + if (dist_i > param.max_trajectory_length) { + break; // distance is over max. + } + + // Check if the distance is longer than input arclength + if (dist_i + front_arclength_value >= trajectory_length) { + is_endpoint_included = true; // distance is over input endpoint. + break; + } + + // Check if the distance is longer than minimum_trajectory_length + if (i > Nt && dist_i >= param.min_trajectory_length) { + if ( + std::fabs(out_arclength.back() - (param.min_trajectory_length + front_arclength_value)) < + 1e-3) { + out_arclength.back() = param.min_trajectory_length + front_arclength_value; + } else { + out_arclength.push_back(param.min_trajectory_length + front_arclength_value); + } + break; + } + + // Handle Close Stop Point + if ( + dist_to_closest_stop_point && dist_i > *dist_to_closest_stop_point && + !is_zero_point_included) { + if (std::fabs(dist_i - *dist_to_closest_stop_point) > 1e-3) { + // dist_i is much bigger than zero_vel_arclength_value + if ( + !out_arclength.empty() && + std::fabs(out_arclength.back() - (*dist_to_closest_stop_point + front_arclength_value)) < + 1e-3) { + out_arclength.back() = *dist_to_closest_stop_point + front_arclength_value; + } else { + out_arclength.push_back(*dist_to_closest_stop_point + front_arclength_value); + } + } else { + // dist_i is close to the zero_vel_arclength_value + dist_i = *dist_to_closest_stop_point; + } + + is_zero_point_included = true; + } + + out_arclength.push_back(dist_i + front_arclength_value); + } + + if (input.size() < 2 || out_arclength.size() < 2 || trajectory_length < out_arclength.back()) { + return input; + } + + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); + auto output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); + + // add end point directly to consider the endpoint velocity. + if (is_endpoint_included) { + constexpr double ep_dist = 1.0E-3; + if (autoware_utils::calc_distance2d(output.back(), input.back()) < ep_dist) { + output.back() = input.back(); + } else { + output.push_back(input.back()); + } + } + + return output; +} + +TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v) +{ + // input arclength + const double trajectory_length = autoware::motion_utils::calcArcLength(input); + const auto dist_to_closest_stop_point = + autoware::motion_utils::calcDistanceToForwardStopPoint(input, current_pose); + + // distance to stop point + double stop_arclength_value = param.max_trajectory_length; + if (dist_to_closest_stop_point) { + stop_arclength_value = std::min(*dist_to_closest_stop_point, stop_arclength_value); + } + if (param.min_trajectory_length < stop_arclength_value) { + stop_arclength_value = param.min_trajectory_length; + } + + // Do dense resampling before the stop line(3[m] ahead of the stop line) + const double start_stop_arclength_value = std::max(stop_arclength_value - 3.0, 0.0); + + std::vector out_arclength; + + // Step1. Resample front trajectory + // Arc length from the initial point to the closest point + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = autoware::motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, + static_cast(0)); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); + for (double s = 0.0; s <= front_arclength_value; s += nominal_ds) { + out_arclength.push_back(s); + } + if (std::fabs(out_arclength.back() - front_arclength_value) < 1e-3) { + out_arclength.back() = front_arclength_value; + } else { + out_arclength.push_back(front_arclength_value); + } + + // Step2. Resample behind trajectory + double dist_i{0.0}; + bool is_zero_point_included{false}; + bool is_endpoint_included{false}; + while (rclcpp::ok()) { + double ds = nominal_ds; + if (start_stop_arclength_value <= dist_i && dist_i <= stop_arclength_value) { + // Dense sampling before the stop point + ds = 0.01; + } + dist_i += ds; + + // Check if the distance is longer than max_trajectory_length + if (dist_i > param.max_trajectory_length) { + break; // distance is over max. + } + + // Check if the distance is longer than input arclength + if (dist_i + front_arclength_value >= trajectory_length) { + is_endpoint_included = true; // distance is over input endpoint. + break; + } + + // Check if the distance is longer than minimum_trajectory_length + if (dist_i >= param.min_trajectory_length) { + if ( + std::fabs(out_arclength.back() - (param.min_trajectory_length + front_arclength_value)) < + 1e-3) { + out_arclength.back() = param.min_trajectory_length + front_arclength_value; + } else { + out_arclength.push_back(param.min_trajectory_length + front_arclength_value); + } + break; + } + + // Handle Close Stop Point + if (dist_i > stop_arclength_value && !is_zero_point_included) { + if (std::fabs(dist_i - stop_arclength_value) > 1e-3) { + // dist_i is much bigger than zero_vel_arclength_value + if ( + !out_arclength.empty() && + std::fabs(out_arclength.back() - (stop_arclength_value + front_arclength_value)) < 1e-3) { + out_arclength.back() = stop_arclength_value + front_arclength_value; + } else { + out_arclength.push_back(stop_arclength_value + front_arclength_value); + } + } else { + // dist_i is close to the zero_vel_arclength_value + dist_i = stop_arclength_value; + } + + is_zero_point_included = true; + } + + out_arclength.push_back(dist_i + front_arclength_value); + } + + if (input.size() < 2 || out_arclength.size() < 2 || trajectory_length < out_arclength.back()) { + return input; + } + + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); + auto output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); + + // add end point directly to consider the endpoint velocity. + if (is_endpoint_included) { + constexpr double ep_dist = 1.0E-3; + if (autoware_utils::calc_distance2d(output.back(), input.back()) < ep_dist) { + output.back() = input.back(); + } else { + output.push_back(input.back()); + } + } + + return output; +} + +} // namespace resampling +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp new file mode 100644 index 0000000000..e385c1f9c1 --- /dev/null +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -0,0 +1,662 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" + +#include +#include +#include +#include +#include +#include + +namespace +{ +using TrajectoryPoints = std::vector; + +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) +{ + tf2::Transform tf_transform1, tf_transform2; + tf2::fromMsg(p1, tf_transform1); + tf2::fromMsg(p2, tf_transform2); + const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); + const auto & tf_quaternion = + tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); + + geometry_msgs::msg::Pose pose; + pose.position.x = tf_point.getX(); + pose.position.y = tf_point.getY(); + pose.position.z = tf_point.getZ(); + pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} + +bool applyMaxVelocity( + const double max_velocity, const size_t start_index, const size_t end_index, + TrajectoryPoints & output_trajectory) +{ + if (end_index < start_index || output_trajectory.size() < end_index) { + return false; + } + + for (size_t i = start_index; i <= end_index; ++i) { + output_trajectory.at(i).longitudinal_velocity_mps = + std::min(output_trajectory.at(i).longitudinal_velocity_mps, static_cast(max_velocity)); + output_trajectory.at(i).acceleration_mps2 = 0.0; + } + return true; +} + +} // namespace + +namespace autoware::velocity_smoother +{ +AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) +{ + auto & p = smoother_param_; + p.resample.ds_resample = node.declare_parameter("resample.ds_resample"); + p.resample.num_resample = node.declare_parameter("resample.num_resample"); + p.resample.delta_yaw_threshold = node.declare_parameter("resample.delta_yaw_threshold"); + p.latacc.enable_constant_velocity_while_turning = + node.declare_parameter("latacc.enable_constant_velocity_while_turning"); + p.latacc.constant_velocity_dist_threshold = + node.declare_parameter("latacc.constant_velocity_dist_threshold"); + p.forward.max_acc = node.declare_parameter("forward.max_acc"); + p.forward.min_acc = node.declare_parameter("forward.min_acc"); + p.forward.max_jerk = node.declare_parameter("forward.max_jerk"); + p.forward.min_jerk = node.declare_parameter("forward.min_jerk"); + p.forward.kp = node.declare_parameter("forward.kp"); + p.backward.start_jerk = node.declare_parameter("backward.start_jerk"); + p.backward.min_jerk_mild_stop = node.declare_parameter("backward.min_jerk_mild_stop"); + p.backward.min_jerk = node.declare_parameter("backward.min_jerk"); + p.backward.min_acc_mild_stop = node.declare_parameter("backward.min_acc_mild_stop"); + p.backward.min_acc = node.declare_parameter("backward.min_acc"); + p.backward.span_jerk = node.declare_parameter("backward.span_jerk"); +} + +void AnalyticalJerkConstrainedSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +AnalyticalJerkConstrainedSmoother::Param AnalyticalJerkConstrainedSmoother::getParam() const +{ + return smoother_param_; +} + +bool AnalyticalJerkConstrainedSmoother::apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, [[maybe_unused]] std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) +{ + RCLCPP_DEBUG(logger_, "-------------------- Start --------------------"); + + // guard + if (input.empty()) { + RCLCPP_DEBUG(logger_, "Fail. input trajectory is empty"); + return false; + } + + // intput trajectory is cropped, so closest_index = 0 + const size_t closest_index = 0; + + // Find deceleration targets + if (input.size() == 1) { + RCLCPP_DEBUG( + logger_, + "Input trajectory size is too short. Cannot find decel targets and " + "return v0, a0"); + output = input; + output.front().longitudinal_velocity_mps = initial_vel; + output.front().acceleration_mps2 = initial_acc; + return true; + } + std::vector> decel_target_indices; + searchDecelTargetIndices(input, closest_index, decel_target_indices); + RCLCPP_DEBUG(logger_, "Num deceleration targets: %zd", decel_target_indices.size()); + for (auto & index : decel_target_indices) { + RCLCPP_DEBUG( + logger_, "Target deceleration index: %ld, target velocity: %f", index.first, index.second); + } + + // Apply filters according to deceleration targets + TrajectoryPoints reference_trajectory = input; + TrajectoryPoints filtered_trajectory = input; + for (size_t i = 0; i < decel_target_indices.size(); ++i) { + size_t fwd_start_index; + double fwd_start_vel; + double fwd_start_acc; + if (i == 0) { + fwd_start_index = closest_index; + fwd_start_vel = initial_vel; + fwd_start_acc = initial_acc; + } else { + fwd_start_index = decel_target_indices.at(i - 1).first; + fwd_start_vel = filtered_trajectory.at(fwd_start_index).longitudinal_velocity_mps; + fwd_start_acc = filtered_trajectory.at(fwd_start_index).acceleration_mps2; + } + + RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", fwd_start_index); + applyForwardJerkFilter( + reference_trajectory, fwd_start_index, fwd_start_vel, fwd_start_acc, smoother_param_, + filtered_trajectory); + + size_t bwd_start_index = closest_index; + double bwd_start_vel = initial_vel; + double bwd_start_acc = initial_acc; + for (int j = i; j >= 0; --j) { + if (j == 0) { + bwd_start_index = closest_index; + bwd_start_vel = initial_vel; + bwd_start_acc = initial_acc; + break; + } + if (decel_target_indices.at(j - 1).second < decel_target_indices.at(j).second) { + bwd_start_index = decel_target_indices.at(j - 1).first; + bwd_start_vel = filtered_trajectory.at(bwd_start_index).longitudinal_velocity_mps; + bwd_start_acc = filtered_trajectory.at(bwd_start_index).acceleration_mps2; + break; + } + } + std::vector start_indices; + if (bwd_start_index != fwd_start_index) { + start_indices.push_back(bwd_start_index); + start_indices.push_back(fwd_start_index); + } else { + start_indices.push_back(bwd_start_index); + } + + const size_t decel_target_index = decel_target_indices.at(i).first; + const double decel_target_vel = decel_target_indices.at(i).second; + RCLCPP_DEBUG( + logger_, "Apply backward decel filter from: %s, to: %ld (%f)", + strStartIndices(start_indices).c_str(), decel_target_index, decel_target_vel); + if (!applyBackwardDecelFilter( + start_indices, decel_target_index, decel_target_vel, smoother_param_, + filtered_trajectory)) { + RCLCPP_DEBUG( + logger_, + "Failed to apply backward decel filter, so apply max velocity filter. max velocity = %f, " + "start_index = %s, end_index = %zd", + decel_target_vel, strStartIndices(start_indices).c_str(), filtered_trajectory.size() - 1); + + const double ep = 0.001; + if (std::abs(decel_target_vel) < ep) { + applyMaxVelocity(0.0, bwd_start_index, filtered_trajectory.size() - 1, filtered_trajectory); + output = filtered_trajectory; + RCLCPP_DEBUG(logger_, "-------------------- Finish --------------------"); + return true; + } + applyMaxVelocity(decel_target_vel, bwd_start_index, decel_target_index, reference_trajectory); + RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", bwd_start_index); + applyForwardJerkFilter( + reference_trajectory, bwd_start_index, bwd_start_vel, bwd_start_acc, smoother_param_, + filtered_trajectory); + } + } + + size_t start_index; + double start_vel; + double start_acc; + if (decel_target_indices.empty() == true) { + start_index = closest_index; + start_vel = initial_vel; + start_acc = initial_acc; + } else { + start_index = decel_target_indices.back().first; + start_vel = filtered_trajectory.at(start_index).longitudinal_velocity_mps; + start_acc = filtered_trajectory.at(start_index).acceleration_mps2; + } + RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", start_index); + applyForwardJerkFilter( + reference_trajectory, start_index, start_vel, start_acc, smoother_param_, filtered_trajectory); + + output = filtered_trajectory; + + RCLCPP_DEBUG(logger_, "-------------------- Finish --------------------"); + return true; +} + +TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose, + [[maybe_unused]] const double nearest_dist_threshold, + [[maybe_unused]] const double nearest_yaw_threshold) const +{ + TrajectoryPoints output; + if (input.empty()) { + RCLCPP_WARN(logger_, "Input trajectory is empty"); + return input; + } + + const double ds = 1.0 / static_cast(smoother_param_.resample.num_resample); + + for (size_t i = 0; i < input.size() - 1; ++i) { + double s = 0.0; + const auto tp0 = input.at(i); + const auto tp1 = input.at(i + 1); + + const double dist_thr = 0.001; // 1mm + const double dist_tp0_tp1 = autoware_utils::calc_distance2d(tp0, tp1); + if (std::fabs(dist_tp0_tp1) < dist_thr) { + output.push_back(input.at(i)); + continue; + } + + for (size_t j = 0; j < static_cast(smoother_param_.resample.num_resample); ++j) { + auto tp = input.at(i); + + tp.pose = lerpByPose(tp0.pose, tp1.pose, s); + tp.longitudinal_velocity_mps = tp0.longitudinal_velocity_mps; + tp.heading_rate_rps = (1.0 - s) * tp0.heading_rate_rps + s * tp1.heading_rate_rps; + tp.acceleration_mps2 = tp0.acceleration_mps2; + // tp.accel.angular.z = (1.0 - s) * tp0.accel.angular.z + s * tp1.accel.angular.z; + + output.push_back(tp); + + s += ds; + } + } + + output.push_back(input.back()); + + return output; +} + +TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling, const double input_points_interval) const +{ + if (input.size() < 3) { + return input; // cannot calculate lateral acc. do nothing. + } + + // Interpolate with constant interval distance for lateral acceleration calculation. + const double points_interval = use_resampling ? 0.1 : input_points_interval; // [m] + + TrajectoryPoints output; + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const std::vector in_arclength = trajectory_utils::calcArclengthArray(input); + for (double s = 0; s < in_arclength.back(); s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength); + output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; + } + + constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points + const size_t idx_dist = + static_cast(std::max(static_cast((curvature_calc_dist) / points_interval), 1)); + + // Calculate curvature assuming the trajectory points interval is constant + const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); + + // Decrease speed according to lateral G + const size_t before_decel_index = + static_cast(std::round(base_param_.decel_distance_before_curve / points_interval)); + const size_t after_decel_index = + static_cast(std::round(base_param_.decel_distance_after_curve / points_interval)); + const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel); + + std::vector filtered_points; + for (size_t i = 0; i < output.size(); ++i) { + double curvature = 0.0; + const size_t start = i > before_decel_index ? i - before_decel_index : 0; + const size_t end = std::min(output.size(), i + after_decel_index); + for (size_t j = start; j < end; ++j) { + curvature = std::max(curvature, std::fabs(curvature_v.at(j))); + } + double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5)); + v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity); + if (output.at(i).longitudinal_velocity_mps > v_curvature_max) { + output.at(i).longitudinal_velocity_mps = v_curvature_max; + filtered_points.push_back(i); + } + } + + // Keep constant velocity while turning + const double dist_threshold = smoother_param_.latacc.constant_velocity_dist_threshold; + std::vector> latacc_filtered_ranges; + size_t start_index = 0; + size_t end_index = 0; + bool is_updated = false; + double min_latacc_velocity; + for (size_t i = 0; i < filtered_points.size(); ++i) { + const size_t index = filtered_points.at(i); + + if (is_updated == false) { + start_index = index; + end_index = index; + min_latacc_velocity = output.at(index).longitudinal_velocity_mps; + is_updated = true; + continue; + } + + if (autoware_utils::calc_distance2d(output.at(end_index), output.at(index)) < dist_threshold) { + end_index = index; + min_latacc_velocity = std::min( + static_cast(output.at(index).longitudinal_velocity_mps), min_latacc_velocity); + } else { + latacc_filtered_ranges.emplace_back(start_index, end_index, min_latacc_velocity); + start_index = index; + end_index = index; + min_latacc_velocity = output.at(index).longitudinal_velocity_mps; + } + } + if (is_updated) { + latacc_filtered_ranges.emplace_back(start_index, end_index, min_latacc_velocity); + } + + for (size_t i = 0; i < output.size(); ++i) { + for (const auto & lat_acc_filtered_range : latacc_filtered_ranges) { + const size_t filtered_start_index = std::get<0>(lat_acc_filtered_range); + const size_t filtered_end_index = std::get<1>(lat_acc_filtered_range); + const double filtered_min_latacc_velocity = std::get<2>(lat_acc_filtered_range); + + if ( + filtered_start_index <= i && i <= filtered_end_index && + smoother_param_.latacc.enable_constant_velocity_while_turning) { + output.at(i).longitudinal_velocity_mps = filtered_min_latacc_velocity; + break; + } + } + } + + return output; +} + +bool AnalyticalJerkConstrainedSmoother::searchDecelTargetIndices( + const TrajectoryPoints & trajectory, const size_t closest_index, + std::vector> & decel_target_indices) const +{ + const double ep = -0.00001; + const size_t start_index = std::max(1, closest_index); + std::vector> tmp_indices; + for (size_t i = start_index; i < trajectory.size() - 1; ++i) { + const double dv_before = + trajectory.at(i).longitudinal_velocity_mps - trajectory.at(i - 1).longitudinal_velocity_mps; + const double dv_after = + trajectory.at(i + 1).longitudinal_velocity_mps - trajectory.at(i).longitudinal_velocity_mps; + if (dv_before < ep && dv_after > ep) { + tmp_indices.emplace_back(i, trajectory.at(i).longitudinal_velocity_mps); + } + } + + const unsigned int i = trajectory.size() - 1; + const double dv_before = + trajectory.at(i).longitudinal_velocity_mps - trajectory.at(i - 1).longitudinal_velocity_mps; + if (dv_before < ep) { + tmp_indices.emplace_back(i, trajectory.at(i).longitudinal_velocity_mps); + } + + if (!tmp_indices.empty()) { + for (unsigned int j = 0; j < tmp_indices.size() - 1; ++j) { + const size_t index_err = 10; + if ( + (tmp_indices.at(j + 1).first - tmp_indices.at(j).first < index_err) && + (tmp_indices.at(j + 1).second < tmp_indices.at(j).second)) { + continue; + } + + decel_target_indices.emplace_back(tmp_indices.at(j).first, tmp_indices.at(j).second); + } + } + if (!tmp_indices.empty()) { + decel_target_indices.emplace_back(tmp_indices.back().first, tmp_indices.back().second); + } + return true; +} + +bool AnalyticalJerkConstrainedSmoother::applyForwardJerkFilter( + const TrajectoryPoints & base_trajectory, const size_t start_index, const double initial_vel, + const double initial_acc, const Param & params, TrajectoryPoints & output_trajectory) const +{ + output_trajectory.at(start_index).longitudinal_velocity_mps = initial_vel; + output_trajectory.at(start_index).acceleration_mps2 = initial_acc; + + for (size_t i = start_index + 1; i < base_trajectory.size(); ++i) { + const double prev_vel = output_trajectory.at(i - 1).longitudinal_velocity_mps; + const double ds = + autoware_utils::calc_distance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); + const double dt = ds / std::max(prev_vel, 1.0); + + const double prev_acc = output_trajectory.at(i - 1).acceleration_mps2; + const double curr_vel = std::max(prev_vel + prev_acc * dt, 0.0); + + const double error_vel = base_trajectory.at(i).longitudinal_velocity_mps - curr_vel; + const double fb_acc = params.forward.kp * error_vel; + const double limited_acc = + std::max(params.forward.min_acc, std::min(params.forward.max_acc, fb_acc)); + const double fb_jerk = (limited_acc - prev_acc) / dt; + const double limited_jerk = + std::max(params.forward.min_jerk, std::min(params.forward.max_jerk, fb_jerk)); + + const double curr_acc = prev_acc + limited_jerk * dt; + + output_trajectory.at(i).longitudinal_velocity_mps = curr_vel; + output_trajectory.at(i).acceleration_mps2 = curr_acc; + } + + return true; +} + +bool AnalyticalJerkConstrainedSmoother::applyBackwardDecelFilter( + const std::vector & start_indices, const size_t decel_target_index, + const double decel_target_vel, const Param & params, TrajectoryPoints & output_trajectory) const +{ + const double ep = 0.001; + + double output_planning_jerk = -100.0; + size_t output_start_index = 0; + std::vector output_dist_to_target; + int output_type; + std::vector output_times; + + for (size_t start_index : start_indices) { + double dist = 0.0; + std::vector dist_to_target(output_trajectory.size(), 0); + dist_to_target.at(decel_target_index) = dist; + for (size_t i = start_index; i < decel_target_index; ++i) { + if (output_trajectory.at(i).longitudinal_velocity_mps >= decel_target_vel) { + start_index = i; + break; + } + } + for (size_t i = decel_target_index; i > start_index; --i) { + dist += autoware_utils::calc_distance2d(output_trajectory.at(i - 1), output_trajectory.at(i)); + dist_to_target.at(i - 1) = dist; + } + + RCLCPP_DEBUG(logger_, "Check enough dist to decel. start_index: %ld", start_index); + double planning_jerk; + int type; + std::vector times; + double stop_dist; + bool is_enough_dist = false; + for (planning_jerk = params.backward.start_jerk; planning_jerk > params.backward.min_jerk - ep; + planning_jerk += params.backward.span_jerk) { + if (calcEnoughDistForDecel( + output_trajectory, start_index, decel_target_vel, planning_jerk, params, dist_to_target, + is_enough_dist, type, times, stop_dist)) { + break; + } + } + + if (!is_enough_dist) { + RCLCPP_DEBUG(logger_, "Distance is not enough for decel with all jerk condition"); + continue; + } + + if (planning_jerk >= output_planning_jerk) { + output_planning_jerk = planning_jerk; + output_start_index = start_index; + output_dist_to_target = dist_to_target; + output_type = type; + output_times = times; + RCLCPP_DEBUG( + logger_, "Update planning jerk: %f, start_index: %ld", planning_jerk, start_index); + } + } + + if (output_planning_jerk == -100.0) { + RCLCPP_DEBUG( + logger_, + "Distance is not enough for decel with all jerk and start index " + "condition"); + return false; + } + + RCLCPP_DEBUG(logger_, "Search decel start index"); + size_t decel_start_index = output_start_index; + if (output_planning_jerk == params.backward.start_jerk) { + for (size_t i = decel_target_index - 1; i >= output_start_index; --i) { + bool is_enough_dist = false; + double stop_dist; + if (calcEnoughDistForDecel( + output_trajectory, i, decel_target_vel, output_planning_jerk, params, + output_dist_to_target, is_enough_dist, output_type, output_times, stop_dist)) { + decel_start_index = i; + break; + } + } + } + + RCLCPP_DEBUG( + logger_, + "Apply filter. decel_start_index: %ld, target_vel: %f, " + "planning_jerk: %f, type: %d, times: %s", + decel_start_index, decel_target_vel, output_planning_jerk, output_type, + strTimes(output_times).c_str()); + if (!applyDecelVelocityFilter( + decel_start_index, decel_target_vel, output_planning_jerk, params, output_type, + output_times, output_trajectory)) { + RCLCPP_DEBUG( + logger_, + "[applyDecelVelocityFilter] dist is enough, but fail to plan backward decel velocity"); + return false; + } + + return true; +} + +bool AnalyticalJerkConstrainedSmoother::calcEnoughDistForDecel( + const TrajectoryPoints & trajectory, const size_t start_index, const double decel_target_vel, + const double planning_jerk, const Param & params, const std::vector & dist_to_target, + bool & is_enough_dist, int & type, std::vector & times, double & stop_dist) const +{ + const double v0 = trajectory.at(start_index).longitudinal_velocity_mps; + const double a0 = trajectory.at(start_index).acceleration_mps2; + const double jerk_acc = std::abs(planning_jerk); + const double jerk_dec = planning_jerk; + auto calcMinAcc = [¶ms](const double planning_jerk) { + if (planning_jerk < params.backward.min_jerk_mild_stop) { + return params.backward.min_acc; + } + return params.backward.min_acc_mild_stop; + }; + const double min_acc = calcMinAcc(planning_jerk); + type = 0; + times.clear(); + stop_dist = 0.0; + + if (!analytical_velocity_planning_utils::calcStopDistWithJerkAndAccConstraints( + v0, a0, jerk_acc, jerk_dec, min_acc, decel_target_vel, type, times, stop_dist)) { + return false; + } + + const double allowed_dist = dist_to_target.at(start_index); + if (0.0 <= stop_dist && stop_dist <= allowed_dist) { + RCLCPP_DEBUG( + logger_, + "Distance is enough. v0: %f, a0: %f, jerk: %f, stop_dist: %f, " + "allowed_dist: %f", + v0, a0, planning_jerk, stop_dist, allowed_dist); + is_enough_dist = true; + return true; + } + RCLCPP_DEBUG( + logger_, + "Distance is not enough. v0: %f, a0: %f, jerk: %f, stop_dist: %f, " + "allowed_dist: %f", + v0, a0, planning_jerk, stop_dist, allowed_dist); + return false; +} + +bool AnalyticalJerkConstrainedSmoother::applyDecelVelocityFilter( + const size_t decel_start_index, const double decel_target_vel, const double planning_jerk, + const Param & params, const int type, const std::vector & times, + TrajectoryPoints & output_trajectory) const +{ + const double v0 = output_trajectory.at(decel_start_index).longitudinal_velocity_mps; + const double a0 = output_trajectory.at(decel_start_index).acceleration_mps2; + const double jerk_acc = std::abs(planning_jerk); + const double jerk_dec = planning_jerk; + auto calcMinAcc = [¶ms](const double planning_jerk) { + if (planning_jerk < params.backward.min_jerk_mild_stop) { + return params.backward.min_acc; + } + return params.backward.min_acc_mild_stop; + }; + const double min_acc = calcMinAcc(planning_jerk); + + if (!analytical_velocity_planning_utils::calcStopVelocityWithConstantJerkAccLimit( + v0, a0, jerk_acc, jerk_dec, min_acc, decel_target_vel, type, times, decel_start_index, + output_trajectory)) { + return false; + } + + return true; +} + +std::string AnalyticalJerkConstrainedSmoother::strTimes(const std::vector & times) const +{ + std::stringstream ss; + unsigned int i = 0; + for (double time : times) { + ss << "time[" << i << "] = " << time << ", "; + i++; + } + return ss.str(); +} + +std::string AnalyticalJerkConstrainedSmoother::strStartIndices( + const std::vector & start_indices) const +{ + std::stringstream ss; + for (size_t i = 0; i < start_indices.size(); ++i) { + if (i != (start_indices.size() - 1)) { + ss << start_indices.at(i) << ", "; + } else { + ss << start_indices.at(i); + } + } + return ss.str(); +} + +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp new file mode 100644 index 0000000000..f9db6277c3 --- /dev/null +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -0,0 +1,350 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware_utils/geometry/geometry.hpp" + +#include +#include + +namespace autoware::velocity_smoother +{ +namespace analytical_velocity_planning_utils +{ +bool calcStopDistWithJerkAndAccConstraints( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double target_vel, int & type, std::vector & times, + double & stop_dist) +{ + const double t_min = + (target_vel - v0 - 0.5 * (0 - a0) / jerk_dec * a0 - 0.5 * min_acc / jerk_dec * min_acc - + 0.5 * (0 - min_acc) / jerk_acc * min_acc) / + min_acc; + + if (t_min > 0) { + double t1 = (min_acc - a0) / jerk_dec; + if (t1 < 0.01) { + t1 = 0; + } + + const double a1 = a0 + jerk_dec * t1; + const double v1 = v0 + a0 * t1 + 0.5 * jerk_dec * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_dec * t1 * t1 * t1; + + double t2 = t_min; + if (t2 < 0.01) { + t2 = 0; + } + + const double a2 = a1; + const double v2 = v1 + a1 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; + + double t3 = (0 - min_acc) / jerk_acc; + if (t3 < 0.01) { + t3 = 0; + } + + const double a3 = a2 + jerk_acc * t3; + const double v3 = v2 + a2 * t3 + 0.5 * jerk_acc * t3 * t3; + const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * jerk_acc * t3 * t3 * t3; + + const double a_target = 0.0; + const double v_margin = 0.3; // [m/s] + const double a_margin = 0.1; // [m/s^2] + if (!validCheckCalcStopDist(v3, a3, target_vel, a_target, v_margin, a_margin)) { + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 1"); + return false; + } + + type = 1; + times.push_back(t1); + times.push_back(t2); + times.push_back(t3); + stop_dist = x3; + } else { + const double is_decel_needed = 0.5 * (0 - a0) / jerk_acc * a0 - (target_vel - v0); + if (is_decel_needed > 0 || a0 > 0) { + const double a1_square = (target_vel - v0 - 0.5 * (0 - a0) / jerk_dec * a0) * + (2 * jerk_acc * jerk_dec / (jerk_acc - jerk_dec)); + const double a1 = -std::sqrt(a1_square); + + double t1 = (a1 - a0) / jerk_dec; + if (t1 < 0.01) { + t1 = 0; + } + + const double v1 = v0 + a0 * t1 + 0.5 * jerk_dec * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_dec * t1 * t1 * t1; + + double t2 = (0 - a1) / jerk_acc; + if (t2 < 0.01) { + t2 = 0; + } + + const double a2 = a1 + jerk_acc * t2; + const double v2 = v1 + a1 * t2 + 0.5 * jerk_acc * t2 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * jerk_acc * t2 * t2 * t2; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + if (!validCheckCalcStopDist(v2, a2, target_vel, a_target, v_margin, a_margin)) { + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 2"); + return false; + } + + type = 2; + times.push_back(t1); + times.push_back(t2); + stop_dist = x2; + } else { + double t1 = (0 - a0) / jerk_acc; + if (t1 < 0) { + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "t1 < 0. unexpected condition."); + return false; + } + if (t1 < 0.01) { + t1 = 0; + } + + const double a1 = a0 + jerk_acc * t1; + const double v1 = v0 + a0 * t1 + 0.5 * jerk_acc * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_acc * t1 * t1 * t1; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + if (!validCheckCalcStopDist(v1, a1, target_vel, a_target, v_margin, a_margin)) { + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 3"); + return false; + } + + type = 3; + times.push_back(t1); + stop_dist = x1; + } + } + return true; +} + +bool validCheckCalcStopDist( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + if (v_end < v_min || v_max < v_end) { + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "Valid check error! v_target = %f, v_end = %f", + v_target, v_end); + return false; + } + if (a_end < a_min || a_max < a_end) { + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "Valid check error! a_target = %f, a_end = %f", + a_target, a_end); + return false; + } + return true; +} + +bool calcStopVelocityWithConstantJerkAccLimit( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double decel_target_vel, const int type, + const std::vector & times, const size_t start_index, TrajectoryPoints & output_trajectory) +{ + const double t_total = std::accumulate(times.begin(), times.end(), 0.0); + std::vector ts, xs, vs, as, js; + const double dt = 0.1; + double x = 0.0; + double v = 0.0; + double a = 0.0; + double j = 0.0; + + for (double t = 0.0; t < t_total; t += dt) { + updateStopVelocityStatus(v0, a0, jerk_acc, jerk_dec, type, times, t, x, v, a, j); + if (v > 0.0) { + a = std::max(a, min_acc); + ts.push_back(t); + xs.push_back(x); + vs.push_back(v); + as.push_back(a); + js.push_back(j); + } + } + updateStopVelocityStatus(v0, a0, jerk_acc, jerk_dec, type, times, t_total, x, v, a, j); + if (v > 0.0 && !xs.empty() && xs.back() < x) { + a = std::max(a, min_acc); + ts.push_back(t_total); + xs.push_back(x); + vs.push_back(v); + as.push_back(a); + js.push_back(j); + } + + // for debug + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity."); + for (unsigned int i = 0; i < ts.size(); ++i) { + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "--- t: %f, x: %f, v: %f, a: %f, j: %f", + ts.at(i), xs.at(i), vs.at(i), as.at(i), js.at(i)); + } + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + if (!validCheckCalcStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) { + return false; + } + + if (xs.empty()) { + for (size_t i = start_index; i < output_trajectory.size(); ++i) { + output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; + output_trajectory.at(i).acceleration_mps2 = 0.0; + } + return true; + } + + double distance = 0.0; + std::vector distances; + distances.push_back(distance); + for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { + distance += + autoware_utils::calc_distance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); + if (distance > xs.back()) { + break; + } + distances.push_back(distance); + } + + if ( + !autoware::interpolation::isIncreasing(xs) || + !autoware::interpolation::isIncreasing(distances) || + !autoware::interpolation::isNotDecreasing(xs) || + !autoware::interpolation::isNotDecreasing(distances)) { + return false; + } + + if ( + xs.size() < 2 || vs.size() < 2 || as.size() < 2 || js.size() < 2 || distances.empty() || + distances.front() < xs.front() || xs.back() < distances.back()) { + return false; + } + + const auto vel_at_wp = autoware::interpolation::lerp(xs, vs, distances); + const auto acc_at_wp = autoware::interpolation::lerp(xs, as, distances); + const auto jerk_at_wp = autoware::interpolation::lerp(xs, js, distances); + + for (size_t i = 0; i < vel_at_wp.size(); ++i) { + output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); + output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp.at(i); + } + for (size_t i = start_index + vel_at_wp.size(); i < output_trajectory.size(); ++i) { + output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; + output_trajectory.at(i).acceleration_mps2 = 0.0; + } + + return true; +} + +void updateStopVelocityStatus( + double v0, double a0, double jerk_acc, double jerk_dec, int type, + const std::vector & times, double t, double & x, double & v, double & a, double & j) +{ + if (type == 1) { + if (0 <= t && t < times.at(0)) { + j = jerk_dec; + a = integ_a(a0, j, t); + v = integ_v(v0, a0, j, t); + x = integ_x(0, v0, a0, j, t); + } else if (times.at(0) <= t && t < times.at(0) + times.at(1)) { + const double t1 = times.at(0); + const double a1 = integ_a(a0, jerk_dec, t1); + const double v1 = integ_v(v0, a0, jerk_dec, t1); + const double x1 = integ_x(0, v0, a0, jerk_dec, t1); + + const double dt = t - t1; + j = 0; + a = integ_a(a1, j, dt); + v = integ_v(v1, a1, j, dt); + x = integ_x(x1, v1, a1, j, dt); + } else if (times.at(0) + times.at(1) <= t && t <= times.at(0) + times.at(1) + times.at(2)) { + const double t1 = times.at(0); + const double a1 = integ_a(a0, jerk_dec, t1); + const double v1 = integ_v(v0, a0, jerk_dec, t1); + const double x1 = integ_x(0, v0, a0, jerk_dec, t1); + + const double t2 = times.at(1); + const double a2 = integ_a(a1, 0, t2); + const double v2 = integ_v(v1, a1, 0, t2); + const double x2 = integ_x(x1, v1, a1, 0, t2); + + const double dt = t - (t1 + t2); + j = jerk_acc; + a = integ_a(a2, j, dt); + v = integ_v(v2, a2, j, dt); + x = integ_x(x2, v2, a2, j, dt); + } + } else if (type == 2) { + if (0 <= t && t < times.at(0)) { + j = jerk_dec; + a = integ_a(a0, j, t); + v = integ_v(v0, a0, j, t); + x = integ_x(0, v0, a0, j, t); + } else if (times.at(0) <= t && t <= times.at(0) + times.at(1)) { + const double t1 = times.at(0); + const double a1 = integ_a(a0, jerk_dec, t1); + const double v1 = integ_v(v0, a0, jerk_dec, t1); + const double x1 = integ_x(0, v0, a0, jerk_dec, t1); + + const double dt = t - t1; + j = jerk_acc; + a = integ_a(a1, j, dt); + v = integ_v(v1, a1, j, dt); + x = integ_x(x1, v1, a1, j, dt); + } + } else if (type == 3) { + if (0 <= t && t <= times.at(0)) { + j = jerk_acc; + a = integ_a(a0, j, t); + v = integ_v(v0, a0, j, t); + x = integ_x(0, v0, a0, j, t); + } + } else { + } +} + +double integ_x(double x0, double v0, double a0, double j0, double t) +{ + return x0 + v0 * t + 0.5 * a0 * t * t + (1.0 / 6.0) * j0 * t * t * t; +} + +double integ_v(double v0, double a0, double j0, double t) +{ + return v0 + a0 * t + 0.5 * j0 * t * t; +} + +double integ_a(double a0, double j0, double t) +{ + return a0 + j0 * t; +} + +} // namespace analytical_velocity_planning_utils +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp new file mode 100644 index 0000000000..c375472d52 --- /dev/null +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -0,0 +1,495 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" + +#include "autoware/qp_interface/proxqp_interface.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#define VERBOSE_TRAJECTORY_VELOCITY false + +namespace autoware::velocity_smoother +{ +JerkFilteredSmoother::JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) +{ + auto & p = smoother_param_; + p.jerk_weight = node.declare_parameter("jerk_weight"); + p.over_v_weight = node.declare_parameter("over_v_weight"); + p.over_a_weight = node.declare_parameter("over_a_weight"); + p.over_j_weight = node.declare_parameter("over_j_weight"); + p.jerk_filter_ds = node.declare_parameter("jerk_filter_ds"); + + qp_interface_ = + std::make_shared(false, 20000, 1.0e-8, 1.0e-6, false); +} + +void JerkFilteredSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const +{ + return smoother_param_; +} + +bool JerkFilteredSmoother::apply( + const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, + std::vector & debug_trajectories, const bool publish_debug_trajs) +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + output = input; + + if (input.empty()) { + RCLCPP_WARN(logger_, "Input TrajectoryPoints to the jerk filtered optimization is empty."); + return false; + } + + if (input.size() == 1) { + // No need to do optimization + output.front().longitudinal_velocity_mps = v0; + output.front().acceleration_mps2 = a0; + debug_trajectories.resize(3); + debug_trajectories[0] = output; + debug_trajectories[1] = output; + debug_trajectories[2] = output; + return true; + } + + const auto ts = std::chrono::system_clock::now(); + + const double a_max = base_param_.max_accel; + const double a_min = base_param_.min_decel; + const double a_stop_accel = 0.0; + const double a_stop_decel = base_param_.stop_decel; + const double j_max = base_param_.max_jerk; + const double j_min = base_param_.min_jerk; + const double over_j_weight = smoother_param_.over_j_weight; + const double over_v_weight = smoother_param_.over_v_weight; + const double over_a_weight = smoother_param_.over_a_weight; + + // jerk filter + const auto forward_filtered = + forwardJerkFilter(v0, std::max(a0, a_min), a_max, a_stop_accel, j_max, input); + const auto backward_filtered = backwardJerkFilter( + input.back().longitudinal_velocity_mps, a_stop_decel, a_min, a_stop_decel, j_min, input); + const auto filtered = + mergeFilteredTrajectory(v0, a0, a_min, j_min, forward_filtered, backward_filtered); + + // Resample TrajectoryPoints for Optimization + // TODO(planning/control team) deal with overlapped lanes with the same direction + const auto initial_traj_pose = filtered.front().pose; + + const auto resample = [&](const auto & trajectory) { + autoware_utils::ScopedTimeTrack st("resample", *time_keeper_); + + return resampling::resampleTrajectory( + trajectory, v0, initial_traj_pose, std::numeric_limits::max(), + std::numeric_limits::max(), base_param_.resample_param); + }; + + auto opt_resampled_trajectory = resample(filtered); + + // Set debug trajectories + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = resample(forward_filtered); + debug_trajectories[1] = resample(backward_filtered); + debug_trajectories[2] = resample(filtered); + } + + // Ensure terminal velocity is zero + opt_resampled_trajectory.back().longitudinal_velocity_mps = 0.0; + + // If Resampled Size is too small, we don't do optimization + if (opt_resampled_trajectory.size() == 1) { + // No need to do optimization + output.front().longitudinal_velocity_mps = v0; + output.front().acceleration_mps2 = a0; + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = output; + debug_trajectories[1] = output; + debug_trajectories[2] = output; + } + return true; + } + + // to avoid getting 0 as a stop point, search zero velocity index from 1. + // the size of the resampled trajectory must not be less than 2. + const auto zero_vel_id = autoware::motion_utils::searchZeroVelocityIndex( + opt_resampled_trajectory, 1, opt_resampled_trajectory.size()); + + if (!zero_vel_id) { + RCLCPP_WARN(logger_, "opt_resampled_trajectory must have stop point."); + return false; + } + + // Clip trajectory from 0 to zero_vel_id (the size becomes zero_vel_id_ + 1) + const size_t N = *zero_vel_id + 1; + + output = opt_resampled_trajectory; + + const std::vector interval_dist_arr = + trajectory_utils::calcTrajectoryIntervalDistance(opt_resampled_trajectory); + + std::vector v_max_arr(N, 0.0); + for (size_t i = 0; i < N; ++i) { + v_max_arr.at(i) = opt_resampled_trajectory.at(i).longitudinal_velocity_mps; + } + + time_keeper_->start_track("initOptimization"); + /* + * x = [ + * b[0], b[1], ..., b[N], : 0~N + * a[0], a[1], .... a[N], : N~2N + * delta[0], ..., delta[N], : 2N~3N + * sigma[0], sigma[1], ...., sigma[N], : 3N~4N + * gamma[0], gamma[1], ..., gamma[N] : 4N~5N + * ] + * + * b[i] : velocity^2 + * delta : 0 < b[i]-delta[i] < max_vel[i]*max_vel[i] + * sigma : a_min < a[i] - sigma[i] < a_max + * gamma : jerk_min < pseudo_jerk[i] * ref_vel[i] - gamma[i] < jerk_max + */ + const uint32_t IDX_B0 = 0; + const uint32_t IDX_A0 = N; + const uint32_t IDX_DELTA0 = 2 * N; + const uint32_t IDX_SIGMA0 = 3 * N; + const uint32_t IDX_GAMMA0 = 4 * N; + + const uint32_t l_variables = 5 * N; + const uint32_t l_constraints = 4 * N + 1; + + // the matrix size depends on constraint numbers. + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(l_constraints, l_variables); + + std::vector lower_bound(l_constraints, 0.0); + std::vector upper_bound(l_constraints, 0.0); + + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables); + std::vector q(l_variables, 0.0); + + /**************************************************************/ + /**************************************************************/ + /**************** design objective function *******************/ + /**************************************************************/ + /**************************************************************/ + + // jerk: d(ai)/ds * v_ref -> minimize weight * ((a1 - a0) / ds * v_ref)^2 * ds + const double smooth_weight = smoother_param_.jerk_weight; + for (size_t i = 0; i < N - 1; ++i) { + const double ref_vel = 0.5 * (v_max_arr.at(i) + v_max_arr.at(i + 1)); + const double interval_dist = std::max(interval_dist_arr.at(i), 0.0001); + const double w_x_ds_inv = (1.0 / interval_dist) * ref_vel; + P(IDX_A0 + i, IDX_A0 + i) += smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; + P(IDX_A0 + i, IDX_A0 + i + 1) -= smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; + P(IDX_A0 + i + 1, IDX_A0 + i) -= smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; + P(IDX_A0 + i + 1, IDX_A0 + i + 1) += smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; + } + + // |v_max_i^2 - b_i|/v_max^2 -> minimize (-bi) * ds / v_max^2 + for (size_t i = 0; i < N; ++i) { + if (v_max_arr.at(i) > 0.01) { + // Note that if v_max[i] is too small, we did not minimize the corresponding -b[i] + double v_weight_term = -1.0 / (v_max_arr.at(i) * v_max_arr.at(i)); + if (i < N - 1) { + v_weight_term *= std::max(interval_dist_arr.at(i), 0.0001); + } + q.at(IDX_B0 + i) += v_weight_term; + } + P(IDX_DELTA0 + i, IDX_DELTA0 + i) += over_v_weight; // over velocity cost + P(IDX_SIGMA0 + i, IDX_SIGMA0 + i) += over_a_weight; // over acceleration cost + P(IDX_GAMMA0 + i, IDX_GAMMA0 + i) += over_j_weight; // over jerk cost + } + + /**************************************************************/ + /**************************************************************/ + /**************** design constraint matrix ********************/ + /**************************************************************/ + /**************************************************************/ + + /* + NOTE: The delta allows b to be negative. This is actually invalid because the definition is b=v^2. + But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of + v=0 & a<0. To avoid the infeasibility, we allow b<0. The negative b is dealt as b=0 when it is + converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small), + b is almost 0, and is not a big problem. + */ + + size_t constr_idx = 0; + + // Soft Constraint Velocity Limit: 0 < b - delta < v_max^2 + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_B0 + i) = 1.0; // b_i + A(constr_idx, IDX_DELTA0 + i) = -1.0; // -delta_i + upper_bound[constr_idx] = v_max_arr.at(i) * v_max_arr.at(i); + lower_bound[constr_idx] = 0.0; + } + + // Soft Constraint Acceleration Limit: a_min < a - sigma < a_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_A0 + i) = 1.0; // a_i + A(constr_idx, IDX_SIGMA0 + i) = -1.0; // -sigma_i + + constexpr double stop_vel = 1e-3; + if (v_max_arr.at(i) < stop_vel) { + // Stop Point + upper_bound[constr_idx] = a_stop_decel; + lower_bound[constr_idx] = a_stop_decel; + } else { + upper_bound[constr_idx] = a_max; + lower_bound[constr_idx] = a_min; + } + } + + // Soft Constraint Jerk Limit: jerk_min < pseudo_jerk[i] * ref_vel[i] - gamma[i] < jerk_max + // -> jerk_min * ds < (a[i+1] - a[i]) * ref_vel[i] - gamma[i] * ds < jerk_max * ds + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double ref_vel = 0.5 * (v_max_arr.at(i) + v_max_arr.at(i + 1)); + const double ds = interval_dist_arr.at(i); + A(constr_idx, IDX_A0 + i) = -ref_vel; // -a[i] * ref_vel + A(constr_idx, IDX_A0 + i + 1) = ref_vel; // a[i+1] * ref_vel + A(constr_idx, IDX_GAMMA0 + i) = -ds; // -gamma[i] * ds + upper_bound[constr_idx] = j_max * ds; // jerk_max * ds + lower_bound[constr_idx] = j_min * ds; // jerk_min * ds + } + + // b' = 2a ... (b(i+1) - b(i)) / ds = 2a(i) + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + A(constr_idx, IDX_B0 + i) = -1.0; // b(i) + A(constr_idx, IDX_B0 + i + 1) = 1.0; // b(i+1) + A(constr_idx, IDX_A0 + i) = -2.0 * interval_dist_arr.at(i); // a(i) * ds + upper_bound[constr_idx] = 0.0; + lower_bound[constr_idx] = 0.0; + } + + // initial condition + { + A(constr_idx, IDX_B0) = 1.0; // b0 + upper_bound[constr_idx] = v0 * v0; + lower_bound[constr_idx] = v0 * v0; + ++constr_idx; + + A(constr_idx, IDX_A0) = 1.0; // a0 + upper_bound[constr_idx] = a0; + lower_bound[constr_idx] = a0; + ++constr_idx; + } + time_keeper_->end_track("initOptimization"); + + // execute optimization + time_keeper_->start_track("optimize"); + const auto optval = qp_interface_->optimize(P, A, q, lower_bound, upper_bound); + time_keeper_->end_track("optimize"); + if (!qp_interface_->isSolved()) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_interface_->getStatus().c_str()); + return false; + } + + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } + + const auto tf1 = std::chrono::system_clock::now(); + const double dt_ms1 = + std::chrono::duration_cast(tf1 - ts).count() * 1.0e-6; + RCLCPP_DEBUG(logger_, "optimization time = %f [ms]", dt_ms1); + + // get velocity & acceleration + for (size_t i = 0; i < N; ++i) { + double b = optval.at(IDX_B0 + i); + output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(b, 0.0)); + output.at(i).acceleration_mps2 = optval.at(IDX_A0 + i); + } + for (size_t i = N; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + output.at(i).acceleration_mps2 = a_stop_decel; + } + + if (VERBOSE_TRAJECTORY_VELOCITY) { + const auto s_output = trajectory_utils::calcArclengthArray(output); + + std::cerr << "\n\n" << std::endl; + for (size_t i = 0; i < N; ++i) { + const auto v_opt = output.at(i).longitudinal_velocity_mps; + const auto a_opt = output.at(i).acceleration_mps2; + const auto ds = i < interval_dist_arr.size() ? interval_dist_arr.at(i) : 0.0; + const auto v_rs = i < opt_resampled_trajectory.size() + ? opt_resampled_trajectory.at(i).longitudinal_velocity_mps + : 0.0; + RCLCPP_INFO( + logger_, "i = %4lu | s: %5f | ds: %5f | rs: %9f | op_v: %10f | op_a: %10f |", i, + s_output.at(i), ds, v_rs, v_opt, a_opt); + } + } + + return true; +} + +TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( + const double v0, const double a0, const double a_max, const double a_start, const double j_max, + const TrajectoryPoints & input) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + auto applyLimits = [&input, &a_start](double & v, double & a, size_t i) { + double v_lim = input.at(i).longitudinal_velocity_mps; + static constexpr double ep = 1.0e-5; + if (v > v_lim + ep) { + v = v_lim; + a = 0.0; + + if (v_lim < 1e-3 && i < input.size() - 1) { + double next_v_lim = input.at(i + 1).longitudinal_velocity_mps; + if (next_v_lim >= 1e-3) { + a = a_start; // start from stop velocity + } + } + } + + if (v < 0.0) { + v = a = 0.0; + } + }; + + auto output = input; + + double current_vel = v0; + double current_acc = a0; + applyLimits(current_vel, current_acc, 0); + + output.front().longitudinal_velocity_mps = current_vel; + output.front().acceleration_mps2 = current_acc; + for (size_t i = 1; i < input.size(); ++i) { + const double ds = autoware_utils::calc_distance2d(input.at(i), input.at(i - 1)); + const double max_dt = std::pow(6.0 * ds / j_max, 1.0 / 3.0); // assuming v0 = a0 = 0. + const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); + + if (current_acc + j_max * dt >= a_max) { + const double tmp_jerk = std::min((a_max - current_acc) / dt, j_max); + current_vel = current_vel + current_acc * dt + 0.5 * tmp_jerk * dt * dt; + current_acc = a_max; + } else { + current_vel = current_vel + current_acc * dt + 0.5 * j_max * dt * dt; + current_acc = current_acc + j_max * dt; + } + applyLimits(current_vel, current_acc, i); + output.at(i).longitudinal_velocity_mps = current_vel; + output.at(i).acceleration_mps2 = current_acc; + } + return output; +} + +TrajectoryPoints JerkFilteredSmoother::backwardJerkFilter( + const double v0, const double a0, const double a_min, const double a_stop, const double j_min, + const TrajectoryPoints & input) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + auto input_rev = input; + std::reverse(input_rev.begin(), input_rev.end()); + auto filtered = forwardJerkFilter( + v0, std::fabs(a0), std::fabs(a_min), std::fabs(a_stop), std::fabs(j_min), input_rev); + std::reverse(filtered.begin(), filtered.end()); + for (size_t i = 0; i < filtered.size(); ++i) { + filtered.at(i).acceleration_mps2 *= -1.0; // Deceleration + } + return filtered; +} + +TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( + const double v0, const double a0, const double a_min, const double j_min, + const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + TrajectoryPoints merged; + merged = forward_filtered; + + auto getVx = [](const TrajectoryPoints & trajectory, int i) { + return trajectory.at(i).longitudinal_velocity_mps; + }; + + size_t i = 0; + + if (getVx(backward_filtered, 0) < v0) { + double current_vel = v0; + double current_acc = a0; + while (getVx(backward_filtered, i) < current_vel && i < merged.size() - 1) { + merged.at(i).longitudinal_velocity_mps = current_vel; + merged.at(i).acceleration_mps2 = current_acc; + + const double ds = + autoware_utils::calc_distance2d(forward_filtered.at(i + 1), forward_filtered.at(i)); + const double max_dt = + std::pow(6.0 * ds / std::fabs(j_min), 1.0 / 3.0); // assuming v0 = a0 = 0. + const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); + + if (current_acc + j_min * dt < a_min) { + const double tmp_jerk = std::max((a_min - current_acc) / dt, j_min); + current_vel = current_vel + current_acc * dt + 0.5 * tmp_jerk * dt * dt; + current_acc = std::max(current_acc + tmp_jerk * dt, a_min); + } else { + current_vel = current_vel + current_acc * dt + 0.5 * j_min * dt * dt; + current_acc = current_acc + j_min * dt; + } + + if (current_vel > getVx(forward_filtered, i)) { + current_vel = getVx(forward_filtered, i); + } + ++i; + } + } + + // take smaller velocity point + for (; i < merged.size(); ++i) { + merged.at(i) = (getVx(forward_filtered, i) < getVx(backward_filtered, i)) + ? forward_filtered.at(i) + : backward_filtered.at(i); + } + return merged; +} + +TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + return resampling::resampleTrajectory( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, + smoother_param_.jerk_filter_ds); +} + +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp new file mode 100644 index 0000000000..66d09c518f --- /dev/null +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -0,0 +1,249 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" + +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ +L2PseudoJerkSmoother::L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) +{ + auto & p = smoother_param_; + p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); + p.over_v_weight = node.declare_parameter("over_v_weight"); + p.over_a_weight = node.declare_parameter("over_a_weight"); + + qp_solver_.updateMaxIter(4000); + qp_solver_.updateRhoInterval(0); // 0 means automatic + qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 + qp_solver_.updateEpsAbs(1.0e-4); // def: 1.0e-4 + qp_solver_.updateVerbose(false); +} + +void L2PseudoJerkSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +L2PseudoJerkSmoother::Param L2PseudoJerkSmoother::getParam() const +{ + return smoother_param_; +} + +bool L2PseudoJerkSmoother::apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) +{ + debug_trajectories.clear(); + + const auto ts = std::chrono::system_clock::now(); + + output = input; + + if (std::fabs(input.front().longitudinal_velocity_mps) < 0.1) { + RCLCPP_DEBUG(logger_, "closest v_max < 0.1. assume vehicle stopped. return."); + return true; + } + + const unsigned int N = input.size(); + + if (N < 2) { + RCLCPP_WARN(logger_, "trajectory length is not enough."); + return false; + } + + const std::vector interval_dist_arr = + trajectory_utils::calcTrajectoryIntervalDistance(input); + + std::vector v_max(N, 0.0); + for (unsigned int i = 0; i < N; ++i) { + v_max.at(i) = input.at(i).longitudinal_velocity_mps; + } + /* + * x = [b0, b1, ..., bN, | a0, a1, ..., aN, | delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., + * sigmaN] in R^{4N} b: velocity^2 a: acceleration delta: 0 < bi < v_max^2 + delta sigma: a_min < + * ai - sigma < a_max + */ + + const uint32_t l_variables = 4 * N; + const uint32_t l_constraints = 3 * N + 1; + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero( + l_constraints, l_variables); // the matrix size depends on constraint numbers. + + std::vector lower_bound(l_constraints, 0.0); + std::vector upper_bound(l_constraints, 0.0); + + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables); + std::vector q(l_variables, 0.0); + + const double a_max = base_param_.max_accel; + const double a_min = base_param_.min_decel; + const double smooth_weight = smoother_param_.pseudo_jerk_weight; + const double over_v_weight = smoother_param_.over_v_weight; + const double over_a_weight = smoother_param_.over_a_weight; + + // design objective function + for (unsigned int i = 0; i < N; ++i) { // bi + q[i] = -1.0; // |v_max^2 - b| -> minimize (-bi) + } + + // pseudo jerk: d(ai)/ds -> minimize weight * (a1 - a0)^2 + for (unsigned int i = N; i < 2 * N - 1; ++i) { + unsigned int j = i - N; + const double w_x_ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001); + P(i, i) += w_x_ds_inv * w_x_ds_inv * smooth_weight; + P(i, i + 1) -= w_x_ds_inv * w_x_ds_inv * smooth_weight; + P(i + 1, i) -= w_x_ds_inv * w_x_ds_inv * smooth_weight; + P(i + 1, i + 1) += w_x_ds_inv * w_x_ds_inv * smooth_weight; + } + + for (unsigned int i = 2 * N; i < 3 * N; ++i) { // over velocity cost + P(i, i) += over_v_weight; + } + + for (unsigned int i = 3 * N; i < 4 * N; ++i) { // over acceleration cost + P(i, i) += over_a_weight; + } + + /* design constraint matrix + 0 < b - delta < v_max^2 + NOTE: The delta allows b to be negative. This is actually invalid because the definition is b=v^2. + But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of + v=0 & a<0. To avoid the infeasibility, we allow b<0. The negative b is dealt as b=0 when it is + converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small), + b is almost 0, and is not a big problem. + */ + for (unsigned int i = 0; i < N; ++i) { + const int j = 2 * N + i; + A(i, i) = 1.0; // b_i + A(i, j) = -1.0; // -delta_i + upper_bound[i] = v_max[i] * v_max[i]; + lower_bound[i] = 0.0; + } + + // a_min < a - sigma < a_max + for (unsigned int i = N; i < 2 * N; ++i) { + const int j = 2 * N + i; + A(i, i) = 1.0; // a_i + A(i, j) = -1.0; // -sigma_i + if (i != N && v_max[i - N] < std::numeric_limits::epsilon()) { + upper_bound[i] = 0.0; + lower_bound[i] = 0.0; + } else { + upper_bound[i] = a_max; + lower_bound[i] = a_min; + } + } + + // b' = 2a ... (b(i+1) - b(i)) / ds = 2a(i) + for (unsigned int i = 2 * N; i < 3 * N - 1; ++i) { + const unsigned int j = i - 2 * N; + const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001); + A(i, j) = -ds_inv; // b(i) + A(i, j + 1) = ds_inv; // b(i+1) + A(i, j + N) = -2.0; // a(i) + upper_bound[i] = 0.0; + lower_bound[i] = 0.0; + } + + // initial condition + const double v0 = initial_vel; + { + const unsigned int i = 3 * N - 1; + A(i, 0) = 1.0; // b0 + upper_bound[i] = v0 * v0; + lower_bound[i] = v0 * v0; + + A(i + 1, N) = 1.0; // a0 + upper_bound[i + 1] = initial_acc; + lower_bound[i + 1] = initial_acc; + } + + const auto tf1 = std::chrono::system_clock::now(); + const double dt_ms1 = + std::chrono::duration_cast(tf1 - ts).count() * 1.0e-6; + + // execute optimization + const auto ts2 = std::chrono::system_clock::now(); + const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + + // [b0, b1, ..., bN, | a0, a1, ..., aN, | + // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] + const std::vector optval = result.primal_solution; + const int status_val = result.solution_status; + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } + + for (unsigned int i = 0; i < N; ++i) { + double v = optval.at(i); + // std::cout << "[smoother] v[" << i << "] : " << std::sqrt(std::max(v, 0.0)) << + // ", v_max[" << i << "] : " << v_max[i] << std::endl; + output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(v, 0.0)); + output.at(i).acceleration_mps2 = optval.at(i + N); + } + for (unsigned int i = N; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + output.at(i).acceleration_mps2 = 0.0; + } + + // -- to check the all optimization variables -- + // ROS_DEBUG("[after optimize Linf] idx, vel, acc, over_vel, over_acc "); + // for (unsigned int i = 0; i < N; ++i) { + // ROS_DEBUG( + // "i = %d, v: %f, v_max: %f a: %f, b: %f, delta: %f, sigma: %f\n", + // i, std::sqrt(optval.at(i)), + // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); + // } + + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); + + const auto tf2 = std::chrono::system_clock::now(); + const double dt_ms2 = + std::chrono::duration_cast(tf2 - ts2).count() * 1.0e-6; + RCLCPP_DEBUG(logger_, "init time = %f [ms], optimization time = %f [ms]", dt_ms1, dt_ms2); + + return true; +} + +TrajectoryPoints L2PseudoJerkSmoother::resampleTrajectory( + const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const +{ + return resampling::resampleTrajectory( + input, v0, current_pose, nearest_dist_threshold, nearest_yaw_threshold, + base_param_.resample_param); +} + +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp new file mode 100644 index 0000000000..6b21a59455 --- /dev/null +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -0,0 +1,261 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" + +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ +LinfPseudoJerkSmoother::LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) +{ + auto & p = smoother_param_; + p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); + p.over_v_weight = node.declare_parameter("over_v_weight"); + p.over_a_weight = node.declare_parameter("over_a_weight"); + + qp_solver_.updateMaxIter(20000); + qp_solver_.updateRhoInterval(5000); + qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 + qp_solver_.updateEpsAbs(1.0e-8); // def: 1.0e-4 + qp_solver_.updateVerbose(false); +} + +void LinfPseudoJerkSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +LinfPseudoJerkSmoother::Param LinfPseudoJerkSmoother::getParam() const +{ + return smoother_param_; +} + +bool LinfPseudoJerkSmoother::apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) +{ + debug_trajectories.clear(); + + const auto ts = std::chrono::system_clock::now(); + + output = input; + + if (std::fabs(input.front().longitudinal_velocity_mps) < 0.1) { + RCLCPP_DEBUG( + logger_, + "closest v_max < 0.1, keep stopping. " + "return."); + return false; + } + + const size_t N{input.size()}; + + if (N < 2) { + return false; + } + + std::vector interval_dist_arr = trajectory_utils::calcTrajectoryIntervalDistance(input); + + std::vector v_max(N, 0.0); + for (size_t i = 0; i < N; ++i) { + v_max.at(i) = input.at(i).longitudinal_velocity_mps; + } + + /* + * x = [b0, b1, ..., bN, | a0, a1, ..., aN, | delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., + * sigmaN, | psi] in R^{4N+1} b: velocity^2 a: acceleration delta: 0 < bi < v_max^2 + delta sigma: + * a_min < ai - sigma < a_max psi: a'*curr_v - psi < 0, - a'*curr_v - psi < 0 (<=> |a'|*curr_v < + * psi) + */ + const size_t l_variables{4 * N + 1}; + const size_t l_constraints{3 * N + 1 + 2 * (N - 1)}; + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero( + l_constraints, l_variables); // the matrix size depends on constraint numbers. + + std::vector lower_bound(l_constraints, 0.0); + std::vector upper_bound(l_constraints, 0.0); + + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables); + std::vector q(l_variables, 0.0); + + const double a_max{base_param_.max_accel}; + const double a_min{base_param_.min_decel}; + const double smooth_weight{smoother_param_.pseudo_jerk_weight}; + const double over_v_weight{smoother_param_.over_v_weight}; + const double over_a_weight{smoother_param_.over_a_weight}; + + // design objective function + for (unsigned int i = 0; i < N; ++i) { // bi + q[i] = -1.0; // |v_max^2 - b| -> minimize (-bi) + } + + for (unsigned int i = 2 * N; i < 3 * N; ++i) { // over velocity cost + P(i, i) += over_v_weight; + } + + for (unsigned int i = 3 * N; i < 4 * N; ++i) { // over acceleration cost + P(i, i) += over_a_weight; + } + + // pseudo jerk (Linf): minimize psi, subject to |a'|*curr_v < psi + q[4 * N] = smooth_weight; + + /* design constraint matrix + 0 < b - delta < v_max^2 + NOTE: The delta allows b to be negative. This is actually invalid because the definition is b=v^2. + But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of + v=0 & a<0. To avoid the infeasibility, we allow b<0. The negative b is dealt as b=0 when it is + converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small), + b is almost 0, and is not a big problem. + */ + for (unsigned int i = 0; i < N; ++i) { + const int j = 2 * N + i; + A(i, i) = 1.0; // b_i + A(i, j) = -1.0; // -delta_i + upper_bound[i] = v_max[i] * v_max[i]; + lower_bound[i] = 0.0; + } + + // a_min < a - sigma < a_max + for (unsigned int i = N; i < 2 * N; ++i) { + const int j = 2 * N + i; + A(i, i) = 1.0; // a_i + A(i, j) = -1.0; // -sigma_i + if (i != N && v_max[i - N] < std::numeric_limits::epsilon()) { + upper_bound[i] = 0.0; + lower_bound[i] = 0.0; + } else { + upper_bound[i] = a_max; + lower_bound[i] = a_min; + } + } + + // b' = 2a + for (unsigned int i = 2 * N; i < 3 * N - 1; ++i) { + const unsigned int j = i - 2 * N; + const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001); + A(i, j) = -ds_inv; + A(i, j + 1) = ds_inv; + A(i, j + N) = -2.0; + upper_bound[i] = 0.0; + lower_bound[i] = 0.0; + } + + // initial condition + const double v0 = initial_vel; + { + const unsigned int i = 3 * N - 1; + A(i, 0) = 1.0; // b0 + upper_bound[i] = v0 * v0; + lower_bound[i] = v0 * v0; + + A(i + 1, N) = 1.0; // a0 + upper_bound[i + 1] = initial_acc; + lower_bound[i + 1] = initial_acc; + } + + // constraint for slack variable (a[i+1] - a[i] <= psi[i], a[i] - a[i+1] <= psi[i]) + for (unsigned int i = 3 * N + 1; i < 4 * N; ++i) { + const unsigned int ia = i - (3 * N + 1) + N; + const unsigned int ip = 4 * N; + const unsigned int j = i - (3 * N + 1); + const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001); + + A(i, ia) = -ds_inv; + A(i, ia + 1) = ds_inv; + A(i, ip) = -1; + lower_bound[i] = -OSQP_INFTY; + upper_bound[i] = 0; + + A(i + N - 1, ia) = ds_inv; + A(i + N - 1, ia + 1) = -ds_inv; + A(i + N - 1, ip) = -1; + lower_bound[i + N - 1] = -OSQP_INFTY; + upper_bound[i + N - 1] = 0; + } + + const auto tf1 = std::chrono::system_clock::now(); + const double dt_ms1 = + std::chrono::duration_cast(tf1 - ts).count() * 1.0e-6; + + // execute optimization + const auto ts2 = std::chrono::system_clock::now(); + const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + + // [b0, b1, ..., bN, | a0, a1, ..., aN, | + // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] + const std::vector optval = result.primal_solution; + const int status_val = result.solution_status; + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } + + /* get velocity & acceleration */ + for (unsigned int i = 0; i < N; ++i) { + double v = optval.at(i); + output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(v, 0.0)); + output.at(i).acceleration_mps2 = optval.at(i + N); + } + for (unsigned int i = N; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + output.at(i).acceleration_mps2 = 0.0; + } + + // -- to check the all optimization variables -- + // ROS_DEBUG("[after optimize Linf] idx, vel, acc, over_vel, over_acc "); + // for (unsigned int i = 0; i < N; ++i) { + // ROS_DEBUG( + // "i = %d, v: %f, v_max: %f a: %f, b: %f, delta: %f, sigma: %f", i, std::sqrt(optval.at(i)), + // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); + // } + + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); + + const auto tf2 = std::chrono::system_clock::now(); + const double dt_ms2 = + std::chrono::duration_cast(tf2 - ts2).count() * 1.0e-6; + RCLCPP_DEBUG(logger_, "init time = %f [ms], optimization time = %f [ms]", dt_ms1, dt_ms2); + return true; +} + +TrajectoryPoints LinfPseudoJerkSmoother::resampleTrajectory( + const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const +{ + return resampling::resampleTrajectory( + input, v0, current_pose, nearest_dist_threshold, nearest_yaw_threshold, + base_param_.resample_param); +} + +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp new file mode 100644 index 0000000000..5dcd530c79 --- /dev/null +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -0,0 +1,291 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/velocity_smoother/resample.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" + +#include +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ + +namespace +{ +TrajectoryPoints applyPreProcess( + const TrajectoryPoints & input, const double interval, const bool use_resampling) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::convertToTrajectory; + using autoware::motion_utils::convertToTrajectoryPointArray; + using autoware::motion_utils::resampleTrajectory; + + if (!use_resampling) { + return input; + } + + TrajectoryPoints output; + std::vector arc_length; + + // since the resampling takes a long time, omit the resampling when it is not requested + const auto traj_length = calcArcLength(input); + for (double s = 0; s < traj_length; s += interval) { + arc_length.push_back(s); + } + + const auto points = resampleTrajectory(convertToTrajectory(input), arc_length); + output = convertToTrajectoryPointArray(points); + output.back() = input.back(); // keep the final speed. + + return output; +} +} // namespace + +SmootherBase::SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: time_keeper_(time_keeper) +{ + auto & p = base_param_; + p.max_accel = node.declare_parameter("normal.max_acc"); + p.min_decel = node.declare_parameter("normal.min_acc"); + p.stop_decel = node.declare_parameter("stop_decel"); + p.max_jerk = node.declare_parameter("normal.max_jerk"); + p.min_jerk = node.declare_parameter("normal.min_jerk"); + p.max_lateral_accel = node.declare_parameter("max_lateral_accel"); + p.min_decel_for_lateral_acc_lim_filter = + node.declare_parameter("min_decel_for_lateral_acc_lim_filter"); + p.sample_ds = node.declare_parameter("resample_ds"); + p.curvature_threshold = node.declare_parameter("curvature_threshold"); + p.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate"); + p.curvature_calculation_distance = + node.declare_parameter("curvature_calculation_distance"); + p.decel_distance_before_curve = node.declare_parameter("decel_distance_before_curve"); + p.decel_distance_after_curve = node.declare_parameter("decel_distance_after_curve"); + p.min_curve_velocity = node.declare_parameter("min_curve_velocity"); + p.resample_param.max_trajectory_length = node.declare_parameter("max_trajectory_length"); + p.resample_param.min_trajectory_length = node.declare_parameter("min_trajectory_length"); + p.resample_param.resample_time = node.declare_parameter("resample_time"); + p.resample_param.dense_resample_dt = node.declare_parameter("dense_resample_dt"); + p.resample_param.dense_min_interval_distance = + node.declare_parameter("dense_min_interval_distance"); + p.resample_param.sparse_resample_dt = node.declare_parameter("sparse_resample_dt"); + p.resample_param.sparse_min_interval_distance = + node.declare_parameter("sparse_min_interval_distance"); +} + +void SmootherBase::setWheelBase(const double wheel_base) +{ + base_param_.wheel_base = wheel_base; +} + +void SmootherBase::setMaxAccel(const double max_acceleration) +{ + base_param_.max_accel = max_acceleration; +} + +void SmootherBase::setMaxJerk(const double max_jerk) +{ + base_param_.max_jerk = max_jerk; +} + +void SmootherBase::setParam(const BaseParam & param) +{ + base_param_ = param; +} + +SmootherBase::BaseParam SmootherBase::getBaseParam() const +{ + return base_param_; +} + +double SmootherBase::getMaxAccel() const +{ + return base_param_.max_accel; +} + +double SmootherBase::getMinDecel() const +{ + return base_param_.min_decel; +} + +double SmootherBase::getMaxJerk() const +{ + return base_param_.max_jerk; +} + +double SmootherBase::getMinJerk() const +{ + return base_param_.min_jerk; +} + +TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling, const double input_points_interval) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (input.size() < 3) { + return input; // cannot calculate lateral acc. do nothing. + } + + // Interpolate with constant interval distance for lateral acceleration calculation. + TrajectoryPoints output; + const double points_interval = + use_resampling ? base_param_.sample_ds : input_points_interval; // [m] + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const auto traj_length = autoware::motion_utils::calcArcLength(input); + for (double s = 0; s < traj_length; s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength); + output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; + } + + const size_t idx_dist = static_cast( + std::max(static_cast((base_param_.curvature_calculation_distance) / points_interval), 1)); + + // Calculate curvature assuming the trajectory points interval is constant + const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); + + // Decrease speed according to lateral G + const size_t before_decel_index = + static_cast(std::round(base_param_.decel_distance_before_curve / points_interval)); + const size_t after_decel_index = + static_cast(std::round(base_param_.decel_distance_after_curve / points_interval)); + const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel); + + const auto latacc_min_vel_arr = + enable_smooth_limit ? trajectory_utils::calcVelocityProfileWithConstantJerkAndAccelerationLimit( + output, v0, a0, base_param_.min_jerk, base_param_.max_accel, + base_param_.min_decel_for_lateral_acc_lim_filter) + : std::vector{}; + + for (size_t i = 0; i < output.size(); ++i) { + double curvature = 0.0; + const size_t start = i > after_decel_index ? i - after_decel_index : 0; + const size_t end = std::min(output.size(), i + before_decel_index + 1); + for (size_t j = start; j < end; ++j) { + if (j >= curvature_v.size()) return output; + curvature = std::max(curvature, std::fabs(curvature_v.at(j))); + } + double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5)); + v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity); + + if (enable_smooth_limit) { + if (i >= latacc_min_vel_arr.size()) return output; + v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i)); + } + if (output.at(i).longitudinal_velocity_mps > v_curvature_max) { + output.at(i).longitudinal_velocity_mps = v_curvature_max; + } + } + return output; +} + +TrajectoryPoints SmootherBase::applySteeringRateLimit( + const TrajectoryPoints & input, const bool use_resampling, + const double input_points_interval) const +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (input.size() < 3) { + return input; // cannot calculate the desired velocity. do nothing. + } + + // Interpolate with constant interval distance for lateral acceleration calculation. + const double points_interval = use_resampling ? base_param_.sample_ds : input_points_interval; + + auto output = applyPreProcess(input, points_interval, use_resampling); + + const size_t idx_dist = static_cast( + std::max(static_cast((base_param_.curvature_calculation_distance) / points_interval), 1)); + + // Step1. Calculate curvature assuming the trajectory points interval is constant. + const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); + + // Step2. Calculate steer rate for each trajectory point. + std::vector steer_rate_arr(output.size(), 0.0); + for (size_t i = 0; i < output.size() - 1; i++) { + // velocity + const auto & v_front = output.at(i + 1).longitudinal_velocity_mps; + const auto & v_back = output.at(i).longitudinal_velocity_mps; + // steer + auto & steer_front = output.at(i + 1).front_wheel_angle_rad; + auto & steer_back = output.at(i).front_wheel_angle_rad; + + // calculate the just 2 steering angle + steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i + 1)); + steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i)); + + const auto mean_vel = 0.5 * (v_front + v_back); + const auto dt = std::max(points_interval / mean_vel, std::numeric_limits::epsilon()); + const auto steering_diff = std::fabs(steer_front - steer_back); + + steer_rate_arr.at(i) = steering_diff / dt; + } + + steer_rate_arr.back() = steer_rate_arr.at((output.size() - 2)); + + // Step3. Remove noise by mean filter. + for (size_t i = 1; i < steer_rate_arr.size() - 1; i++) { + steer_rate_arr.at(i) = + (steer_rate_arr.at(i - 1) + steer_rate_arr.at(i) + steer_rate_arr.at(i + 1)) / 3.0; + } + + // Step4. Limit velocity by steer rate. + for (size_t i = 0; i < output.size() - 1; i++) { + if (fabs(curvature_v.at(i)) < base_param_.curvature_threshold) { + continue; + } + + const auto steer_rate = steer_rate_arr.at(i); + if (steer_rate < autoware_utils::deg2rad(base_param_.max_steering_angle_rate)) { + continue; + } + + const auto mean_vel = + (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; + const auto target_mean_vel = + mean_vel * (autoware_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); + + for (size_t k = 0; k < 2; k++) { + auto & velocity = output.at(i + k).longitudinal_velocity_mps; + const float target_velocity = std::max( + base_param_.min_curve_velocity, + std::min(target_mean_vel, velocity * (target_mean_vel / mean_vel))); + velocity = std::min(velocity, target_velocity); + } + } + + return output; +} + +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp new file mode 100644 index 0000000000..e3748ffcea --- /dev/null +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -0,0 +1,475 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware_utils/geometry/geometry.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ +using geometry_msgs::msg::Point; +namespace trajectory_utils +{ +inline void convertEulerAngleToMonotonic(std::vector & a) +{ + for (unsigned int i = 1; i < a.size(); ++i) { + const double da = a[i] - a[i - 1]; + a[i] = a[i - 1] + autoware_utils::normalize_radian(da); + } +} + +inline tf2::Vector3 getTransVector3(const Pose & from, const Pose & to) +{ + double dx = to.position.x - from.position.x; + double dy = to.position.y - from.position.y; + double dz = to.position.z - from.position.z; + return tf2::Vector3(dx, dy, dz); +} + +inline double integ_x(double x0, double v0, double a0, double j0, double t) +{ + return x0 + v0 * t + 0.5 * a0 * t * t + (1.0 / 6.0) * j0 * t * t * t; +} + +inline double integ_v(double v0, double a0, double j0, double t) +{ + return v0 + a0 * t + 0.5 * j0 * t * t; +} + +inline double integ_a(double a0, double j0, double t) +{ + return a0 + j0 * t; +} + +TrajectoryPoint calcInterpolatedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx) +{ + TrajectoryPoint traj_p{}; + traj_p.pose = target_pose; + + if (trajectory.empty()) { + traj_p.longitudinal_velocity_mps = 0.0; + traj_p.acceleration_mps2 = 0.0; + return traj_p; + } + + if (trajectory.size() == 1) { + traj_p.longitudinal_velocity_mps = trajectory.at(0).longitudinal_velocity_mps; + traj_p.acceleration_mps2 = trajectory.at(0).acceleration_mps2; + return traj_p; + } + + auto v1 = getTransVector3(trajectory.at(seg_idx).pose, trajectory.at(seg_idx + 1).pose); + auto v2 = getTransVector3(trajectory.at(seg_idx).pose, target_pose); + // calc internal proportion + const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))}; + + { + const auto & seg_pt = trajectory.at(seg_idx); + const auto & next_pt = trajectory.at(seg_idx + 1); + traj_p.pose = autoware_utils::calc_interpolated_pose(seg_pt.pose, next_pt.pose, prop); + traj_p.longitudinal_velocity_mps = autoware::interpolation::lerp( + seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); + traj_p.acceleration_mps2 = + autoware::interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); + } + + return traj_p; +} + +TrajectoryPoints extractPathAroundIndex( + const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length, + const double & behind_length) +{ + if (trajectory.size() == 0 || trajectory.size() - 1 < index) { + return {}; + } + + // calc ahead distance + size_t ahead_index{trajectory.size() - 1}; + { + double dist_sum = 0.0; + for (size_t i = index; i < trajectory.size() - 1; ++i) { + dist_sum += autoware_utils::calc_distance2d(trajectory.at(i), trajectory.at(i + 1)); + if (dist_sum > ahead_length) { + ahead_index = i + 1; + break; + } + } + } + + // calc behind distance + size_t behind_index{0}; + { + double dist_sum{0.0}; + for (size_t i = index; i != 0; --i) { + dist_sum += autoware_utils::calc_distance2d(trajectory.at(i), trajectory[i - 1]); + if (dist_sum > behind_length) { + behind_index = i - 1; + break; + } + } + } + + // extract trajectory + TrajectoryPoints extracted_traj{}; + for (size_t i = behind_index; i < ahead_index + 1; ++i) { + extracted_traj.push_back(trajectory.at(i)); + } + + return extracted_traj; +} + +std::vector calcArclengthArray(const TrajectoryPoints & trajectory) +{ + if (trajectory.empty()) { + return {}; + } + + std::vector arclength(trajectory.size()); + double dist = 0.0; + arclength.front() = dist; + for (unsigned int i = 1; i < trajectory.size(); ++i) { + const TrajectoryPoint tp = trajectory.at(i); + const TrajectoryPoint tp_prev = trajectory.at(i - 1); + dist += autoware_utils::calc_distance2d(tp.pose, tp_prev.pose); + arclength.at(i) = dist; + } + return arclength; +} + +std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory) +{ + std::vector intervals; + for (unsigned int i = 1; i < trajectory.size(); ++i) { + const TrajectoryPoint tp = trajectory.at(i); + const TrajectoryPoint tp_prev = trajectory.at(i - 1); + const double dist = autoware_utils::calc_distance2d(tp.pose, tp_prev.pose); + intervals.push_back(dist); + } + return intervals; +} + +std::vector calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, size_t idx_dist) +{ + using autoware_utils::calc_curvature; + using autoware_utils::get_point; + + if (trajectory.size() < 3) { + const std::vector k_arr(trajectory.size(), 0.0); + return k_arr; + } + + // if the idx size is not enough, change the idx_dist + const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); + idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); + + if (idx_dist < 1) { + throw std::logic_error("idx_dist less than 1 is not expected"); + } + + // calculate curvature by circle fitting from three points + std::vector k_arr(trajectory.size(), 0.0); + + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + double curvature = 0.0; + const auto p0 = get_point(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = get_point(trajectory.at(i)); + const auto p2 = get_point(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + try { + curvature = calc_curvature(p0, p1, p2); + } catch (std::exception const & e) { + // ...code that handles the error... + RCLCPP_WARN( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "%s", + e.what()); + if (i > 1) { + curvature = k_arr.at(i - 1); // previous curvature + } else { + curvature = 0.0; + } + } + k_arr.at(i) = curvature; + } + // copy curvatures for the last and first points; + k_arr.at(0) = k_arr.at(1); + k_arr.back() = k_arr.at((trajectory.size() - 2)); + + return k_arr; +} + +void applyMaximumVelocityLimit( + const size_t begin, const size_t end, const double max_vel, TrajectoryPoints & trajectory) +{ + for (size_t idx = begin; idx < end; ++idx) { + if (trajectory.at(idx).longitudinal_velocity_mps > max_vel) { + trajectory.at(idx).longitudinal_velocity_mps = max_vel; + } + } +} + +bool calcStopDistWithJerkConstraints( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double target_vel, std::map & jerk_profile, + double & stop_dist) +{ + auto calculateTime = [](const double a0, const double target_acc, const double jerk) { + return (target_acc - a0) / jerk; + }; + enum class AccelerationType { TRAPEZOID = 0, TRIANGLE = 1, LINEAR = 2 } type; + + jerk_profile.clear(); + + constexpr double t_threshold = 1e-06; // Threshold for truncating value to 0 + constexpr double a_target = 0.0; // [m/s^2] + + // Calculate time interval with constant acceleration (min_acc) + double t_min; + { + const double v_diff_jerk_dec = 0.5 * calculateTime(a0, 0.0, jerk_dec) * a0 + + 0.5 * calculateTime(0.0, min_acc, jerk_dec) * min_acc; + const double v_diff_jerk_acc = 0.5 * calculateTime(min_acc, 0.0, jerk_acc) * min_acc; + const double v_error = target_vel - v0; + // From v_error = v_diff_jerk_dec + min_acc * t_min + v_diff_jerk_acc + t_min = (v_error - v_diff_jerk_dec - v_diff_jerk_acc) / min_acc; + } + + if (t_min > 0) { + // Necessary to increase deceleration to min_acc + type = AccelerationType::TRAPEZOID; + } else { + const double v_error = target_vel - v0; + const double v_diff_decel = 0.5 * a0 * calculateTime(a0, 0.0, jerk_acc); + if (v_diff_decel > v_error || a0 > 0.0) { + // Necessary to increase deceleration case + type = AccelerationType::TRIANGLE; + } else { + type = AccelerationType::LINEAR; + } + } + + double x, v, a, j; + + switch (type) { + case AccelerationType::TRAPEZOID: { + // Calculate time + double t1 = calculateTime(a0, min_acc, jerk_dec); + double t2 = t_min; + double t3 = calculateTime(min_acc, 0.0, jerk_acc); + if (t1 < t_threshold) { + t1 = 0; + } + if (t2 < t_threshold) { + t2 = 0; + } + if (t3 < t_threshold) { + t3 = 0; + } + + // Set jerk profile + jerk_profile.insert(std::make_pair(jerk_dec, t1)); + jerk_profile.insert(std::make_pair(0.0, t2)); + jerk_profile.insert(std::make_pair(jerk_acc, t3)); + + // Calculate state = (x, v, a, j) at t = t1 + t2 + t3 + const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1 + t2 + t3); + if (!state) { + return false; + } + std::tie(x, v, a, j) = *state; + break; + } + + case AccelerationType::TRIANGLE: { + /* + v_error = v_diff_from_a0_to_zero + v_diff_from_zero_to_a1 + v_diff_from_a1_to_zero, + where v_diff_from_zero_to_a1 = 1/2 * ((a1 - 0.0) / jerk_dec) * a1, v_diff_from_a1_to_zero + = 1/2 * ((0.0 - a1) / jerk_acc) * a1. Thus a1^2 = (v_error - v_diff_from_a0_to_zero) * 2 + * 1.0 / (1.0 / jerk_dec - 1.0 / jerk_acc). + */ + const double v_error = target_vel - v0; + const double v_diff_from_a0_to_zero = 0.5 * calculateTime(a0, 0.0, jerk_dec) * a0; + const double a1_square = + (v_error - v_diff_from_a0_to_zero) * 2 * 1.0 / (1.0 / jerk_dec - 1.0 / jerk_acc); + const double a1 = -std::sqrt(a1_square); + + // Calculate time + double t1 = calculateTime(a0, a1, jerk_dec); + double t2 = calculateTime(a1, 0.0, jerk_acc); + if (t1 < t_threshold) { + t1 = 0; + } + if (t2 < t_threshold) { + t2 = 0; + } + + // Set jerk profile + jerk_profile.insert(std::make_pair(jerk_dec, t1)); + jerk_profile.insert(std::make_pair(jerk_acc, t2)); + + // Calculate state = (x, v, a, j) at t = t1 + t2 + const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1 + t2); + if (!state) { + return false; + } + std::tie(x, v, a, j) = *state; + break; + } + + case AccelerationType::LINEAR: { + // Calculate time + const double jerk = (0.0 - a0 * a0) / (2 * (target_vel - v0)); + double t1 = calculateTime(a0, 0.0, jerk); + if (t1 < 0) { + RCLCPP_WARN( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "t1 < 0. unexpected condition."); + return false; + } else if (t1 < t_threshold) { + t1 = 0; + } + + // Set jerk profile + jerk_profile.insert(std::make_pair(jerk, t1)); + + // Calculate state = (x, v, a, j) at t = t1 + t2 + const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1); + if (!state) { + return false; + } + std::tie(x, v, a, j) = *state; + break; + } + } + + constexpr double v_margin = 0.3; // [m/s] + constexpr double a_margin = 0.1; // [m/s^2] + stop_dist = x; + if (!isValidStopDist(v, a, target_vel, a_target, v_margin, a_margin)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "valid error. type = " << static_cast(type)); + return false; + } + return true; +} + +bool isValidStopDist( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + if (v_end < v_min || v_max < v_end) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "valid check error! v_target = " << v_target << ", v_end = " << v_end); + return false; + } + if (a_end < a_min || a_max < a_end) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "valid check error! a_target = " << a_target << ", a_end = " << a_end); + return false; + } + return true; +} + +std::optional> updateStateWithJerkConstraint( + const double v0, const double a0, const std::map & jerk_profile, const double t) +{ + // constexpr double eps = 1.0E-05; + double a{a0}; + double v{v0}; + double x{0.0}; + double t_sum{0.0}; + + for (const auto & it : jerk_profile) { + double j = it.first; + t_sum += it.second; + if (t > t_sum) { + x = integ_x(x, v, a, j, it.second); + v = integ_v(v, a, j, it.second); + a = integ_a(a, j, it.second); + } else { + const double dt = t - (t_sum - it.second); + x = integ_x(x, v, a, j, dt); + v = integ_v(v, a, j, dt); + a = integ_a(a, j, dt); + return std::make_tuple(x, v, a, j); + } + } + + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "Invalid jerk profile"); + return std::nullopt; +} + +std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( + const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk, + const double acc_max, const double acc_min) +{ + if (trajectory.empty()) return {}; + + std::vector velocities(trajectory.size()); + velocities.at(0) = v0; + auto curr_v = v0; + auto curr_a = a0; + + const auto intervals = calcTrajectoryIntervalDistance(trajectory); + + if (intervals.size() + 1 != trajectory.size()) { + throw std::logic_error("interval calculation result has unexpected array size."); + } + + for (size_t i = 0; i < intervals.size(); ++i) { + const auto t = intervals.at(i) / std::max(velocities.at(i), 1.0e-5); + curr_v = integ_v(curr_v, curr_a, jerk, t); + velocities.at(i + 1) = curr_v; + curr_a = std::clamp(integ_a(curr_a, jerk, t), acc_min, acc_max); + } + + return velocities; +} + +double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest) +{ + const auto idx = autoware::motion_utils::searchZeroVelocityIndex(trajectory); + + if (!idx) { + return std::numeric_limits::max(); // stop point is located far away + } + + // TODO(Horibe): use arc length distance + const double stop_dist = + autoware_utils::calc_distance2d(trajectory.at(*idx), trajectory.at(closest)); + + return stop_dist; +} + +} // namespace trajectory_utils +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp new file mode 100644 index 0000000000..5daf332fb0 --- /dev/null +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -0,0 +1,63 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include + +#include + +using autoware::velocity_smoother::trajectory_utils::TrajectoryPoints; +using autoware_planning_msgs::msg::TrajectoryPoint; + +TrajectoryPoints genStraightTrajectory(const size_t size) +{ + double x = 0.0; + double dx = 1.0; + geometry_msgs::msg::Quaternion o; + o.x = 0.0; + o.y = 0.0; + o.z = 0.0; + o.w = 1.0; + + TrajectoryPoints tps; + TrajectoryPoint p; + for (size_t i = 0; i < size; ++i) { + p.pose.position.x = x; + p.pose.orientation = o; + x += dx; + tps.push_back(p); + } + return tps; +} + +TEST(TestTrajectoryUtils, CalcTrajectoryCurvatureFrom3Points) +{ + // the output curvature vector should have the same size of the input trajectory. + const auto checkOutputSize = [](const size_t trajectory_size, const size_t idx_dist) { + const auto trajectory_points = genStraightTrajectory(trajectory_size); + const auto curvatures = + autoware::velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( + trajectory_points, idx_dist); + EXPECT_EQ(curvatures.size(), trajectory_size) << ", idx_dist = " << idx_dist; + }; + + const auto trajectory_size_arr = {0, 1, 2, 3, 4, 5, 10, 100}; + const auto idx_dist_arr = {0, 1, 2, 3, 4, 5, 10, 100}; + for (const auto trajectory_size : trajectory_size_arr) { + for (const auto idx_dist : idx_dist_arr) { + checkOutputSize(trajectory_size, idx_dist); + } + } +} diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp new file mode 100644 index 0000000000..eb2cfc3a8b --- /dev/null +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -0,0 +1,115 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/node.hpp" + +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::planning_test_manager::PlanningInterfaceTestManager; +using autoware::velocity_smoother::VelocitySmootherNode; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + test_manager->subscribeOutput( + "velocity_smoother/output/trajectory"); + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.append_parameter_override("algorithm_type", "JerkFiltered"); + node_options.append_parameter_override("publish_debug_trajs", false); + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); + node_options.arguments( + {"--ros-args", "--params-file", autoware_test_utils_dir + "/config/test_common.param.yaml", + "--params-file", autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + "--params-file", autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + "--params-file", velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + "--params-file", velocity_smoother_dir + "/config/default_common.param.yaml", "--params-file", + velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + rclcpp::Node::SharedPtr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInput( + test_target_node, "/localization/kinematic_state", autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/external_velocity_limit_mps", + autoware_internal_planning_msgs::msg::VelocityLimit{}); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/operation_mode_state", + autoware_adapi_v1_msgs::msg::OperationModeState{}); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/acceleration", + geometry_msgs::msg::AccelWithCovarianceStamped{}); +} + +TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_trajectory_topic = "velocity_smoother/output/trajectory"; + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); + + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_trajectory_topic = "velocity_smoother/output/trajectory"; + const std::string input_odometry_topic = "/localization/kinematic_state"; + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); + + rclcpp::shutdown(); +}