From eb47ec5cab27e3bb5b31759f20a37ffd9bd832d3 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 7 Dec 2021 09:50:51 +0900 Subject: [PATCH] feat: trajectory follower package (#84) * Back port .auto control packages (#571) * Implement Lateral and Longitudinal Control Muxer * [#570] Porting wf_simulator * [#1189] Deactivate flaky test in 'trajectory_follower_nodes' * [#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer' * [#1057] Add osqp_interface package * [#1057] Add library code for MPC-based lateral control * [#1271] Use std::abs instead of abs * [#1057] Implement Lateral Controller for Cargo ODD * [#1246] Resolve "Test case names currently use snake_case but should be CamelCase" * [#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes' * [#1058] Add library code of longitudinal controller * Fix build error for trajectory follower Signed-off-by: wep21 * Fix build error for trajectory follower nodes Signed-off-by: wep21 * [#1272] Add AckermannControlCommand support to simple_planning_simulator * [#1058] Add Longitudinal Controller node * [#1058] Rename velocity_controller -> longitudinal_controller * [#1058] Update CMakeLists.txt for the longitudinal_controller_node * [#1058] Add smoke test python launch file * [#1058] Use LowPassFilter1d from trajectory_follower * [#1058] Use autoware_auto_msgs * [#1058] Changes for .auto (debug msg tmp fix, common func, tf listener) * [#1058] Remove unused parameters * [#1058] Fix ros test * [#1058] Rm default params from declare_parameters + use autoware types * [#1058] Use default param file to setup NodeOptions in the ros test * [#1058] Fix docstring * [#1058] Replace receiving a Twist with a VehicleKinematicState * [#1058] Change class variables format to m_ prefix * [#1058] Fix plugin name of LongitudinalController in CMakeLists.txt * [#1058] Fix copyright dates * [#1058] Reorder includes * [#1058] Add some tests (~89% coverage without disabling flaky tests) * [#1058] Add more tests (90+% coverage without disabling flaky tests) * [#1058] Use Float32MultiArrayDiagnostic message for debug and slope * [#1058] Calculate wheel_base value from vehicle parameters * [#1058] Cleanup redundant logger setting in tests * [#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures * [#1058] Remove TF listener and use published vehicle state instead * [#1058] Change smoke tests to use autoware_testing * [#1058] Add plotjuggler cfg for both lateral and longitudinal control * [#1058] Improve design documents * [#1058] Disable flaky test * [#1058] Properly transform vehicle state in longitudinal node * [#1058] Fix TF buffer of lateral controller * [#1058] Tuning of lateral controller for LGSVL * [#1058] Fix formating * [#1058] Fix /tf_static sub to be transient_local * [#1058] Fix yaw recalculation of reverse trajs in the lateral controller * modify trajectory_follower for galactic build Signed-off-by: Takamasa Horibe * [#1379] Update trajectory_follower * [#1379] Update simple_planning_simulator * [#1379] Update trajectory_follower_nodes * apply trajectory msg modification in control Signed-off-by: Takamasa Horibe * move directory Signed-off-by: Takamasa Horibe * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe * remove .iv trajectory follower Signed-off-by: Takamasa Horibe * use .auto trajectory_follower Signed-off-by: Takamasa Horibe * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe * apply .auto message split Signed-off-by: Takamasa Horibe * fix build depend Signed-off-by: Takamasa Horibe * fix packages using osqp * fix autoware_auto_geometry * ignore lint of some packages * ignore ament_lint of some packages * ignore lint/pre-commit of trajectory_follower_nodes * disable unit tests of some packages Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: tomoya.kimura Signed-off-by: tanaka3 * Fix message interface and tests of 'trajectory_follower_nodes' (#617) * Update longitudinal_controller_node to use VehicleOdometry * Update lateral_controller_node for VehicleOdometry and SteeringReport * Fix tests Signed-off-by: tanaka3 * Change the topic name of the control_cmd to align them in trajectory_follower_node (#659) * Fix topic names * Fix namespace * Fix test codes Signed-off-by: tanaka3 * Fix type of current odometry in trajectory_follower_node (#666) Signed-off-by: tanaka3 * [latlon muxer] fix gtest (#670) * fix gtest * remove unchanged Signed-off-by: tanaka3 * Update lateral controller (#708) * Fix parameter names of mpc_follower (#1376) * remove yaw_recalc param in mpc (#1241) (#1476) * parameterize curvature num (#1674) (#1577) * fix rosparam steer_rate_lim_degs to steer_rate_lim_dps in mpc_follower (#1848) * Fix spellcheck fail for some packages #1842 * use interpolation::slerp (#2161) * Fix/mpc reset prev result (#2185) * add add guard (#2184) * add-mpc-optimization-status-print (#2189) * Apply ament_uncrustify * Update control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp Co-authored-by: Takamasa Horibe Co-authored-by: Takamasa Horibe Signed-off-by: tanaka3 * Use vehicle info utils (#709) * Use vehicle info utils and disable unit test * Update trajectory_follower_nodes default parameters and reenable tests * Separate vehicle info param file for test Co-authored-by: Maxime CLEMENT Signed-off-by: tanaka3 * remove stop distance condition from stopState decision (#1916) (#715) Signed-off-by: tanaka3 * velocity_controller sync to .iv develop latest (#699) * non extrapolate velocity in lerpTrajectory to avoid negative velocity just before vehicle stops (#2033) * Add keep braking function at driving state (#2346) * Add keep braking function at driving state Signed-off-by: Makoto Kurihara * Remove debug messages Signed-off-by: Makoto Kurihara * Fix format Signed-off-by: Makoto Kurihara * Feature/add doc for keep braking function at driving state (#2366) * Add the description of brake keeping Signed-off-by: Makoto Kurihara * Add the english document Signed-off-by: Makoto Kurihara * Improve description Signed-off-by: Makoto Kurihara * Add english description Signed-off-by: Makoto Kurihara Co-authored-by: Takayuki Murooka Co-authored-by: Makoto Kurihara Co-authored-by: Fumiya Watanabe Signed-off-by: tanaka3 * fix topic name in plot juggler (#734) Signed-off-by: tanaka3 * Fix unstable test in trajectory_follower_nodes (#731) * Added spin_some to store tf_buffer * add more spin * uncomment statement * Remove temp file * Update control/trajectory_follower_nodes/src/lateral_controller_node.cpp Co-authored-by: Takamasa Horibe Co-authored-by: Takamasa Horibe Signed-off-by: tanaka3 Co-authored-by: Takamasa Horibe Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: tomoya.kimura Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: Takayuki Murooka Co-authored-by: Makoto Kurihara Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> --- .../trajectory_follower_nodes/CMakeLists.txt | 90 ++ .../plot_juggler_trajectory_follower.xml | 276 +++++ .../design/lateral_controller-design.md | 122 +++ .../design/latlon_muxer-design.md | 37 + .../design/longitudinal_controller-design.md | 151 +++ .../design/media/BrakeKeeping.drawio.svg | 4 + .../LongitudinalControllerDiagram.drawio.svg | 4 + ...udinalControllerStateTransition.drawio.svg | 348 +++++++ .../design/trajectory_follower-design.md | 26 + .../lateral_controller_node.hpp | 231 +++++ .../latlon_muxer_node.hpp | 74 ++ .../longitudinal_controller_node.hpp | 370 +++++++ .../visibility_control.hpp | 37 + control/trajectory_follower_nodes/package.xml | 34 + .../param/lateral_controller_defaults.yaml | 69 ++ .../param/latlon_muxer_defaults.yaml | 3 + .../longitudinal_controller_defaults.yaml | 82 ++ .../param/test_vehicle_info.yaml | 11 + .../src/lateral_controller_node.cpp | 554 ++++++++++ .../src/latlon_muxer_node.cpp | 102 ++ .../src/longitudinal_controller_node.cpp | 971 ++++++++++++++++++ .../test/test_lateral_controller_node.cpp | 457 +++++++++ .../test/test_latlon_muxer_node.cpp | 201 ++++ .../test_longitudinal_controller_node.cpp | 470 +++++++++ .../test/trajectory_follower_test_utils.hpp | 85 ++ 25 files changed, 4809 insertions(+) create mode 100644 control/trajectory_follower_nodes/CMakeLists.txt create mode 100644 control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml create mode 100644 control/trajectory_follower_nodes/design/lateral_controller-design.md create mode 100644 control/trajectory_follower_nodes/design/latlon_muxer-design.md create mode 100644 control/trajectory_follower_nodes/design/longitudinal_controller-design.md create mode 100644 control/trajectory_follower_nodes/design/media/BrakeKeeping.drawio.svg create mode 100644 control/trajectory_follower_nodes/design/media/LongitudinalControllerDiagram.drawio.svg create mode 100644 control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg create mode 100644 control/trajectory_follower_nodes/design/trajectory_follower-design.md create mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp create mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp create mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp create mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp create mode 100644 control/trajectory_follower_nodes/package.xml create mode 100644 control/trajectory_follower_nodes/param/lateral_controller_defaults.yaml create mode 100644 control/trajectory_follower_nodes/param/latlon_muxer_defaults.yaml create mode 100644 control/trajectory_follower_nodes/param/longitudinal_controller_defaults.yaml create mode 100644 control/trajectory_follower_nodes/param/test_vehicle_info.yaml create mode 100644 control/trajectory_follower_nodes/src/lateral_controller_node.cpp create mode 100644 control/trajectory_follower_nodes/src/latlon_muxer_node.cpp create mode 100644 control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp create mode 100644 control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp create mode 100644 control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp create mode 100644 control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp create mode 100644 control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt new file mode 100644 index 0000000000000..32e4c5d42523b --- /dev/null +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -0,0 +1,90 @@ +# Copyright 2021 The Autoware Foundation +# +# 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. + +cmake_minimum_required(VERSION 3.5) +project(trajectory_follower_nodes) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +set(LATERAL_CONTROLLER_NODE lateral_controller_node) +ament_auto_add_library(${LATERAL_CONTROLLER_NODE} SHARED + include/trajectory_follower_nodes/lateral_controller_node.hpp + src/lateral_controller_node.cpp +) + +autoware_set_compile_options(${LATERAL_CONTROLLER_NODE}) +# TODO(lateral_controller) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. +# TODO(lateral_controller) : Temporary workaround until this is fixed. +target_compile_options(${LATERAL_CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) +rclcpp_components_register_node(${LATERAL_CONTROLLER_NODE} + PLUGIN "autoware::motion::control::trajectory_follower_nodes::LateralController" + EXECUTABLE ${LATERAL_CONTROLLER_NODE}_exe +) + +set(LONGITUDINAL_CONTROLLER_NODE longitudinal_controller_node) +ament_auto_add_library(${LONGITUDINAL_CONTROLLER_NODE} SHARED + include/trajectory_follower_nodes/longitudinal_controller_node.hpp + src/longitudinal_controller_node.cpp +) + +autoware_set_compile_options(${LONGITUDINAL_CONTROLLER_NODE}) +# TODO(longitudinal_controller) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. +# TODO(longitudinal_controller) : Temporary workaround until this is fixed. +target_compile_options(${LONGITUDINAL_CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) +rclcpp_components_register_node(${LONGITUDINAL_CONTROLLER_NODE} + PLUGIN "autoware::motion::control::trajectory_follower_nodes::LongitudinalController" + EXECUTABLE ${LONGITUDINAL_CONTROLLER_NODE}_exe +) + +set(LATLON_MUXER_NODE latlon_muxer_node) +ament_auto_add_library(${LATLON_MUXER_NODE} SHARED + include/trajectory_follower_nodes/latlon_muxer_node.hpp + src/latlon_muxer_node.cpp +) + +autoware_set_compile_options(${LATLON_MUXER_NODE}) +# TODO(latlon_muxer) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. +# TODO(latlon_muxer) : Temporary workaround until this is fixed. +target_compile_options(${LATLON_MUXER_NODE} PRIVATE -Wno-error=old-style-cast) +rclcpp_components_register_node(${LATLON_MUXER_NODE} + PLUGIN "autoware::motion::control::trajectory_follower_nodes::LatLonMuxer" + EXECUTABLE ${LATLON_MUXER_NODE}_exe +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + # Unit tests + set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_nodes) + ament_add_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} + test/trajectory_follower_test_utils.hpp + test/test_latlon_muxer_node.cpp + test/test_lateral_controller_node.cpp + test/test_longitudinal_controller_node.cpp + ) + ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node) + autoware_set_compile_options(${TRAJECTORY_FOLLOWER_NODES_TEST}) + target_link_libraries(${TRAJECTORY_FOLLOWER_NODES_TEST} ${LATLON_MUXER_NODE} ${LATERAL_CONTROLLER_NODE} ${LONGITUDINAL_CONTROLLER_NODE}) + + #find_package(autoware_testing REQUIRED) + #add_smoke_test(${PROJECT_NAME} ${LATLON_MUXER_NODE}_exe PARAM_FILENAME latlon_muxer_defaults.yaml) + #add_smoke_test(${PROJECT_NAME} ${LATERAL_CONTROLLER_NODE}_exe PARAM_FILENAME lateral_controller_defaults.yaml) + #add_smoke_test(${PROJECT_NAME} ${LONGITUDINAL_CONTROLLER_NODE}_exe PARAM_FILENAME longitudinal_controller_defaults.yaml) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param +) diff --git a/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml b/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml new file mode 100644 index 0000000000000..289ffd8dddacc --- /dev/null +++ b/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml @@ -0,0 +1,276 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/trajectory_follower_nodes/design/lateral_controller-design.md b/control/trajectory_follower_nodes/design/lateral_controller-design.md new file mode 100644 index 0000000000000..6530cb5701fbf --- /dev/null +++ b/control/trajectory_follower_nodes/design/lateral_controller-design.md @@ -0,0 +1,122 @@ +Lateral Controller {#lateral-controller-design} +============================================= + +This is the design document for the lateral controller node +in the `trajectory_follower_nodes` package. + +# Purpose / Use cases + + +This node is used to general lateral control commands (steering angle and steering rate) +when following a path. + +# Design + + +The node uses an implementation of linear model predictive control (MPC) for accurate path tracking. +The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command. +The optimization of the control command is formulated as a Quadradic Program (QP). + +These functionalities are implemented in the `trajectory_follower` package +(see @subpage trajectory_follower-mpc-design) + +## Assumptions / Known limits + +The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose. + - Issue to add points behind ego: https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1273 + +## Inputs / Outputs / API + + +Inputs + - `input/reference_trajectory` : reference trajectory to follow. + - `input/current_kinematic_state`: current state of the vehicle (position, velocity, etc). +Output + - `output/lateral_control_cmd`: generated lateral control command. + +## Parameter description + +The default parameters defined in `param/lateral_controller_defaults.yaml` are adjusted to the +AutonomouStuff Lexus RX 450h for under 40 km/h driving. + +| Name | Type | Description | Default value | +| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------ | :------------ | +| show_debug_info | bool | display debug info | false | +| ctrl_period | double | control period [s] | 0.03 | +| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | +| enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true | +| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 | +| path_smoothing_times | int | number of times of applying path smoothing filter | 1 | +| curvature_smoothing_num_ref_steer | double | index distance of points used in curvature calculation for reference steer command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35 | +| curvature_smoothing_num_traj | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 1 | +| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | +| admissible_position_error | double | stop vehicle when following position error is larger than this value [m]. | 5.0 | +| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 | + +### MPC algorithm + +| Name | Type | Description | Default value | +| :-------------------------------------- | :----- | :---------------------------------------------------------------------------------------------- | :---------------- | +| qp_solver_type | string | QP solver option. described below in detail. | unconstraint_fast | +| vehicle_model_type | string | vehicle model option. described below in detail. | kinematics | +| prediction_horizon | int | total prediction step for MPC | 70 | +| prediction_sampling_time | double | prediction period for one step [s] | 0.1 | +| weight_lat_error | double | weight for lateral error | 0.1 | +| weight_heading_error | double | weight for heading error | 0.0 | +| weight_heading_error_squared_vel_coeff | double | weight for heading error \* velocity | 5.0 | +| weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | +| weight_steering_input_squared_vel_coeff | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | +| weight_terminal_lat_error | double | terminal cost weight for lateral error | 1.0 | +| weight_terminal_heading_error | double | terminal cost weight for heading error | 0.1 | +| zero_ff_steer_deg | double | threshold of feedforward angle [deg]. feedforward angle smaller than this value is set to zero. | 2.0 | + +### Vehicle + +| Name | Type | Description | Default value | +| :------------ | :----- | :--------------------------------------------------------------------------------- | :------------ | +| cg_to_front_m | double | distance from baselink to the front axle[m] | 1.228 | +| cg_to_rear_m | double | distance from baselink to the rear axle [m] | 1.5618 | +| mass_fl | double | mass applied to front left tire [kg] | 600 | +| mass_fr | double | mass applied to front right tire [kg] | 600 | +| mass_rl | double | mass applied to rear left tire [kg] | 600 | +| mass_rr | double | mass applied to rear right tire [kg] | 600 | +| cf | double | front cornering power [N/rad] | 155494.663 | +| cr | double | rear cornering power [N/rad] | 155494.663 | +| steering_tau | double | steering dynamics time constant (1d approximation) for vehicle model [s] | 0.3 | +| steer_lim_deg | double | steering angle limit for vehicle model [deg]. This is also used for QP constraint. | 35.0 | + +## How to tune MPC parameters + +1. Set appropriate vehicle kinematics parameters for distance to front and rear axle, and `steer_lim_deg`. +Also check that the input `VehicleKinematicState` has appropriate values (speed: vehicle rear-wheels-center velocity [km/h], angle: steering (tire) angle [rad]). +These values give a vehicle information to the controller for path following. +Errors in these values cause fundamental tracking error. + +2. Set appropriate vehicle dynamics parameters of `steering_tau`, which is the approximated delay from steering angle command to actual steering angle. + +3. Set `weight_steering_input` = 1.0, `weight_lat_error` = 0.1, and other weights to 0. +If the vehicle oscillates when driving with low speed, set `weight_lat_error` smaller. + +4. Adjust other weights. +One of the simple way for tuning is to increase `weight_lat_error` until oscillation occurs. +If the vehicle is unstable with very small `weight_lat_error`, increase terminal weight : +`weight_terminal_lat_error` and `weight_terminal_heading_error` to improve tracking stability. +Larger `prediction_horizon` and smaller `prediction_sampling_time` is effective for tracking performance, but it is a trade-off between computational costs. +Other parameters can be adjusted like below. + +- `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID. +- `weight_heading_error`: Make a drive straight. This acts like D gain in PID. +- `weight_heading_error_squared_vel_coeff` : Make a drive straight in high speed range. +- `weight_steering_input`: Reduce oscillation of tracking. +- `weight_steering_input_squared_vel_coeff`: Reduce oscillation of tracking in high speed range. +- `weight_lat_jerk`: Reduce lateral jerk. +- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. +- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. + +# Related issues + +- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057 diff --git a/control/trajectory_follower_nodes/design/latlon_muxer-design.md b/control/trajectory_follower_nodes/design/latlon_muxer-design.md new file mode 100644 index 0000000000000..31458fe29b802 --- /dev/null +++ b/control/trajectory_follower_nodes/design/latlon_muxer-design.md @@ -0,0 +1,37 @@ +Lateral/Longitudinal Control Muxer {#latlon-muxer-design} +============================================= + +# Purpose + +When using controllers that independently compute lateral and longitudinal commands, +this node combines the resulting messages into a single control command message. + +# Design + +Inputs. +- `AckermannLateralCommand`: lateral command. +- `LongitudinalCommand`: longitudinal command. + +Output. +- `AckermannControlCommand`: message containing both lateral and longitudinal commands. + +Parameter. +- `timeout_thr_sec`: duration in second after which input messages are discarded. + +Each time the node receives an input message it publishes an `AckermannControlCommand` +if the following two conditions are met. +1. Both inputs have been received. +2. The last received input messages are not older than defined by `timeout_thr_sec`. + +# Implementation Details + +Callbacks `latCtrlCmdCallback` and `lonCtrlCmdCallback` are defined for each input message. +Upon reception, the message is stored and function `publishCmd` is called. + +Function `publishCmd` first checks that both messages have been received +and that the stored messages are not older than the timeout. +If both conditions are true, the combined control message is built and published. + +`checkTimeout` is used to check that the stored messages are not too old +by comparing the timeout parameter `timeout_thr_sec` +with the difference between `rclcpp::Time stamp` and the current time `now()`. diff --git a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md b/control/trajectory_follower_nodes/design/longitudinal_controller-design.md new file mode 100644 index 0000000000000..c91a40ad959e3 --- /dev/null +++ b/control/trajectory_follower_nodes/design/longitudinal_controller-design.md @@ -0,0 +1,151 @@ +Longitudinal Controller {#longitudinal-controller-design} +=========== + +# Purpose / Use cases + +The longitudinal_controller computes the target acceleration to achieve the target velocity set at each point of the target trajectory using a feed-forward/back control. + +It also contains a slope force correction that takes into account road slope information, and a delay compensation function. +It is assumed that the target acceleration calculated here will be properly realized by the vehicle interface. + +Note that the use of this module is not mandatory for Autoware if the vehicle supports the "target speed" interface. + +## Separated lateral (steering) and longitudinal (velocity) controls + +This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. + +- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. +- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. + +Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. + +### Complex requirements for longitudinal motion + +The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. + +In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. + +There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. + +### Nonlinear coupling of lateral and longitudinal motion + +The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. + +Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. + +# Design + +## Assumptions / Known limits + +1. Smoothed target velocity and its acceleration shall be set in the trajectory + 1. The velocity command is not smoothed inside the controller (only noise may be removed). + 2. For step-like target signal, tracking is performed as fast as possible. +2. The vehicle velocity must be an appropriate value + 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction + 2. The ego-velocity should be given with appropriate noise processing. + 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. +3. The output of this controller must be achieved by later modules (e.g. vehicle interface). + 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. + +## Inputs / Outputs / API + +### Output + +- control_cmd [`autoware_auto_msgs/LongitudinalCommand`] : command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. +- debug_values [`autoware_auto_msgs/Float32MultiArrayDiagnostic`] : debug values used for the control command generation (e.g. the contributions of each P-I-D terms). + +### Input + +- current_state [`autoware_auto_msgs/VehicleKinematicState`] : Current ego state including the current pose and velocity. +- current_trajectory [`autoware_auto_msgs/Trajectory`] : Current target trajectory for the desired velocity on the each trajectory points. + +# Inner-workings / Algorithms + +## States + +This module has four state transitions as shown below in order to handle special processing in a specific situation. + +- **DRIVE** + - Executes target velocity tracking by PID control. + - It also applies the delay compensation and slope compensation. +- **STOPPING** + - Controls the motion just before stopping. + - Special sequence is performed to achieve accurate and smooth stopping. +- **STOPPED** + - Performs operations in the stopped state (e.g. brake hold) +- **EMERGENCY**. + - Enters an emergency state when certain conditions are met (e.g., when the vehicle has crossed a certain distance of a stop line). + - The recovery condition (whether or not to keep emergency state until the vehicle completely stops) or the deceleration in the emergency state are defined by parameters. + +The state transition diagram is shown below. + +![LongitudinalControllerStateTransition](./media/LongitudinalControllerStateTransition.drawio.svg) + +## Logics + +### Control Block Diagram + +![LongitudinalControllerDiagram](./media/LongitudinalControllerDiagram.drawio.svg) + +### FeedForward (FF) + +The reference acceleration set in the trajectory and slope compensation terms are output as a feedforward. Under ideal conditions with no modeling error, this FF term alone should be sufficient for velocity tracking. + +Tracking errors causing modeling or discretization errors are removed by the feedback control (now using PID). + +#### Brake keeping + +From the viewpoint of ride comfort, stopping with 0 acceleration is important because it reduces the impact of braking. However, if the target acceleration when stopping is 0, the vehicle may cross over the stop line or accelerate a little in front of the stop line due to vehicle model error or gradient estimation error. + +For reliable stopping, the target acceleration calculated by the FeedForward system is limited to a negative acceleration when stopping. + +![BrakeKeepingDiagram](./media/BrakeKeeping.drawio.svg) + +### Slope compensation + +Based on the slope information, a compensation term is added to the target acceleration. + +There are two sources of the slope information, which can be switched by a parameter. + +- Pitch of the estimated ego-pose (default) + - Calculates the current slope from the pitch angle of the estimated ego-pose + - Pros: Easily available + - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. +- Z coordinate on the trajectory + - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory + - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained + - Pros: Can be used in combination with delay compensation (not yet implemented) + - Cons: z-coordinates of high-precision map is needed. + - Cons: Does not support free space planning (for now) + +### PID control + +For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. + +This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity. + +This PID logic has a maximum value for the output of each term. This is to prevent the following: + +- Large integral terms may cause unintended behavior by users. +- Unintended noise may cause the output of the derivative term to be very large. + +Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as "Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start. +On the other hand, if the vehicle gets stuck in a depression in the road surface when starting, the vehicle will not start forever, which is currently being addressed by developers. + +At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. +This may be replaced by a higher performance controller (adaptive control or robust control) in future development. + +### Time delay compensation + +At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy. +Depending on the actuating principle of the vehicle, the mechanism that physically controls the gas pedal and brake typically has a delay of about a hundred millisecond. + +In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem. + +# References / External links + +# Future extensions / Unimplemented parts + +# Related issues + +- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058 diff --git a/control/trajectory_follower_nodes/design/media/BrakeKeeping.drawio.svg b/control/trajectory_follower_nodes/design/media/BrakeKeeping.drawio.svg new file mode 100644 index 0000000000000..1affbdf0bd304 --- /dev/null +++ b/control/trajectory_follower_nodes/design/media/BrakeKeeping.drawio.svg @@ -0,0 +1,4 @@ + + + +
Velocity
Velocity
Acceleration
Acceleration
Time
Time
Time
Time
Brake keeping
acceleration
Brake keeping...
After brake keeping
After brake keeping
Target acceleration from trajectory
Target acceleration from trajecto...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/control/trajectory_follower_nodes/design/media/LongitudinalControllerDiagram.drawio.svg b/control/trajectory_follower_nodes/design/media/LongitudinalControllerDiagram.drawio.svg new file mode 100644 index 0000000000000..45773434eb112 --- /dev/null +++ b/control/trajectory_follower_nodes/design/media/LongitudinalControllerDiagram.drawio.svg @@ -0,0 +1,4 @@ + + + +
Predicted
Reference
Velocity
Predict...
Predicted
Reference
Acceleration
Predict...
Slope Force Conversion
g * sin(pitch)
Slope Force Conversion...
Slope
Angle
Slope...
P
P
I
I
D
D
∫
∫
d/dt
d/dt
LPF
LPF
+
+
+
+
+
+
+
+
+
+
+
+
-
-
Slope Compensation Limit
Slope Compensation Limit
FeedForward Acceleration Limit
FeedForward Acceleration Limit
FeedBack Control
FeedBack Control
+
+
-
-
Reference
Trajectory
Referen...
Delay Compensation
Delay Compensation
Ego Pose
Ego Pose
Predicted
Ego Velocity
Predict...
Prediction Model
Prediction Model
getReferencePoint
getReferencePoint
Predicted
Ego Pose
Predict...
Ego Velocity
Ego Vel...
FeedForward Control
FeedForward Control
Acceleration
Acceler...
Vehicle
Vehicle
Brake keeping Limit
Brake keeping LimitViewer does not support full SVG 1.1 \ No newline at end of file diff --git a/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg b/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg new file mode 100644 index 0000000000000..94706f229733e --- /dev/null +++ b/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg @@ -0,0 +1,348 @@ + + + + + + + + + + + + +
+
+
+ + + 0. DRIVE + + +
+
+
+ + v = interpolateTargetVel() + +
+
+ + a = applyVelocityFeedback() + +
+
+
+
+ + + 0. DRIVE... + + + + + + + + + + + +
+
+
+ + + 1. STOPPING + +
+ +
+
+ + v = stopped_vel_ + +
+
+ + a = smooth_stop_.calculate() + +
+
+
+
+ + + 1. STOPPING... + + + + + + + + + +
+
+
+ + 2. STOPPED +
+ +
+
+ + v = stopped_vel_ + +
+
+ + a = stopped_acc_ + +
+
+
+
+ + + 2. STOPPED... + + + + + + + + + +
+
+
+ + 3. EMERGENCY +
+ +
+
+ + v = emergency_vel_ + +
+
+ + a = emergency_acc_ + +
+
+
+
+ + + 3. EMERGENCY... + + + + + + + +
+
+
+ stop condition +
+
+
+ + + stop condition + + + + + + + +
+
+
+

+ + strict departure condition + + : Distance to stop is larger than A1. +
+ + loose departure condition + + : Distance to stop is larger than A1 + A2. +
+ + stopping condition + + : Distance to stop is smaller than B1. +
+ + stop condition + + : For 500 [ms], self velocity is smaller than C1, and self acceleration is smaller than C2. +
+ + emergency condition + + + : Distance to stop is smaller than D1, or self pose is not within D2, D3 from trajectory. + +

+
+
+
+ + + strict departure condition : Distance to stop is larger than... + + + + + + + +
+
+
+ strict departure +
+ condition +
+
+
+ + + strict departure... + + + + + + + +
+
+
+ stopping +
+ condition +
+
+
+ + + stopping... + + + + + + + +
+
+
+ loose departure +
+ condition +
+
+
+ + + loose departure... + + + + + + + +
+
+
+ emergency condition +
+
+
+ + + emergency con... + + + + + + + +
+
+
+ + stop condition + +
+ and +
+ not + + emergency condition + +
+
+
+ + + stop condition... + + + + + + + +
+
+
+

+ A1 : drive_state_stop_dist +
+ A2 : drive_state_offset_stop_dist +
+ B1 : stopping_state_stop_dist +
+ C1 : stopped_state_entry_vel +
+ C2 : stopped_state_entry_acc +
+ D1 : emergency_state_overshoot_stop_dist +
+ D2 : emergency_state_traj_trans_dev +
+ D3 : emergency_state_traj_rot_dev +

+
+
+
+ + + A1 : drive_state_stop_dist... + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/control/trajectory_follower_nodes/design/trajectory_follower-design.md b/control/trajectory_follower_nodes/design/trajectory_follower-design.md new file mode 100644 index 0000000000000..aee0370bda671 --- /dev/null +++ b/control/trajectory_follower_nodes/design/trajectory_follower-design.md @@ -0,0 +1,26 @@ +Trajectory Follower Nodes {#trajectory_follower_nodes-package-design} +============================================= + +# Purpose + +Generate control commands to follow a given Trajectory. + +# Design + +This functionality is decomposed into three nodes. +- @subpage lateral-controller-design : generates lateral control messages. +- @subpage longitudinal-controller-design : generates longitudinal control messages. +- @subpage latlon-muxer-design : combines the lateral and longitudinal control commands +into a single control command. + +Core functionalities are implemented in the @subpage trajectory_follower-package-design package. + +@image html images/trajectory_follower-diagram.png "Overview of the Trajectory Follower package" + +# Debugging + +Debug information are published by the lateral and longitudinal controller using `autoware_auto_msgs/Float32MultiArrayDiagnostic` messages. + +A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. + +In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. \ No newline at end of file diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp new file mode 100644 index 0000000000000..f1680f05cea43 --- /dev/null +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp @@ -0,0 +1,231 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 TRAJECTORY_FOLLOWER_NODES__LATERAL_CONTROLLER_NODE_HPP_ +#define TRAJECTORY_FOLLOWER_NODES__LATERAL_CONTROLLER_NODE_HPP_ + +#include +#include +#include +#include + +#include "trajectory_follower_nodes/visibility_control.hpp" +#include "trajectory_follower/interpolate.hpp" +#include "trajectory_follower/lowpass_filter.hpp" +#include "trajectory_follower/mpc.hpp" +#include "trajectory_follower/mpc_trajectory.hpp" +#include "trajectory_follower/mpc_utils.hpp" +#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" +#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" +#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "common/types.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "osqp_interface/osqp_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tf2/utils.h" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "vehicle_info_util/vehicle_info_util.hpp" + + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower_nodes +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; + +class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node +{ +public: + /** + * @brief constructor + */ + explicit LateralController(const rclcpp::NodeOptions & node_options); + + /** + * @brief destructor + */ + virtual ~LateralController(); + +private: + //!< @brief topic publisher for control command + rclcpp::Publisher::SharedPtr + m_pub_ctrl_cmd; + //!< @brief topic publisher for predicted trajectory + rclcpp::Publisher::SharedPtr m_pub_predicted_traj; + //!< @brief topic publisher for control diagnostic + rclcpp::Publisher::SharedPtr + m_pub_diagnostic; + //!< @brief topic subscription for reference waypoints + rclcpp::Subscription::SharedPtr m_sub_ref_path; + //!< @brief subscription for current velocity + rclcpp::Subscription::SharedPtr m_sub_odometry; + //!< @brief subscription for current steering + rclcpp::Subscription::SharedPtr m_sub_steering; + //!< @brief timer to update after a given interval + rclcpp::TimerBase::SharedPtr m_timer; + //!< @brief subscription for transform messages + rclcpp::Subscription::SharedPtr m_tf_sub; + rclcpp::Subscription::SharedPtr m_tf_static_sub; + + /* parameters for path smoothing */ + //!< @brief flag for path smoothing + bool8_t m_enable_path_smoothing; + //!< @brief param of moving average filter for path smoothing + int64_t m_path_filter_moving_ave_num; + //!< @brief point-to-point index distance for curvature calculation for trajectory //NOLINT + int64_t m_curvature_smoothing_num_traj; + //!< @brief point-to-point index distance for curvature calculation for reference steer command //NOLINT + int64_t m_curvature_smoothing_num_ref_steer; + //!< @brief path resampling interval [m] + float64_t m_traj_resample_dist; + + /* parameters for stop state */ + float64_t m_stop_state_entry_ego_speed; + float64_t m_stop_state_entry_target_speed; + + // MPC object + trajectory_follower::MPC m_mpc; + + //!< @brief measured pose + geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose_ptr; + //!< @brief measured velocity + nav_msgs::msg::Odometry::SharedPtr m_current_odometry_ptr; + //!< @brief measured steering + autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr m_current_steering_ptr; + //!< @brief reference trajectory + autoware_auto_planning_msgs::msg::Trajectory::SharedPtr + m_current_trajectory_ptr; + + //!< @brief mpc filtered output in previous period + float64_t m_steer_cmd_prev = 0.0; + + //!< @brief flag of m_ctrl_cmd_prev initialization + bool8_t m_is_ctrl_cmd_prev_initialized = false; + //!< @brief previous control command + autoware_auto_control_msgs::msg::AckermannLateralCommand m_ctrl_cmd_prev; + + //!< @brief buffer for transforms + tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME}; + tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; + + //!< initialize timer to work in real, simulation, and replay + void initTimer(float64_t period_s); + /** + * @brief compute and publish control command for path follow with a constant control period + */ + void onTimer(); + + /** + * @brief set m_current_trajectory with received message + */ + void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + + /** + * @brief update current_pose from tf + * @return true if the current pose was updated, false otherwise + */ + bool8_t updateCurrentPose(); + + /** + * @brief check if the received data is valid. + */ + bool8_t checkData() const; + + /** + * @brief set current_velocity with received message + */ + void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief set current_steering with received message + */ + void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); + + /** + * @brief publish control command + * @param [in] cmd published control command + */ + void publishCtrlCmd(autoware_auto_control_msgs::msg::AckermannLateralCommand cmd); + + /** + * @brief publish predicted future trajectory + * @param [in] predicted_traj published predicted trajectory + */ + void publishPredictedTraj(autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const; + + /** + * @brief publish diagnostic message + * @param [in] diagnostic published diagnostic + */ + void publishDiagnostic(autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) + const; + + /** + * @brief get stop command + */ + autoware_auto_control_msgs::msg::AckermannLateralCommand getStopControlCommand() const; + + /** + * @brief get initial command + */ + autoware_auto_control_msgs::msg::AckermannLateralCommand getInitialControlCommand() const; + + /** + * @brief check ego car is in stopped state + */ + bool8_t isStoppedState() const; + + /** + * @brief check if the trajectory has valid value + */ + bool8_t isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const; + + OnSetParametersCallbackHandle::SharedPtr m_set_param_res; + + /** + * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly + */ + void declareMPCparameters(); + + /** + * @brief Called when parameters are changed outside of node + */ + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); +}; +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER_NODES__LATERAL_CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp new file mode 100644 index 0000000000000..bcfdc4c05c1c0 --- /dev/null +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp @@ -0,0 +1,74 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 TRAJECTORY_FOLLOWER_NODES__LATLON_MUXER_NODE_HPP_ +#define TRAJECTORY_FOLLOWER_NODES__LATLON_MUXER_NODE_HPP_ + +#include +#include + +#include "trajectory_follower_nodes/visibility_control.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +/// \brief Resources relating to the trajectory_follower_nodes package +namespace trajectory_follower_nodes +{ +/// \class LatLonMuxer +/// \brief The node class used for muxing lateral and longitudinal messages +class LatLonMuxer : public rclcpp::Node +{ +public: + explicit TRAJECTORY_FOLLOWER_PUBLIC LatLonMuxer(const rclcpp::NodeOptions & node_options); + +private: + // \brief Callback for the lateral control command + void latCtrlCmdCallback(const autoware_auto_control_msgs::msg::AckermannLateralCommand::SharedPtr msg); + // \brief Callback for the longitudinal control command + void lonCtrlCmdCallback(const autoware_auto_control_msgs::msg::LongitudinalCommand::SharedPtr msg); + // \brief Publish the combined control command message + void publishCmd(); + // \brief Check that the received messages are not too old + // \return bool True if the stored messages timed out + bool checkTimeout(); + + rclcpp::Publisher::SharedPtr m_control_cmd_pub; + rclcpp::Subscription::SharedPtr + m_lat_control_cmd_sub; + rclcpp::Subscription::SharedPtr + m_lon_control_cmd_sub; + + std::shared_ptr m_lat_cmd; + std::shared_ptr m_lon_cmd; + // \brief Timeout duration in seconds + double m_timeout_thr_sec; +}; // class LatLonMuxer +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER_NODES__LATLON_MUXER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp new file mode 100644 index 0000000000000..87d17db4a8f80 --- /dev/null +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp @@ -0,0 +1,370 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TRAJECTORY_FOLLOWER_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_ +#define TRAJECTORY_FOLLOWER_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_ + +#include +#include +#include +#include + +#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "motion_common/motion_common.hpp" +#include "motion_common/trajectory_common.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "trajectory_follower/debug_values.hpp" +#include "trajectory_follower/longitudinal_controller_utils.hpp" +#include "trajectory_follower/lowpass_filter.hpp" +#include "trajectory_follower/pid.hpp" +#include "trajectory_follower/smooth_stop.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower_nodes +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; +namespace motion_common = ::autoware::motion::motion_common; + +/// \class LongitudinalController +/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) +class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node +{ +public: + explicit LongitudinalController(const rclcpp::NodeOptions & node_options); + +private: + struct Motion + { + float64_t vel{0.0}; + float64_t acc{0.0}; + }; + + enum class Shift { Forward = 0, Reverse }; + + struct ControlData + { + bool8_t is_far_from_trajectory{false}; + size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx + Motion current_motion{}; + Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation + float64_t stop_dist{0.0}; // signed distance that is positive when car is before the stopline + float64_t slope_angle{0.0}; + float64_t dt{0.0}; + }; + + // ros variables + rclcpp::Subscription::SharedPtr + m_sub_current_velocity; + rclcpp::Subscription::SharedPtr m_sub_trajectory; + rclcpp::Publisher::SharedPtr m_pub_control_cmd; + rclcpp::Publisher::SharedPtr m_pub_slope; + rclcpp::Publisher::SharedPtr m_pub_debug; + rclcpp::TimerBase::SharedPtr m_timer_control; + + rclcpp::Subscription::SharedPtr m_tf_sub; + rclcpp::Subscription::SharedPtr m_tf_static_sub; + tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME}; + tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; + + OnSetParametersCallbackHandle::SharedPtr m_set_param_res; + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + // pointers for ros topic + std::shared_ptr m_current_velocity_ptr{nullptr}; + std::shared_ptr m_prev_velocity_ptr{nullptr}; + std::shared_ptr m_trajectory_ptr{nullptr}; + + // vehicle info + float64_t m_wheel_base; + + // control state + enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; + ControlState m_control_state{ControlState::STOPPED}; + + // timer callback + float64_t m_control_rate; + + // delay compensation + float64_t m_delay_compensation_time; + + // enable flags + bool8_t m_enable_smooth_stop; + bool8_t m_enable_overshoot_emergency; + bool8_t m_enable_slope_compensation; + + // smooth stop transition + struct StateTransitionParams + { + // drive + float64_t drive_state_stop_dist; + float64_t drive_state_offset_stop_dist; + // stopping + float64_t stopping_state_stop_dist; + // stop + float64_t stopped_state_entry_vel; + float64_t stopped_state_entry_acc; + // emergency + float64_t emergency_state_overshoot_stop_dist; + float64_t emergency_state_traj_trans_dev; + float64_t emergency_state_traj_rot_dev; + }; + StateTransitionParams m_state_transition_params; + + // drive + trajectory_follower::PIDController m_pid_vel; + std::shared_ptr m_lpf_vel_error{nullptr}; + float64_t m_current_vel_threshold_pid_integrate; + bool8_t m_enable_brake_keeping_before_stop; + float64_t m_brake_keeping_acc; + + // smooth stop + trajectory_follower::SmoothStop m_smooth_stop; + + // stop + struct StoppedStateParams + { + float64_t vel; + float64_t acc; + float64_t jerk; + }; + StoppedStateParams m_stopped_state_params; + + // emergency + struct EmergencyStateParams + { + float64_t vel; + float64_t acc; + float64_t jerk; + }; + EmergencyStateParams m_emergency_state_params; + + // acceleration limit + float64_t m_max_acc; + float64_t m_min_acc; + + // jerk limit + float64_t m_max_jerk; + float64_t m_min_jerk; + + // slope compensation + bool8_t m_use_traj_for_pitch; + std::shared_ptr m_lpf_pitch{nullptr}; + float64_t m_max_pitch_rad; + float64_t m_min_pitch_rad; + + // 1st order lowpass filter for acceleration + std::shared_ptr m_lpf_acc{nullptr}; + + // buffer of send command + std::vector m_ctrl_cmd_vec; + + // for calculating dt + std::shared_ptr m_prev_control_time{nullptr}; + + // shift mode + Shift m_prev_shift{Shift::Forward}; + + // diff limit + Motion m_prev_ctrl_cmd{}; // with slope compensation + Motion m_prev_raw_ctrl_cmd{}; // without slope compensation + std::vector> m_vel_hist; + + // debug values + trajectory_follower::DebugValues m_debug_values; + + std::shared_ptr m_last_running_time{std::make_shared(this->now())}; + + /** + * @brief set current and previous velocity with received message + * @param [in] msg current state message + */ + void callbackCurrentVelocity( + const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + /** + * @brief set reference trajectory with received message + * @param [in] msg trajectory message + */ + void callbackTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + /** + * @brief compute control command, and publish periodically + */ + void callbackTimerControl(); + + /** + * @brief calculate data for controllers whose type is ControlData + * @param [in] current_pose current ego pose + */ + ControlData getControlData(const geometry_msgs::msg::Pose & current_pose); + + /** + * @brief calculate control command in emergency state + * @param [in] dt time between previous and current one + */ + Motion calcEmergencyCtrlCmd(const float64_t dt) const; + + /** + * @brief update control state according to the current situation + * @param [in] current_control_state current control state + * @param [in] control_data control data + */ + ControlState updateControlState( + const ControlState current_control_state, const ControlData & control_data); + + /** + * @brief calculate control command based on the current control state + * @param [in] current_control_state current control state + * @param [in] current_pose current ego pose + * @param [in] control_data control data + */ + Motion calcCtrlCmd( + const ControlState & current_control_state, const geometry_msgs::msg::Pose & current_pose, + const ControlData & control_data); + + /** + * @brief publish control command + * @param [in] ctrl_cmd calculated control command to control velocity + * @param [in] current_vel current velocity of the vehicle + */ + void publishCtrlCmd(const Motion & ctrl_cmd, const float64_t current_vel); + + /** + * @brief publish debug data + * @param [in] ctrl_cmd calculated control command to control velocity + * @param [in] control_data data for control calculation + */ + void publishDebugData(const Motion & ctrl_cmd, const ControlData & control_data); + + /** + * @brief calculate time between current and previous one + */ + float64_t getDt(); + + /** + * @brief calculate current velocity and acceleration + */ + Motion getCurrentMotion() const; + + /** + * @brief calculate direction (forward or backward) that vehicle moves + * @param [in] nearest_idx nearest index on trajectory to vehicle + */ + enum Shift getCurrentShift(const size_t nearest_idx) const; + + /** + * @brief filter acceleration command with limitation of acceleration and jerk, and slope compensation + * @param [in] raw_acc acceleration before filtered + * @param [in] control_data data for control calculation + */ + float64_t calcFilteredAcc(const float64_t raw_acc, const ControlData & control_data); + + /** + * @brief store acceleration command before slope compensation + * @param [in] accel command before slope compensation + */ + void storeAccelCmd(const float64_t accel); + + /** + * @brief add acceleration to compensate for slope + * @param [in] acc acceleration before slope compensation + * @param [in] pitch pitch angle (upward is negative) + * @param [in] shift direction that vehicle move (forward or backward) + */ + float64_t applySlopeCompensation( + const float64_t acc, const float64_t pitch, + const Shift shift) const; + + /** + * @brief keep target motion acceleration negative before stop + * @param [in] traj reference trajectory + * @param [in] motion delay compensated target motion + */ + Motion keepBrakeBeforeStop( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, + const size_t nearest_idx) const; + + /** + * @brief interpolate trajectory point that is nearest to vehicle + * @param [in] traj reference trajectory + * @param [in] point vehicle position + * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position + */ + autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Point & point, + const size_t nearest_idx) const; + + /** + * @brief calculate predicted velocity after time delay based on past control commands + * @param [in] current_motion current velocity and acceleration of the vehicle + * @param [in] delay_compensation_time predicted time delay + */ + float64_t predictedVelocityInTargetPoint( + const Motion current_motion, const float64_t delay_compensation_time) const; + + /** + * @brief calculate velocity feedback with feed forward and pid controller + * @param [in] target_motion reference velocity and acceleration. This acceleration will be used as feed forward. + * @param [in] dt time step to use + * @param [in] current_vel current velocity of the vehicle + */ + float64_t applyVelocityFeedback( + const Motion target_motion, const float64_t dt, const float64_t current_vel); + + /** + * @brief update variables for debugging about pitch + * @param [in] pitch current pitch of the vehicle (filtered) + * @param [in] traj_pitch current trajectory pitch + * @param [in] raw_pitch current raw pitch of the vehicle (unfiltered) + */ + void updatePitchDebugValues( + const float64_t pitch, const float64_t traj_pitch, + const float64_t raw_pitch); + + /** + * @brief update variables for velocity and acceleration + * @param [in] ctrl_cmd latest calculated control command + * @param [in] current_pose current pose of the vehicle + * @param [in] control_data data for control calculation + */ + void updateDebugVelAcc( + const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, + const ControlData & control_data); +}; +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp new file mode 100644 index 0000000000000..8d169f7516c3d --- /dev/null +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ +#define TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) + #if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) + #define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) + #define TRAJECTORY_FOLLOWER_LOCAL + #else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) + #define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) + #define TRAJECTORY_FOLLOWER_LOCAL + #endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#elif defined(__linux__) + #define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) + #define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) + #define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#else + #error "Unsupported Build Configuration" +#endif + +#endif // TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_nodes/package.xml new file mode 100644 index 0000000000000..6f22e6bddff2f --- /dev/null +++ b/control/trajectory_follower_nodes/package.xml @@ -0,0 +1,34 @@ + + + + trajectory_follower_nodes + 1.0.0 + Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + autoware_auto_cmake + + autoware_auto_planning_msgs + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + autoware_auto_system_msgs + trajectory_follower + rclcpp + rclcpp_components + vehicle_info_util + + ros2launch + + ament_cmake_gtest + ament_index_python + + + autoware_testing + fake_test_node + + + ament_cmake + + diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.yaml new file mode 100644 index 0000000000000..75e3c87f5f569 --- /dev/null +++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.yaml @@ -0,0 +1,69 @@ +/**: + ros__parameters: + + # -- system -- + ctrl_period: 0.03 # control period [s] + traj_resample_dist: 0.1 # path resampling interval [m] + use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) + admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value + admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + + # -- path smoothing -- + enable_path_smoothing: false # flag for path smoothing + path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing + curvature_smoothing_num_traj: 1 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + + # -- mpc optimization -- + qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) + mpc_prediction_horizon: 50 # prediction horizon step + mpc_prediction_dt: 0.1 # prediction horizon period [s] + mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q + mpc_weight_heading_error: 0.0 # heading error weight in matrix Q + mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q + mpc_weight_steering_input: 1.0 # steering error weight in matrix R + mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R + mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R + mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R + mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R + mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point + mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point + mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability + mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability + mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero + mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration + mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + + # -- vehicle model -- + vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics + input_delay: 0.24 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] + steer_lim_deg: 20.0 # steering angle limit [deg] + steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] + acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] + + # -- lowpass filter for noise reduction -- + steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] + error_deriv_lpf_cutoff_hz: 5.0 + + # stop state + stop_state_entry_ego_speed: 0.2 + stop_state_entry_target_speed: 0.5 + + # vehicle parameters + mass_kg: 2400.0 + mass_fl: 600.0 + mass_fr: 600.0 + mass_rl: 600.0 + mass_rr: 600.0 + cf: 155494.663 + cr: 155494.663 + diff --git a/control/trajectory_follower_nodes/param/latlon_muxer_defaults.yaml b/control/trajectory_follower_nodes/param/latlon_muxer_defaults.yaml new file mode 100644 index 0000000000000..371bb99787559 --- /dev/null +++ b/control/trajectory_follower_nodes/param/latlon_muxer_defaults.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + timeout_thr_sec: 0.5 # [s] Time limit after which input messages are discarded. diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.yaml new file mode 100644 index 0000000000000..0b3e262b065b3 --- /dev/null +++ b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + control_rate: 30.0 + delay_compensation_time: 0.17 + + enable_smooth_stop: true + enable_overshoot_emergency: true + enable_slope_compensation: false + + # state transition + drive_state_stop_dist: 0.5 + drive_state_offset_stop_dist: 1.0 + stopping_state_stop_dist: 0.49 + stopped_state_entry_vel: 0.1 + stopped_state_entry_acc: 0.1 + emergency_state_overshoot_stop_dist: 1.5 + emergency_state_traj_trans_dev: 3.0 + emergency_state_traj_rot_dev: 0.7 + + # drive state + kp: 1.0 + ki: 0.1 + kd: 0.0 + max_out: 1.0 + min_out: -1.0 + max_p_effort: 1.0 + min_p_effort: -1.0 + max_i_effort: 0.3 + min_i_effort: -0.3 + max_d_effort: 0.0 + min_d_effort: 0.0 + lpf_vel_error_gain: 0.9 + current_vel_threshold_pid_integration: 0.5 + enable_brake_keeping_before_stop: false + brake_keeping_acc: -0.2 + + # smooth stop state + smooth_stop_max_strong_acc: -0.5 + smooth_stop_min_strong_acc: -1.0 + smooth_stop_weak_acc: -0.3 + smooth_stop_weak_stop_acc: -0.8 + smooth_stop_strong_stop_acc: -3.4 + smooth_stop_max_fast_vel: 0.5 + smooth_stop_min_running_vel: 0.01 + smooth_stop_min_running_acc: 0.01 + smooth_stop_weak_stop_time: 0.8 + smooth_stop_weak_stop_dist: -0.3 + smooth_stop_strong_stop_dist: -0.5 + + # stopped state + stopped_vel: 0.0 + stopped_acc: -3.4 + stopped_jerk: -5.0 + + # emergency state + emergency_vel: 0.0 + emergency_acc: -5.0 + emergency_jerk: -3.0 + + # acceleration limit + max_acc: 3.0 + min_acc: -5.0 + + # jerk limit + max_jerk: 2.0 + min_jerk: -5.0 + + # pitch + use_trajectory_for_pitch_calculation: false + lpf_pitch_gain: 0.95 + max_pitch_rad: 0.1 + min_pitch_rad: -0.1 + + # vehicle parameters + vehicle: + mass_kg: 2400.0 + mass_fl: 600.0 + mass_fr: 600.0 + mass_rl: 600.0 + mass_rr: 600.0 + cf: 155494.663 + cr: 155494.663 diff --git a/control/trajectory_follower_nodes/param/test_vehicle_info.yaml b/control/trajectory_follower_nodes/param/test_vehicle_info.yaml new file mode 100644 index 0000000000000..b1279b50ef04a --- /dev/null +++ b/control/trajectory_follower_nodes/param/test_vehicle_info.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + wheel_radius: 0.39 + wheel_width: 0.42 + wheel_base: 2.74 # between front wheel center and rear wheel center + wheel_tread: 1.63 # between left wheel center and right wheel center + front_overhang: 1.0 # between front wheel center and vehicle front + rear_overhang: 1.03 # between rear wheel center and vehicle rear + left_overhang: 0.1 # between left wheel center and vehicle left + right_overhang: 0.1 # between right wheel center and vehicle right + vehicle_height: 2.5 diff --git a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp b/control/trajectory_follower_nodes/src/lateral_controller_node.cpp new file mode 100644 index 0000000000000..52b818ea2e3c3 --- /dev/null +++ b/control/trajectory_follower_nodes/src/lateral_controller_node.cpp @@ -0,0 +1,554 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 +#include +#include +#include +#include +#include +#include + +#include "trajectory_follower_nodes/lateral_controller_node.hpp" + +#include "tf2_ros/create_timer_ros.h" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower_nodes +{ +namespace +{ +using namespace std::chrono_literals; + +template +void update_param( + const std::vector & parameters, const std::string & name, T & value) +{ + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) {return parameter.get_name() == name;}); + if (it != parameters.cend()) { + value = static_cast(it->template get_value()); + } +} +} // namespace + +LateralController::LateralController(const rclcpp::NodeOptions & node_options) +: Node("lateral_controller", node_options) +{ + using std::placeholders::_1; + + m_mpc.m_ctrl_period = declare_parameter("ctrl_period"); + m_enable_path_smoothing = declare_parameter("enable_path_smoothing"); + m_path_filter_moving_ave_num = declare_parameter("path_filter_moving_ave_num"); + m_curvature_smoothing_num_traj = declare_parameter("curvature_smoothing_num_traj"); + m_curvature_smoothing_num_ref_steer = declare_parameter( + "curvature_smoothing_num_ref_steer"); + m_traj_resample_dist = declare_parameter("traj_resample_dist"); + m_mpc.m_admissible_position_error = + declare_parameter("admissible_position_error"); + m_mpc.m_admissible_yaw_error_rad = declare_parameter("admissible_yaw_error_rad"); + m_mpc.m_use_steer_prediction = declare_parameter("use_steer_prediction"); + m_mpc.m_param.steer_tau = declare_parameter("vehicle_model_steer_tau"); + + /* stop state parameters */ + m_stop_state_entry_ego_speed = declare_parameter("stop_state_entry_ego_speed"); + m_stop_state_entry_target_speed = + declare_parameter("stop_state_entry_target_speed"); + + /* mpc parameters */ + const float64_t steer_lim_deg = declare_parameter("steer_lim_deg"); + const float64_t steer_rate_lim_dps = declare_parameter("steer_rate_lim_dps"); + constexpr float64_t deg2rad = static_cast(autoware::common::types::PI) / 180.0; + m_mpc.m_steer_lim = steer_lim_deg * deg2rad; + m_mpc.m_steer_rate_lim = steer_rate_lim_dps * deg2rad; + const float64_t wheelbase = + vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo().wheel_base_m; + + /* vehicle model setup */ + const std::string vehicle_model_type = declare_parameter("vehicle_model_type"); + std::shared_ptr vehicle_model_ptr; + if (vehicle_model_type == "kinematics") { + vehicle_model_ptr = + std::make_shared( + wheelbase, m_mpc.m_steer_lim, + m_mpc.m_param.steer_tau); + } else if (vehicle_model_type == "kinematics_no_delay") { + vehicle_model_ptr = std::make_shared( + wheelbase, m_mpc.m_steer_lim); + } else if (vehicle_model_type == "dynamics") { + const float64_t mass_fl = declare_parameter("vehicle.mass_fl"); + const float64_t mass_fr = declare_parameter("vehicle.mass_fr"); + const float64_t mass_rl = declare_parameter("vehicle.mass_rl"); + const float64_t mass_rr = declare_parameter("vehicle.mass_rr"); + const float64_t cf = declare_parameter("vehicle.cf"); + const float64_t cr = declare_parameter("vehicle.cr"); + + // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time // NOLINT + vehicle_model_ptr = std::make_shared( + wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); + } else { + RCLCPP_ERROR(get_logger(), "vehicle_model_type is undefined"); + } + + /* QP solver setup */ + const std::string qp_solver_type = declare_parameter("qp_solver_type"); + std::shared_ptr qpsolver_ptr; + if (qp_solver_type == "unconstraint_fast") { + qpsolver_ptr = std::make_shared(); + } else if (qp_solver_type == "osqp") { + qpsolver_ptr = std::make_shared(get_logger()); + } else { + RCLCPP_ERROR(get_logger(), "qp_solver_type is undefined"); + } + + /* delay compensation */ + { + const float64_t delay_tmp = declare_parameter("input_delay"); + const float64_t delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); + m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period; + m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + } + + /* initialize lowpass filter */ + { + const float64_t steering_lpf_cutoff_hz = + declare_parameter("steering_lpf_cutoff_hz"); + const float64_t error_deriv_lpf_cutoff_hz = + declare_parameter("error_deriv_lpf_cutoff_hz"); + m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + } + + /* set up ros system */ + initTimer(m_mpc.m_ctrl_period); + + m_pub_ctrl_cmd = + create_publisher( + "~/output/control_cmd", 1); + m_pub_predicted_traj = + create_publisher( + "~/output/predicted_trajectory", + 1); + m_pub_diagnostic = + create_publisher( + "~/output/diagnostic", 1); + m_sub_ref_path = create_subscription( + "~/input/reference_trajectory", rclcpp::QoS{1}, + std::bind(&LateralController::onTrajectory, this, _1)); + m_sub_steering = create_subscription( + "~/input/current_steering", rclcpp::QoS{1}, std::bind( + &LateralController::onSteering, this, _1)); + m_sub_odometry = create_subscription( + "~/input/current_odometry", rclcpp::QoS{1}, std::bind( + &LateralController::onOdometry, this, _1)); + + // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations + declareMPCparameters(); + + /* get parameter updates */ + m_set_param_res = + this->add_on_set_parameters_callback(std::bind(&LateralController::paramCallback, this, _1)); + + m_mpc.setQPSolver(qpsolver_ptr); + m_mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + + m_mpc.setLogger(get_logger()); + m_mpc.setClock(get_clock()); +} + +LateralController::~LateralController() +{ + autoware_auto_control_msgs::msg::AckermannLateralCommand stop_cmd = getStopControlCommand(); + publishCtrlCmd(stop_cmd); +} + +void LateralController::onTimer() +{ + if (!checkData() || !updateCurrentPose()) { + return; + } + + autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; + autoware_auto_planning_msgs::msg::Trajectory predicted_traj; + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic diagnostic; + + if (!m_is_ctrl_cmd_prev_initialized) { + m_ctrl_cmd_prev = getInitialControlCommand(); + m_is_ctrl_cmd_prev_initialized = true; + } + + const bool8_t is_mpc_solved = m_mpc.calculateMPC( + *m_current_steering_ptr, m_current_odometry_ptr->twist.twist.linear.x, + m_current_pose_ptr->pose, ctrl_cmd, predicted_traj, diagnostic); + + if (isStoppedState()) { + // Reset input buffer + for (auto & value : m_mpc.m_input_buffer) { + value = m_ctrl_cmd_prev.steering_tire_angle; + } + // Use previous command value as previous raw steer command + m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; + + publishCtrlCmd(m_ctrl_cmd_prev); + publishPredictedTraj(predicted_traj); + publishDiagnostic(diagnostic); + return; + } + + if (!is_mpc_solved) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000 /*ms*/, + "MPC is not solved. publish 0 velocity."); + ctrl_cmd = getStopControlCommand(); + } + + m_ctrl_cmd_prev = ctrl_cmd; + publishCtrlCmd(ctrl_cmd); + publishPredictedTraj(predicted_traj); + publishDiagnostic(diagnostic); +} + +bool8_t LateralController::checkData() const +{ + if (!m_mpc.hasVehicleModel()) { + RCLCPP_DEBUG( + get_logger(), "MPC does not have a vehicle model"); + return false; + } + if (!m_mpc.hasQPSolver()) { + RCLCPP_DEBUG( + get_logger(), "MPC does not have a QP solver"); + return false; + } + + if (!m_current_odometry_ptr) { + RCLCPP_DEBUG( + get_logger(), "waiting data. current_velocity = %d", + m_current_odometry_ptr != nullptr); + return false; + } + + if (!m_current_steering_ptr) { + RCLCPP_DEBUG( + get_logger(), "waiting data. current_steering = %d", + m_current_steering_ptr != nullptr); + return false; + } + + if (m_mpc.m_ref_traj.size() == 0) { + RCLCPP_DEBUG(get_logger(), "trajectory size is zero."); + return false; + } + + return true; +} + +void LateralController::onTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + m_current_trajectory_ptr = msg; + + if (!m_current_pose_ptr && !updateCurrentPose()) { + RCLCPP_DEBUG(get_logger(), "Current pose is not received yet."); + return; + } + + if (msg->points.size() < 3) { + RCLCPP_DEBUG(get_logger(), "received path size is < 3, not enough."); + return; + } + + if (!isValidTrajectory(*msg)) { + RCLCPP_ERROR(get_logger(), "Trajectory is invalid!! stop computing."); + return; + } + + m_mpc.setReferenceTrajectory( + *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, + m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, m_current_pose_ptr); +} + +bool8_t LateralController::updateCurrentPose() +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = m_tf_buffer.lookupTransform( + m_current_trajectory_ptr->header.frame_id, + "base_link", + tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000 /*ms*/, + ex.what()); + RCLCPP_WARN_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000 /*ms*/, + m_tf_buffer.allFramesAsString().c_str()); + return false; + } + + geometry_msgs::msg::PoseStamped ps; + ps.header = transform.header; + ps.pose.position.x = transform.transform.translation.x; + ps.pose.position.y = transform.transform.translation.y; + ps.pose.position.z = transform.transform.translation.z; + ps.pose.orientation = transform.transform.rotation; + m_current_pose_ptr = std::make_shared(ps); + return true; +} + +void LateralController::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + m_current_odometry_ptr = msg; +} + +void LateralController::onSteering( + const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) +{ + m_current_steering_ptr = msg; +} + +autoware_auto_control_msgs::msg::AckermannLateralCommand LateralController::getStopControlCommand() +const +{ + autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); + cmd.steering_tire_rotation_rate = 0.0; + return cmd; +} + +autoware_auto_control_msgs::msg::AckermannLateralCommand LateralController::getInitialControlCommand() +const +{ + autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + cmd.steering_tire_angle = m_current_steering_ptr->steering_tire_angle; + cmd.steering_tire_rotation_rate = 0.0; + return cmd; +} + +bool8_t LateralController::isStoppedState() const +{ + // Note: This function used to take into account the distance to the stop line + // for the stop state judgement. However, it has been removed since the steering + // control was turned off when approaching/exceeding the stop line on a curve or + // emergency stop situation and it caused large tracking error. + const int64_t nearest = trajectory_follower::MPCUtils::calcNearestIndex( + *m_current_trajectory_ptr, + m_current_pose_ptr->pose); + + // If the nearest index is not found, return false + if (nearest < 0) {return false;} + + const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x; + const float64_t target_vel = + m_current_trajectory_ptr->points.at(static_cast(nearest)).longitudinal_velocity_mps; + if ( + std::fabs(current_vel) < m_stop_state_entry_ego_speed && + std::fabs(target_vel) < m_stop_state_entry_target_speed) + { + return true; + } else { + return false; + } +} + +void LateralController::publishCtrlCmd( + autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd) +{ + ctrl_cmd.stamp = this->now(); + m_pub_ctrl_cmd->publish(ctrl_cmd); + m_steer_cmd_prev = ctrl_cmd.steering_tire_angle; +} + +void LateralController::publishPredictedTraj( + autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) +const +{ + predicted_traj.header.stamp = this->now(); + predicted_traj.header.frame_id = m_current_trajectory_ptr->header.frame_id; + m_pub_predicted_traj->publish(predicted_traj); +} + +void LateralController::publishDiagnostic( + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) +const +{ + diagnostic.diag_header.data_stamp = this->now(); + diagnostic.diag_header.name = std::string("linear-MPC lateral controller"); + m_pub_diagnostic->publish(diagnostic); +} + +void LateralController::initTimer(float64_t period_s) +{ + auto timer_callback = std::bind(&LateralController::onTimer, this); + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration( + period_s)); + m_timer = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(m_timer, nullptr); +} + +void LateralController::declareMPCparameters() +{ + m_mpc.m_param.prediction_horizon = declare_parameter("mpc_prediction_horizon"); + m_mpc.m_param.prediction_dt = declare_parameter("mpc_prediction_dt"); + m_mpc.m_param.weight_lat_error = declare_parameter("mpc_weight_lat_error"); + m_mpc.m_param.weight_heading_error = + declare_parameter("mpc_weight_heading_error"); + m_mpc.m_param.weight_heading_error_squared_vel = declare_parameter( + "mpc_weight_heading_error_squared_vel"); + m_mpc.m_param.weight_steering_input = + declare_parameter("mpc_weight_steering_input"); + m_mpc.m_param.weight_steering_input_squared_vel = declare_parameter( + "mpc_weight_steering_input_squared_vel"); + m_mpc.m_param.weight_lat_jerk = declare_parameter("mpc_weight_lat_jerk"); + m_mpc.m_param.weight_steer_rate = declare_parameter("mpc_weight_steer_rate"); + m_mpc.m_param.weight_steer_acc = declare_parameter("mpc_weight_steer_acc"); + m_mpc.m_param.low_curvature_weight_lat_error = declare_parameter( + "mpc_low_curvature_weight_lat_error"); + m_mpc.m_param.low_curvature_weight_heading_error = declare_parameter( + "mpc_low_curvature_weight_heading_error"); + m_mpc.m_param.low_curvature_weight_heading_error_squared_vel = declare_parameter( + "mpc_low_curvature_weight_heading_error_squared_vel"); + m_mpc.m_param.low_curvature_weight_steering_input = declare_parameter( + "mpc_low_curvature_weight_steering_input"); + m_mpc.m_param.low_curvature_weight_steering_input_squared_vel = declare_parameter( + "mpc_low_curvature_weight_steering_input_squared_vel"); + m_mpc.m_param.low_curvature_weight_lat_jerk = declare_parameter( + "mpc_low_curvature_weight_lat_jerk"); + m_mpc.m_param.low_curvature_weight_steer_rate = declare_parameter( + "mpc_low_curvature_weight_steer_rate"); + m_mpc.m_param.low_curvature_weight_steer_acc = declare_parameter( + "mpc_low_curvature_weight_steer_acc"); + m_mpc.m_param.low_curvature_thresh_curvature = declare_parameter( + "mpc_low_curvature_thresh_curvature"); + m_mpc.m_param.weight_terminal_lat_error = + declare_parameter("mpc_weight_terminal_lat_error"); + m_mpc.m_param.weight_terminal_heading_error = declare_parameter( + "mpc_weight_terminal_heading_error"); + m_mpc.m_param.zero_ff_steer_deg = declare_parameter("mpc_zero_ff_steer_deg"); + m_mpc.m_param.acceleration_limit = declare_parameter("mpc_acceleration_limit"); + m_mpc.m_param.velocity_time_constant = + declare_parameter("mpc_velocity_time_constant"); +} + +rcl_interfaces::msg::SetParametersResult LateralController::paramCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + // strong exception safety wrt MPCParam + trajectory_follower::MPCParam param = m_mpc.m_param; + try { + update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon); + update_param(parameters, "mpc_prediction_dt", param.prediction_dt); + update_param(parameters, "mpc_weight_lat_error", param.weight_lat_error); + update_param(parameters, "mpc_weight_heading_error", param.weight_heading_error); + update_param( + parameters, "mpc_weight_heading_error_squared_vel", + param.weight_heading_error_squared_vel); + update_param(parameters, "mpc_weight_steering_input", param.weight_steering_input); + update_param( + parameters, "mpc_weight_steering_input_squared_vel", + param.weight_steering_input_squared_vel); + update_param(parameters, "mpc_weight_lat_jerk", param.weight_lat_jerk); + update_param(parameters, "mpc_weight_steer_rate", param.weight_steer_rate); + update_param(parameters, "mpc_weight_steer_acc", param.weight_steer_acc); + update_param( + parameters, "mpc_low_curvature_weight_lat_error", + param.low_curvature_weight_lat_error); + update_param( + parameters, "mpc_low_curvature_weight_heading_error", + param.low_curvature_weight_heading_error); + update_param( + parameters, "mpc_low_curvature_weight_heading_error_squared_vel", + param.low_curvature_weight_heading_error_squared_vel); + update_param( + parameters, "mpc_low_curvature_weight_steering_input", + param.low_curvature_weight_steering_input); + update_param( + parameters, "mpc_low_curvature_weight_steering_input_squared_vel", + param.low_curvature_weight_steering_input_squared_vel); + update_param( + parameters, "mpc_low_curvature_weight_lat_jerk", + param.low_curvature_weight_lat_jerk); + update_param( + parameters, "mpc_low_curvature_weight_steer_rate", + param.low_curvature_weight_steer_rate); + update_param( + parameters, "mpc_low_curvature_weight_steer_acc", + param.low_curvature_weight_steer_acc); + update_param( + parameters, "mpc_low_curvature_thresh_curvature", + param.low_curvature_thresh_curvature); + update_param(parameters, "mpc_weight_terminal_lat_error", param.weight_terminal_lat_error); + update_param( + parameters, "mpc_weight_terminal_heading_error", + param.weight_terminal_heading_error); + update_param(parameters, "mpc_zero_ff_steer_deg", param.zero_ff_steer_deg); + update_param(parameters, "mpc_acceleration_limit", param.acceleration_limit); + update_param(parameters, "mpc_velocity_time_constant", param.velocity_time_constant); + + // initialize input buffer + update_param(parameters, "input_delay", param.input_delay); + const float64_t delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); + const float64_t delay = delay_step * m_mpc.m_ctrl_period; + if (param.input_delay != delay) { + param.input_delay = delay; + m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + } + + // transaction succeeds, now assign values + m_mpc.m_param = param; + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + + return result; +} + +bool8_t LateralController::isValidTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & traj) const +{ + for (const auto & p : traj.points) { + if ( + !isfinite(p.pose.position.x) || !isfinite(p.pose.position.y) || + !isfinite(p.pose.orientation.w) || !isfinite(p.pose.orientation.x) || + !isfinite(p.pose.orientation.y) || !isfinite(p.pose.orientation.z) || + !isfinite(p.longitudinal_velocity_mps) || !isfinite(p.lateral_velocity_mps) || + !isfinite(p.lateral_velocity_mps) || !isfinite(p.heading_rate_rps) || + !isfinite(p.front_wheel_angle_rad) || !isfinite(p.rear_wheel_angle_rad)) + { + return false; + } + } + return true; +} + +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::motion::control::trajectory_follower_nodes::LateralController) diff --git a/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp b/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp new file mode 100644 index 0000000000000..f083615312feb --- /dev/null +++ b/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp @@ -0,0 +1,102 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 +#include + +#include "trajectory_follower_nodes/latlon_muxer_node.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower_nodes +{ + +LatLonMuxer::LatLonMuxer(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("latlon_muxer", node_options) +{ + m_control_cmd_pub = + create_publisher( + "~/output/control_cmd", + rclcpp::QoS{1}.transient_local()); + m_lat_control_cmd_sub = + create_subscription( + "~/input/lateral/control_cmd", rclcpp::QoS{1}, + std::bind(&LatLonMuxer::latCtrlCmdCallback, this, std::placeholders::_1)); + m_lon_control_cmd_sub = + create_subscription( + "~/input/longitudinal/control_cmd", rclcpp::QoS{1}, + std::bind(&LatLonMuxer::lonCtrlCmdCallback, this, std::placeholders::_1)); + m_timeout_thr_sec = declare_parameter("timeout_thr_sec"); +} + +bool LatLonMuxer::checkTimeout() +{ + const auto now = this->now(); + if ((now - m_lat_cmd->stamp).seconds() > m_timeout_thr_sec) { + RCLCPP_ERROR_THROTTLE( + get_logger(), + *get_clock(), 1000 /*ms*/, + "Lateral control command too old, muxed command will not be published."); + return false; + } + if ((now - m_lon_cmd->stamp).seconds() > m_timeout_thr_sec) { + RCLCPP_ERROR_THROTTLE( + get_logger(), + *get_clock(), 1000 /*ms*/, + "Longitudinal control command too old, muxed command will not be published."); + return false; + } + return true; +} + +void LatLonMuxer::publishCmd() +{ + if (!m_lat_cmd || !m_lon_cmd) { + return; + } + if (!checkTimeout()) { + return; + } + + autoware_auto_control_msgs::msg::AckermannControlCommand out; + out.stamp = this->now(); + out.lateral = *m_lat_cmd; + out.longitudinal = *m_lon_cmd; + + m_control_cmd_pub->publish(out); +} + +void LatLonMuxer::latCtrlCmdCallback( + const autoware_auto_control_msgs::msg::AckermannLateralCommand::SharedPtr input_msg) +{ + m_lat_cmd = input_msg; + publishCmd(); +} + +void LatLonMuxer::lonCtrlCmdCallback( + const autoware_auto_control_msgs::msg::LongitudinalCommand::SharedPtr input_msg) +{ + m_lon_cmd = std::make_shared(*input_msg); + publishCmd(); +} +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_nodes::LatLonMuxer) diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp new file mode 100644 index 0000000000000..dc97d14f34c9e --- /dev/null +++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -0,0 +1,971 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 +#include +#include +#include +#include +#include + +#include "trajectory_follower_nodes/longitudinal_controller_node.hpp" + +#include "motion_common/motion_common.hpp" +#include "motion_common/trajectory_common.hpp" +#include "time_utils/time_utils.hpp" + + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower_nodes +{ +LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_options) +: Node("longitudinal_controller", node_options) +{ + using std::placeholders::_1; + + // parameters timer + m_control_rate = declare_parameter("control_rate"); + + m_wheel_base = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo().wheel_base_m; + + // parameters for delay compensation + m_delay_compensation_time = declare_parameter("delay_compensation_time"); // [s] + + // parameters to enable functions + m_enable_smooth_stop = declare_parameter("enable_smooth_stop"); + m_enable_overshoot_emergency = declare_parameter("enable_overshoot_emergency"); + m_enable_slope_compensation = declare_parameter("enable_slope_compensation"); + + // parameters for state transition + { + auto & p = m_state_transition_params; + // drive + p.drive_state_stop_dist = declare_parameter("drive_state_stop_dist"); // [m] + p.drive_state_offset_stop_dist = declare_parameter( + "drive_state_offset_stop_dist"); // [m] + // stopping + p.stopping_state_stop_dist = + declare_parameter("stopping_state_stop_dist"); // [m] + // stop + p.stopped_state_entry_vel = + declare_parameter("stopped_state_entry_vel"); // [m/s] + p.stopped_state_entry_acc = + declare_parameter("stopped_state_entry_acc"); // [m/s²] + // emergency + p.emergency_state_overshoot_stop_dist = declare_parameter( + "emergency_state_overshoot_stop_dist"); // [m] + p.emergency_state_traj_trans_dev = declare_parameter( + "emergency_state_traj_trans_dev"); // [m] + p.emergency_state_traj_rot_dev = declare_parameter( + "emergency_state_traj_rot_dev"); // [m] + } + + // parameters for drive state + { + // initialize PID gain + const float64_t kp{declare_parameter("kp")}; + const float64_t ki{declare_parameter("ki")}; + const float64_t kd{declare_parameter("kd")}; + m_pid_vel.setGains(kp, ki, kd); + + // initialize PID limits + const float64_t max_pid{declare_parameter("max_out")}; // [m/s^2] + const float64_t min_pid{declare_parameter("min_out")}; // [m/s^2] + const float64_t max_p{declare_parameter("max_p_effort")}; // [m/s^2] + const float64_t min_p{declare_parameter("min_p_effort")}; // [m/s^2] + const float64_t max_i{declare_parameter("max_i_effort")}; // [m/s^2] + const float64_t min_i{declare_parameter("min_i_effort")}; // [m/s^2] + const float64_t max_d{declare_parameter("max_d_effort")}; // [m/s^2] + const float64_t min_d{declare_parameter("min_d_effort")}; // [m/s^2] + m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); + + // set lowpass filter for vel error and pitch + const float64_t lpf_vel_error_gain{declare_parameter("lpf_vel_error_gain")}; + m_lpf_vel_error = + std::make_shared(0.0, lpf_vel_error_gain); + + m_current_vel_threshold_pid_integrate = declare_parameter( + "current_vel_threshold_pid_integration"); // [m/s] + + m_enable_brake_keeping_before_stop = + declare_parameter("enable_brake_keeping_before_stop"); // [-] + m_brake_keeping_acc = declare_parameter("brake_keeping_acc"); // [m/s^2] + } + + // parameters for smooth stop state + { + const float64_t max_strong_acc{declare_parameter( + "smooth_stop_max_strong_acc")}; // [m/s^2] + const float64_t min_strong_acc{declare_parameter( + "smooth_stop_min_strong_acc")}; // [m/s^2] + const float64_t weak_acc{declare_parameter( + "smooth_stop_weak_acc")}; // [m/s^2] + const float64_t weak_stop_acc{declare_parameter( + "smooth_stop_weak_stop_acc")}; // [m/s^2] + const float64_t strong_stop_acc{declare_parameter( + "smooth_stop_strong_stop_acc")}; // [m/s^2] + + const float64_t max_fast_vel{declare_parameter( + "smooth_stop_max_fast_vel")}; // [m/s] + const float64_t min_running_vel{declare_parameter( + "smooth_stop_min_running_vel")}; // [m/s] + const float64_t min_running_acc{declare_parameter( + "smooth_stop_min_running_acc")}; // [m/s^2] + const float64_t weak_stop_time{declare_parameter( + "smooth_stop_weak_stop_time")}; // [s] + + const float64_t weak_stop_dist{declare_parameter( + "smooth_stop_weak_stop_dist")}; // [m] + const float64_t strong_stop_dist{declare_parameter( + "smooth_stop_strong_stop_dist")}; // [m] + + m_smooth_stop.setParams( + max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, + min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist); + } + + // parameters for stop state + { + auto & p = m_stopped_state_params; + p.vel = declare_parameter("stopped_vel"); // [m/s] + p.acc = declare_parameter("stopped_acc"); // [m/s^2] + p.jerk = declare_parameter("stopped_jerk"); // [m/s^3] + } + + // parameters for emergency state + { + auto & p = m_emergency_state_params; + p.vel = declare_parameter("emergency_vel"); // [m/s] + p.acc = declare_parameter("emergency_acc"); // [m/s^2] + p.jerk = declare_parameter("emergency_jerk"); // [m/s^3] + } + + // parameters for acceleration limit + m_max_acc = declare_parameter("max_acc"); // [m/s^2] + m_min_acc = declare_parameter("min_acc"); // [m/s^2] + + // parameters for jerk limit + m_max_jerk = declare_parameter("max_jerk"); // [m/s^3] + m_min_jerk = declare_parameter("min_jerk"); // [m/s^3] + + // parameters for slope compensation + m_use_traj_for_pitch = declare_parameter("use_trajectory_for_pitch_calculation"); + const float64_t lpf_pitch_gain{declare_parameter("lpf_pitch_gain")}; + m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); + m_max_pitch_rad = declare_parameter("max_pitch_rad"); // [rad] + m_min_pitch_rad = declare_parameter("min_pitch_rad"); // [rad] + + // subscriber, publisher + m_sub_current_velocity = create_subscription( + "~/input/current_odometry", rclcpp::QoS{1}, + std::bind(&LongitudinalController::callbackCurrentVelocity, this, _1)); + m_sub_trajectory = create_subscription( + "~/input/current_trajectory", rclcpp::QoS{1}, + std::bind(&LongitudinalController::callbackTrajectory, this, _1)); + m_pub_control_cmd = create_publisher( + "~/output/control_cmd", rclcpp::QoS{1}); + m_pub_slope = create_publisher( + "~/output/slope_angle", rclcpp::QoS{1}); + m_pub_debug = create_publisher( + "~/output/diagnostic", rclcpp::QoS{1}); + + + // Timer + { + auto timer_callback = std::bind(&LongitudinalController::callbackTimerControl, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(1.0 / m_control_rate)); + m_timer_control = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(m_timer_control, nullptr); + } + + // set parameter callback + m_set_param_res = + this->add_on_set_parameters_callback( + std::bind( + &LongitudinalController::paramCallback, this, + _1)); + + // set lowpass filter for acc + m_lpf_acc = std::make_shared(0.0, 0.2); +} + +void LongitudinalController::callbackCurrentVelocity( + const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + if (m_current_velocity_ptr) { + m_prev_velocity_ptr = m_current_velocity_ptr; + } + m_current_velocity_ptr = std::make_shared(*msg); +} + +void LongitudinalController::callbackTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +{ + if (!trajectory_follower::longitudinal_utils::isValidTrajectory(*msg)) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 3000, + "received invalid trajectory. ignore."); + return; + } + + if (msg->points.size() < 2) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, + "Unexpected trajectory size < 2. Ignored."); + return; + } + + m_trajectory_ptr = std::make_shared(*msg); +} + +rcl_interfaces::msg::SetParametersResult LongitudinalController::paramCallback( + const std::vector & parameters) +{ + auto update_param = [&](const std::string & name, float64_t & 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; + }; + + // delay compensation + update_param("delay_compensation_time", m_delay_compensation_time); + + // state transition + { + auto & p = m_state_transition_params; + update_param("drive_state_stop_dist", p.drive_state_stop_dist); + update_param("stopping_state_stop_dist", p.stopping_state_stop_dist); + update_param("stopped_state_entry_vel", p.stopped_state_entry_vel); + update_param("stopped_state_entry_acc", p.stopped_state_entry_acc); + update_param("emergency_state_overshoot_stop_dist", p.emergency_state_overshoot_stop_dist); + update_param("emergency_state_traj_trans_dev", p.emergency_state_traj_trans_dev); + update_param("emergency_state_traj_rot_dev", p.emergency_state_traj_rot_dev); + } + + // drive state + { + float64_t kp{get_parameter("kp").as_double()}; + float64_t ki{get_parameter("ki").as_double()}; + float64_t kd{get_parameter("kd").as_double()}; + update_param("kp", kp); + update_param("ki", ki); + update_param("kd", kd); + m_pid_vel.setGains(kp, ki, kd); + + float64_t max_pid{get_parameter("max_out").as_double()}; + float64_t min_pid{get_parameter("min_out").as_double()}; + float64_t max_p{get_parameter("max_p_effort").as_double()}; + float64_t min_p{get_parameter("min_p_effort").as_double()}; + float64_t max_i{get_parameter("max_i_effort").as_double()}; + float64_t min_i{get_parameter("min_i_effort").as_double()}; + float64_t max_d{get_parameter("max_d_effort").as_double()}; + float64_t min_d{get_parameter("min_d_effort").as_double()}; + update_param("max_out", max_pid); + update_param("min_out", min_pid); + update_param("max_p_effort", max_p); + update_param("min_p_effort", min_p); + update_param("max_i_effort", max_i); + update_param("min_i_effort", min_i); + update_param("max_d_effort", max_d); + update_param("min_d_effort", min_d); + m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); + + update_param("current_vel_threshold_pid_integration", m_current_vel_threshold_pid_integrate); + } + + // stopping state + { + float64_t max_strong_acc{get_parameter("smooth_stop_max_strong_acc").as_double()}; + float64_t min_strong_acc{get_parameter("smooth_stop_min_strong_acc").as_double()}; + float64_t weak_acc{get_parameter("smooth_stop_weak_acc").as_double()}; + float64_t weak_stop_acc{get_parameter("smooth_stop_weak_stop_acc").as_double()}; + float64_t strong_stop_acc{get_parameter("smooth_stop_strong_stop_acc").as_double()}; + float64_t max_fast_vel{get_parameter("smooth_stop_max_fast_vel").as_double()}; + float64_t min_running_vel{get_parameter("smooth_stop_min_running_vel").as_double()}; + float64_t min_running_acc{get_parameter("smooth_stop_min_running_acc").as_double()}; + float64_t weak_stop_time{get_parameter("smooth_stop_weak_stop_time").as_double()}; + float64_t weak_stop_dist{get_parameter("smooth_stop_weak_stop_dist").as_double()}; + float64_t strong_stop_dist{get_parameter("smooth_stop_strong_stop_dist").as_double()}; + update_param("smooth_stop_max_strong_acc", max_strong_acc); + update_param("smooth_stop_min_strong_acc", min_strong_acc); + update_param("smooth_stop_weak_acc", weak_acc); + update_param("smooth_stop_weak_stop_acc", weak_stop_acc); + update_param("smooth_stop_strong_stop_acc", strong_stop_acc); + update_param("smooth_stop_max_fast_vel", max_fast_vel); + update_param("smooth_stop_min_running_vel", min_running_vel); + update_param("smooth_stop_min_running_acc", min_running_acc); + update_param("smooth_stop_weak_stop_time", weak_stop_time); + update_param("smooth_stop_weak_stop_dist", weak_stop_dist); + update_param("smooth_stop_strong_stop_dist", strong_stop_dist); + m_smooth_stop.setParams( + max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, + min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist); + } + + // stop state + { + auto & p = m_stopped_state_params; + update_param("stopped_vel", p.vel); + update_param("stopped_acc", p.acc); + update_param("stopped_jerk", p.jerk); + } + + // emergency state + { + auto & p = m_emergency_state_params; + update_param("emergency_vel", p.vel); + update_param("emergency_acc", p.acc); + update_param("emergency_jerk", p.jerk); + } + + // acceleration limit + update_param("min_acc", m_min_acc); + + // jerk limit + update_param("max_jerk", m_max_jerk); + update_param("min_jerk", m_min_jerk); + + // slope compensation + update_param("max_pitch_rad", m_max_pitch_rad); + update_param("min_pitch_rad", m_min_pitch_rad); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +void LongitudinalController::callbackTimerControl() +{ + // wait for initial pointers + if ( + !m_current_velocity_ptr || !m_prev_velocity_ptr || !m_trajectory_ptr || + !m_tf_buffer.canTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero)) + { + return; + } + + // get current ego pose + geometry_msgs::msg::TransformStamped tf = + m_tf_buffer.lookupTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero); + + // calculate current pose and control data + geometry_msgs::msg::Pose current_pose; + current_pose.position.x = tf.transform.translation.x; + current_pose.position.y = tf.transform.translation.y; + current_pose.position.z = tf.transform.translation.z; + current_pose.orientation = tf.transform.rotation; + + const auto control_data = getControlData(current_pose); + + // self pose is far from trajectory + if (control_data.is_far_from_trajectory) { + m_control_state = ControlState::EMERGENCY; // update control state + const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command + m_prev_raw_ctrl_cmd = raw_ctrl_cmd; + publishCtrlCmd(raw_ctrl_cmd, control_data.current_motion.vel); // publish control command + publishDebugData(raw_ctrl_cmd, control_data); // publish debug data + return; + } + + // update control state + m_control_state = updateControlState(m_control_state, control_data); + + // calculate control command + const Motion ctrl_cmd = calcCtrlCmd(m_control_state, current_pose, control_data); + + // publish control command + publishCtrlCmd(ctrl_cmd, control_data.current_motion.vel); + + // publish debug data + publishDebugData(ctrl_cmd, control_data); +} + +LongitudinalController::ControlData LongitudinalController::getControlData( + const geometry_msgs::msg::Pose & current_pose) +{ + ControlData control_data{}; + + // dt + control_data.dt = getDt(); + + // current velocity and acceleration + control_data.current_motion = getCurrentMotion(); + + // nearest idx + const float64_t max_dist = m_state_transition_params.emergency_state_traj_trans_dev; + const float64_t max_yaw = m_state_transition_params.emergency_state_traj_rot_dev; + const auto nearest_idx_opt = + motion_common::findNearestIndex(m_trajectory_ptr->points, current_pose, max_dist, max_yaw); + + // return here if nearest index is not found + if (!nearest_idx_opt) { + control_data.is_far_from_trajectory = true; + return control_data; + } + control_data.nearest_idx = *nearest_idx_opt; + + // shift + control_data.shift = getCurrentShift(control_data.nearest_idx); + if (control_data.shift != m_prev_shift) {m_pid_vel.reset();} + m_prev_shift = control_data.shift; + + // distance to stopline + control_data.stop_dist = + trajectory_follower::longitudinal_utils::calcStopDistance( + current_pose.position, + *m_trajectory_ptr); + + // pitch + const float64_t raw_pitch = trajectory_follower::longitudinal_utils::getPitchByPose( + current_pose.orientation); + const float64_t traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj( + *m_trajectory_ptr, control_data.nearest_idx, m_wheel_base); + control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); + updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); + + return control_data; +} + +LongitudinalController::Motion LongitudinalController::calcEmergencyCtrlCmd(const float64_t dt) +const +{ + // These accelerations are without slope compensation + const auto & p = m_emergency_state_params; + const float64_t vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + p.vel, m_prev_raw_ctrl_cmd.vel, + dt, p.acc); + const float64_t acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + p.acc, m_prev_raw_ctrl_cmd.acc, + dt, p.jerk); + + auto clock = rclcpp::Clock{RCL_ROS_TIME}; + RCLCPP_ERROR_THROTTLE( + get_logger(), clock, 3000, + "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); + + return Motion{vel, acc}; +} + +LongitudinalController::ControlState LongitudinalController::updateControlState( + const ControlState current_control_state, const ControlData & control_data) +{ + const float64_t current_vel = control_data.current_motion.vel; + const float64_t current_acc = control_data.current_motion.acc; + const float64_t stop_dist = control_data.stop_dist; + + // flags for state transition + const auto & p = m_state_transition_params; + + const bool8_t departure_condition_from_stopping = + stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; + const bool8_t departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; + + const bool8_t stopping_condition = stop_dist < p.stopping_state_stop_dist; + if ( + std::fabs(current_vel) > p.stopped_state_entry_vel || + std::fabs(current_acc) > p.stopped_state_entry_acc) + { + m_last_running_time = std::make_shared(this->now()); + } + const bool8_t stopped_condition = + m_last_running_time ? (this->now() - *m_last_running_time).seconds() > 0.5 : false; + + const bool8_t emergency_condition = + m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist; + + // transit state + if (current_control_state == ControlState::DRIVE) { + if (emergency_condition) { + return ControlState::EMERGENCY; + } + + if (m_enable_smooth_stop) { + if (stopping_condition) { + // predictions after input time delay + const float64_t pred_vel_in_target = + predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); + const float64_t pred_stop_dist = + control_data.stop_dist - + 0.5 * (pred_vel_in_target + current_vel) * m_delay_compensation_time; + m_smooth_stop.init(pred_vel_in_target, pred_stop_dist); + return ControlState::STOPPING; + } + } else { + if (stopped_condition && !departure_condition_from_stopped) { + return ControlState::STOPPED; + } + } + } else if (current_control_state == ControlState::STOPPING) { + if (emergency_condition) { + return ControlState::EMERGENCY; + } + + if (stopped_condition) { + return ControlState::STOPPED; + } + + if (departure_condition_from_stopping) { + m_pid_vel.reset(); + m_lpf_vel_error->reset(0.0); + return ControlState::DRIVE; + } + } else if (current_control_state == ControlState::STOPPED) { + if (departure_condition_from_stopped) { + m_pid_vel.reset(); + m_lpf_vel_error->reset(0.0); + return ControlState::DRIVE; + } + } else if (m_control_state == ControlState::EMERGENCY) { + if (stopped_condition && !emergency_condition) { + return ControlState::STOPPED; + } + } + + return current_control_state; +} + +LongitudinalController::Motion LongitudinalController::calcCtrlCmd( + const ControlState & current_control_state, const geometry_msgs::msg::Pose & current_pose, + const ControlData & control_data) +{ + const size_t nearest_idx = control_data.nearest_idx; + const float64_t current_vel = control_data.current_motion.vel; + const float64_t current_acc = control_data.current_motion.acc; + + // velocity and acceleration command + Motion raw_ctrl_cmd{}; + Motion target_motion{}; + if (current_control_state == ControlState::DRIVE) { + const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( + current_pose, m_delay_compensation_time, current_vel); + const auto target_interpolated_point = + calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose.position, nearest_idx); + target_motion = + Motion{target_interpolated_point.longitudinal_velocity_mps, + target_interpolated_point.acceleration_mps2}; + + target_motion = keepBrakeBeforeStop(*m_trajectory_ptr, target_motion, nearest_idx); + + const float64_t pred_vel_in_target = + predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); + m_debug_values.setValues( + trajectory_follower::DebugValues::TYPE::PREDICTED_VEL, + pred_vel_in_target); + + raw_ctrl_cmd.vel = target_motion.vel; + raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); + RCLCPP_DEBUG( + get_logger(), + "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " + "feedback_ctrl_cmd.ac: %3.3f", + raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, current_vel, target_motion.vel, + raw_ctrl_cmd.acc); + } else if (current_control_state == ControlState::STOPPING) { + raw_ctrl_cmd.acc = m_smooth_stop.calculate( + control_data.stop_dist, current_vel, current_acc, m_vel_hist, m_delay_compensation_time); + raw_ctrl_cmd.vel = m_stopped_state_params.vel; + + RCLCPP_DEBUG( + get_logger(), + "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", + raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); + } else if (current_control_state == ControlState::STOPPED) { + // This acceleration is without slope compensation + const auto & p = m_stopped_state_params; + raw_ctrl_cmd.vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + p.vel, + m_prev_raw_ctrl_cmd.vel, + control_data.dt, p.acc); + raw_ctrl_cmd.acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + p.acc, + m_prev_raw_ctrl_cmd.acc, + control_data.dt, p.jerk); + + RCLCPP_DEBUG( + get_logger(), "[Stopped]. vel: %3.3f, acc: %3.3f", + raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); + } else if (current_control_state == ControlState::EMERGENCY) { + raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); + } + + // store acceleration without slope compensation + m_prev_raw_ctrl_cmd = raw_ctrl_cmd; + + // apply slope compensation and filter acceleration and jerk + const float64_t filtered_acc_cmd = calcFilteredAcc(raw_ctrl_cmd.acc, control_data); + const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd}; + + // update debug visualization + updateDebugVelAcc(target_motion, current_pose, control_data); + + return filtered_ctrl_cmd; +} + +// Do not use nearest_idx here +void LongitudinalController::publishCtrlCmd(const Motion & ctrl_cmd, float64_t current_vel) +{ + // publish control command + autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; + cmd.stamp = this->now(); + cmd.speed = static_cast(ctrl_cmd.vel); + cmd.acceleration = static_cast(ctrl_cmd.acc); + m_pub_control_cmd->publish(cmd); + + // store current velocity history + m_vel_hist.push_back({this->now(), current_vel}); + while (m_vel_hist.size() > static_cast(m_control_rate * 0.5)) { + m_vel_hist.erase(m_vel_hist.begin()); + } + + m_prev_ctrl_cmd = ctrl_cmd; +} + +void LongitudinalController::publishDebugData( + const Motion & ctrl_cmd, const ControlData & control_data) +{ + using trajectory_follower::DebugValues; + // set debug values + m_debug_values.setValues(DebugValues::TYPE::DT, control_data.dt); + m_debug_values.setValues(DebugValues::TYPE::CALCULATED_ACC, control_data.current_motion.acc); + m_debug_values.setValues(DebugValues::TYPE::SHIFT, static_cast(control_data.shift)); + m_debug_values.setValues(DebugValues::TYPE::STOP_DIST, control_data.stop_dist); + m_debug_values.setValues( + DebugValues::TYPE::CONTROL_STATE, + static_cast(m_control_state)); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc); + + // publish debug values + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic debug_msg{}; + debug_msg.diag_header.data_stamp = this->now(); + for (const auto & v : m_debug_values.getValues()) { + debug_msg.diag_array.data.push_back( + static_cast(v)); + } + m_pub_debug->publish(debug_msg); + + // slope angle + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic slope_msg{}; + slope_msg.diag_header.data_stamp = this->now(); + slope_msg.diag_array.data.push_back( + static_cast( + control_data.slope_angle)); + m_pub_slope->publish(slope_msg); +} + +float64_t LongitudinalController::getDt() +{ + float64_t dt; + if (!m_prev_control_time) { + dt = 1.0 / m_control_rate; + m_prev_control_time = std::make_shared(this->now()); + } else { + dt = (this->now() - *m_prev_control_time).seconds(); + *m_prev_control_time = this->now(); + } + const float64_t max_dt = 1.0 / m_control_rate * 2.0; + const float64_t min_dt = 1.0 / m_control_rate * 0.5; + return std::max(std::min(dt, max_dt), min_dt); +} + +LongitudinalController::Motion LongitudinalController::getCurrentMotion() const +{ + const float64_t dv = m_current_velocity_ptr->twist.twist.linear.x - + m_prev_velocity_ptr->twist.twist.linear.x; + const float64_t dt = + std::max( + (rclcpp::Time(m_current_velocity_ptr->header.stamp) - + rclcpp::Time(m_prev_velocity_ptr->header.stamp)).seconds(), 1e-03); + const float64_t accel = dv / dt; + + const float64_t current_vel = m_current_velocity_ptr->twist.twist.linear.x; + const float64_t current_acc = m_lpf_acc->filter(accel); + + return Motion{current_vel, current_acc}; +} + +enum LongitudinalController::Shift LongitudinalController::getCurrentShift(const size_t nearest_idx) +const +{ + constexpr float64_t epsilon = 1e-5; + + const float64_t target_vel = m_trajectory_ptr->points.at(nearest_idx).longitudinal_velocity_mps; + + if (target_vel > epsilon) { + return Shift::Forward; + } else if (target_vel < -epsilon) { + return Shift::Reverse; + } + + return m_prev_shift; +} + +float64_t LongitudinalController::calcFilteredAcc( + const float64_t raw_acc, + const ControlData & control_data) +{ + using trajectory_follower::DebugValues; + const float64_t acc_max_filtered = ::motion::motion_common::clamp(raw_acc, m_min_acc, m_max_acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered); + + // store ctrl cmd without slope filter + storeAccelCmd(acc_max_filtered); + + const float64_t acc_slope_filtered = + applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); + + // This jerk filter must be applied after slope compensation + const float64_t acc_jerk_filtered = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); + + return acc_jerk_filtered; +} + +void LongitudinalController::storeAccelCmd(const float64_t accel) +{ + if (m_control_state == ControlState::DRIVE) { + // convert format + autoware_auto_control_msgs::msg::LongitudinalCommand cmd; + cmd.stamp = this->now(); + cmd.acceleration = static_cast(accel); + + // store published ctrl cmd + m_ctrl_cmd_vec.emplace_back(cmd); + } else { + // reset command + m_ctrl_cmd_vec.clear(); + } + + // remove unused ctrl cmd + if (m_ctrl_cmd_vec.size() <= 2) { + return; + } + if ( + (this->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) + { + m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); + } +} + +float64_t LongitudinalController::applySlopeCompensation( + const float64_t input_acc, const float64_t pitch, const Shift shift) const +{ + if (!m_enable_slope_compensation) { + return input_acc; + } + const float64_t pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad); + + // Acceleration command is always positive independent of direction (= shift) when car is running + float64_t sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0); + float64_t compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited); + return compensated_acc; +} + +LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, + const size_t nearest_idx) const +{ + Motion output_motion = target_motion; + + if (m_enable_brake_keeping_before_stop == false) { + return output_motion; + } + // const auto stop_idx = autoware_utils::searchZeroVelocityIndex(traj.points); + const auto stop_idx = motion_common::searchZeroVelocityIndex(traj.points); + if (!stop_idx) { + return output_motion; + } + + float64_t min_acc_before_stop = std::numeric_limits::max(); + size_t min_acc_idx = std::numeric_limits::max(); + for (int i = static_cast(*stop_idx); i >= 0; --i) { + const auto ui = static_cast(i); + if (traj.points.at(ui).acceleration_mps2 > static_cast(min_acc_before_stop)) { + break; + } + min_acc_before_stop = traj.points.at(ui).acceleration_mps2; + min_acc_idx = ui; + } + + const float64_t brake_keeping_acc = std::max(m_brake_keeping_acc, min_acc_before_stop); + if (nearest_idx >= min_acc_idx && target_motion.acc > brake_keeping_acc) { + output_motion.acc = brake_keeping_acc; + } + + return output_motion; +} + +autoware_auto_planning_msgs::msg::TrajectoryPoint LongitudinalController:: +calcInterpolatedTargetValue( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const geometry_msgs::msg::Point & point, + const size_t nearest_idx) const +{ + if (traj.points.size() == 1) { + return traj.points.at(0); + } + + // If the current position is not within the reference trajectory, enable the edge value. + // Else, apply linear interpolation + if (nearest_idx == 0) { + if (motion_common::calcSignedArcLength(traj.points, point, 0) > 0) { + return traj.points.at(0); + } + } + if (nearest_idx == traj.points.size() - 1) { + if (motion_common::calcSignedArcLength(traj.points, point, traj.points.size() - 1) < 0) { + return traj.points.at(traj.points.size() - 1); + } + } + + // apply linear interpolation + return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint(traj.points, point); +} + +float64_t LongitudinalController::predictedVelocityInTargetPoint( + const Motion current_motion, const float64_t delay_compensation_time) const +{ + const float64_t current_vel = current_motion.vel; + const float64_t current_acc = current_motion.acc; + + if (std::fabs(current_vel) < 1e-01) { // when velocity is low, no prediction + return current_vel; + } + + const float64_t current_vel_abs = std::fabs(current_vel); + if (m_ctrl_cmd_vec.size() == 0) { + const float64_t pred_vel = current_vel + current_acc * delay_compensation_time; + // avoid to change sign of current_vel and pred_vel + return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; + } + + float64_t pred_vel = current_vel_abs; + + const auto past_delay_time = + this->now() - rclcpp::Duration::from_seconds(delay_compensation_time); + for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) { + if ( + (this->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < + m_delay_compensation_time) + { + if (i == 0) { + // size of m_ctrl_cmd_vec is less than m_delay_compensation_time + pred_vel = + current_vel_abs + static_cast(m_ctrl_cmd_vec.at(i).acceleration) * + delay_compensation_time; + return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; + } + // add velocity to accel * dt + const float64_t acc = m_ctrl_cmd_vec.at(i - 1).acceleration; + const auto curr_time_i = rclcpp::Time(m_ctrl_cmd_vec.at(i).stamp); + const float64_t time_to_next_acc = std::min( + (curr_time_i - rclcpp::Time(m_ctrl_cmd_vec.at(i - 1).stamp)).seconds(), + (curr_time_i - past_delay_time).seconds()); + pred_vel += acc * time_to_next_acc; + } + } + + const float64_t last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; + const float64_t time_to_current = + (this->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); + pred_vel += last_acc * time_to_current; + + // avoid to change sign of current_vel and pred_vel + return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; +} + +float64_t LongitudinalController::applyVelocityFeedback( + const Motion target_motion, const float64_t dt, const float64_t current_vel) +{ + using trajectory_follower::DebugValues; + const float64_t current_vel_abs = std::fabs(current_vel); + const float64_t target_vel_abs = std::fabs(target_motion.vel); + const bool8_t enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate); + const float64_t error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); + + std::vector pid_contributions(3); + const float64_t pid_acc = + m_pid_vel.calculate(error_vel_filtered, dt, enable_integration, pid_contributions); + const float64_t feedback_acc = target_motion.acc + pid_acc; + + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PID_APPLIED, feedback_acc); + m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL_FILTERED, error_vel_filtered); + m_debug_values.setValues( + DebugValues::TYPE::ACC_CMD_FB_P_CONTRIBUTION, pid_contributions.at(0)); // P + m_debug_values.setValues( + DebugValues::TYPE::ACC_CMD_FB_I_CONTRIBUTION, pid_contributions.at(1)); // I + m_debug_values.setValues( + DebugValues::TYPE::ACC_CMD_FB_D_CONTRIBUTION, pid_contributions.at(2)); // D + + return feedback_acc; +} + +void LongitudinalController::updatePitchDebugValues( + const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch) +{ + using trajectory_follower::DebugValues; + const float64_t to_degrees = (180.0 / static_cast(autoware::common::types::PI)); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch); + m_debug_values.setValues( + DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); +} + +void LongitudinalController::updateDebugVelAcc( + const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, + const ControlData & control_data) +{ + using trajectory_follower::DebugValues; + const float64_t current_vel = control_data.current_motion.vel; + const size_t nearest_idx = control_data.nearest_idx; + + const auto interpolated_point = + calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose.position, nearest_idx); + + m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); + m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); + m_debug_values.setValues(DebugValues::TYPE::TARGET_ACC, target_motion.acc); + m_debug_values.setValues( + DebugValues::TYPE::NEAREST_VEL, + interpolated_point.longitudinal_velocity_mps); + m_debug_values.setValues(DebugValues::TYPE::NEAREST_ACC, interpolated_point.acceleration_mps2); + m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel); +} + +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::motion::control::trajectory_follower_nodes::LongitudinalController) diff --git a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp b/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp new file mode 100644 index 0000000000000..b94d3a251b694 --- /dev/null +++ b/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp @@ -0,0 +1,457 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 + +#include +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "gtest/gtest.h" +#include "fake_test_node/fake_test_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "trajectory_follower_test_utils.hpp" + + +using LateralController = autoware::motion::control::trajectory_follower_nodes::LateralController; +using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand; +using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using VehicleOdometry = nav_msgs::msg::Odometry; +using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; + +using FakeNodeFixture = autoware::tools::testing::FakeTestNode; + +const rclcpp::Duration one_second(1, 0); + +std::shared_ptr makeLateralNode() +{ + // Pass default parameter file to the node + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + rclcpp::NodeOptions node_options; + node_options.arguments( + {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.yaml", + "--params-file", share_dir + "/param/test_vehicle_info.yaml"}); + std::shared_ptr node = std::make_shared(node_options); + + // Enable all logging in the node + auto ret = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) {std::cout << "Failed to set logging severerity to DEBUG\n";} + return node; +} + +TEST_F(FakeNodeFixture, no_input) +{ + // Data to test + LateralCommand::SharedPtr cmd_msg; + bool received_lateral_command = false; + // Node + std::shared_ptr node = makeLateralNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher( + "lateral_controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher( + "lateral_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher( + "lateral_controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "lateral_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { + cmd_msg = msg; received_lateral_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // No published data: expect a stopped command + test_utils::waitForMessage(node, this, received_lateral_command, std::chrono::seconds{1LL}, false); + ASSERT_FALSE(received_lateral_command); +} + +TEST_F(FakeNodeFixture, empty_trajectory) +{ + // Data to test + LateralCommand::SharedPtr cmd_msg; + bool received_lateral_command = false; + // Node + std::shared_ptr node = makeLateralNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher( + "lateral_controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher( + "lateral_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher( + "lateral_controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "lateral_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { + cmd_msg = msg; received_lateral_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Empty trajectory: expect a stopped command + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + traj_msg.header.stamp = node->now(); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + traj_pub->publish(traj_msg); + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_lateral_command, std::chrono::seconds{1LL}, false); + ASSERT_FALSE(received_lateral_command); +} + +TEST_F(FakeNodeFixture, straight_trajectory) +{ + // Data to test + LateralCommand::SharedPtr cmd_msg; + bool received_lateral_command = false; + // Node + std::shared_ptr node = makeLateralNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher( + "lateral_controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher( + "lateral_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher( + "lateral_controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "lateral_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { + cmd_msg = msg; received_lateral_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Straight trajectory: expect no steering + received_lateral_command = false; + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + TrajectoryPoint p; + traj_msg.header.stamp = node->now(); + p.pose.position.x = -1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + traj_pub->publish(traj_msg); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_lateral_command); + ASSERT_TRUE(received_lateral_command); + EXPECT_EQ(cmd_msg->steering_tire_angle, 0.0f); + EXPECT_EQ(cmd_msg->steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); +} + +TEST_F(FakeNodeFixture, right_turn) +{ + // Data to test + LateralCommand::SharedPtr cmd_msg; + bool received_lateral_command = false; + // Node + std::shared_ptr node = makeLateralNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher( + "lateral_controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher( + "lateral_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher( + "lateral_controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "lateral_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { + cmd_msg = msg; received_lateral_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Right turning trajectory: expect right steering + received_lateral_command = false; + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + TrajectoryPoint p; + traj_msg.points.clear(); + p.pose.position.x = -1.0; + p.pose.position.y = -1.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = -1.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = -2.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + traj_pub->publish(traj_msg); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_lateral_command); + ASSERT_TRUE(received_lateral_command); + EXPECT_LT(cmd_msg->steering_tire_angle, 0.0f); + EXPECT_LT(cmd_msg->steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); +} + +TEST_F(FakeNodeFixture, left_turn) +{ + // Data to test + LateralCommand::SharedPtr cmd_msg; + bool received_lateral_command = false; + // Node + std::shared_ptr node = makeLateralNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher( + "lateral_controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher( + "lateral_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher( + "lateral_controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "lateral_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { + cmd_msg = msg; received_lateral_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Left turning trajectory: expect left steering + received_lateral_command = false; + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + TrajectoryPoint p; + traj_msg.points.clear(); + p.pose.position.x = -1.0; + p.pose.position.y = 1.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 1.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 2.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + traj_pub->publish(traj_msg); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_lateral_command); + ASSERT_TRUE(received_lateral_command); + EXPECT_GT(cmd_msg->steering_tire_angle, 0.0f); + EXPECT_GT(cmd_msg->steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); +} + +TEST_F(FakeNodeFixture, stopped) +{ + // Data to test + LateralCommand::SharedPtr cmd_msg; + bool received_lateral_command = false; + // Node + std::shared_ptr node = makeLateralNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher( + "lateral_controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher( + "lateral_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher( + "lateral_controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "lateral_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { + cmd_msg = msg; received_lateral_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Straight trajectory: expect no steering + received_lateral_command = false; + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + TrajectoryPoint p; + traj_msg.header.stamp = node->now(); + p.pose.position.x = -1.0; + p.pose.position.y = 0.0; + // Set a 0 current velocity and 0 target velocity -> stopped state + p.longitudinal_velocity_mps = 0.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + traj_msg.points.push_back(p); + traj_pub->publish(traj_msg); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = -0.5; + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_lateral_command); + ASSERT_TRUE(received_lateral_command); + EXPECT_EQ(cmd_msg->steering_tire_angle, steer_msg.steering_tire_angle); + EXPECT_EQ(cmd_msg->steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); +} + +// TODO(Maxime CLEMENT): disabled as this test crashes in the CI but works locally +TEST_F(FakeNodeFixture, DISABLED_set_lateral_param_smoke_test) +{ + // Node + std::shared_ptr node = makeLateralNode(); + // give the node some time to initialize completely + std::this_thread::sleep_for(std::chrono::milliseconds{100LL}); + + // Change some parameter value + auto result = node->set_parameter(rclcpp::Parameter("mpc_prediction_horizon", 10)); + EXPECT_TRUE(result.successful); +} diff --git a/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp b/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp new file mode 100644 index 0000000000000..df79ad04959ca --- /dev/null +++ b/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp @@ -0,0 +1,201 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 + +#include +#include +#include + +#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" +#include "fake_test_node/fake_test_node.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "gtest/gtest.h" +#include "trajectory_follower_test_utils.hpp" + + +using LatLonMuxer = autoware::motion::control::trajectory_follower_nodes::LatLonMuxer; +using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand; +using LongitudinalCommand = autoware_auto_control_msgs::msg::LongitudinalCommand; +using ControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + +using FakeNodeFixture = autoware::tools::testing::FakeTestNode; + +const rclcpp::Duration one_second(1, 0); + +TEST_F(FakeNodeFixture, TestCorrectOutput) +{ + // Data to test + ControlCommand::SharedPtr cmd_msg; + bool received_combined_command = false; + // Node + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("timeout_thr_sec", 0.5); + std::shared_ptr node = std::make_shared(node_options); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr lat_pub = + this->create_publisher( + "latlon_muxer/input/lateral/control_cmd"); + rclcpp::Publisher::SharedPtr lon_pub = + this->create_publisher( + "latlon_muxer/input/longitudinal/control_cmd"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "latlon_muxer/output/control_cmd", *node, + [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) { + cmd_msg = msg; received_combined_command = true; + }); + // Publish messages + LateralCommand lat_msg; + lat_msg.steering_tire_angle = 1.5; + lat_msg.steering_tire_rotation_rate = 0.2f; + LongitudinalCommand lon_msg; + lon_msg.speed = 5.0; + lon_msg.acceleration = -1.0; + lon_msg.jerk = 0.25; + lat_msg.stamp = node->now(); + lon_msg.stamp = node->now(); + lat_pub->publish(lat_msg); + lon_pub->publish(lon_msg); + + test_utils::waitForMessage(node, this, received_combined_command); + // Ensure the combined control command was published and contains correct data + ASSERT_TRUE(received_combined_command); + EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, lat_msg.steering_tire_angle); + EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, lat_msg.steering_tire_rotation_rate); + EXPECT_EQ(cmd_msg->longitudinal.speed, lon_msg.speed); + EXPECT_EQ(cmd_msg->longitudinal.acceleration, lon_msg.acceleration); + EXPECT_EQ(cmd_msg->longitudinal.jerk, lon_msg.jerk); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(lat_msg.stamp)); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(lon_msg.stamp)); +} + +TEST_F(FakeNodeFixture, TestLateralTimeout) +{ + // Data to test + ControlCommand::SharedPtr cmd_msg; + bool received_combined_command = false; + // Node + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("timeout_thr_sec", 0.5); + std::shared_ptr node = std::make_shared(node_options); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr lat_pub = + this->create_publisher( + "latlon_muxer/input/lateral/control_cmd"); + rclcpp::Publisher::SharedPtr lon_pub = + this->create_publisher( + "latlon_muxer/input/longitudinal/control_cmd"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "latlon_muxer/output/control_cmd", *node, + [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) { + cmd_msg = msg; received_combined_command = true; + }); + // Publish messages + LateralCommand lat_msg; + LongitudinalCommand lon_msg; + // Generate a timeout of the lateral message + lat_msg.stamp = node->now() - one_second; + lon_msg.stamp = node->now(); + lat_pub->publish(lat_msg); + lon_pub->publish(lon_msg); + + test_utils::waitForMessage( + node, this, received_combined_command, std::chrono::seconds{1LL}, + false); + // Ensure combined command was not published + ASSERT_FALSE(received_combined_command); +} + +TEST_F(FakeNodeFixture, TestLongitudinalTimeout) +{ + // Data to test + ControlCommand::SharedPtr cmd_msg; + bool received_combined_command = false; + // Node + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("timeout_thr_sec", 0.5); + std::shared_ptr node = std::make_shared(node_options); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr lat_pub = + this->create_publisher( + "latlon_muxer/input/lateral/control_cmd"); + rclcpp::Publisher::SharedPtr lon_pub = + this->create_publisher( + "latlon_muxer/input/longitudinal/control_cmd"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "latlon_muxer/output/control_cmd", *node, + [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) { + cmd_msg = msg; received_combined_command = true; + }); + // Publish messages + LateralCommand lat_msg; + LongitudinalCommand lon_msg; + // Generate a timeout of the longitudinal message + lat_msg.stamp = node->now(); + lon_msg.stamp = node->now() - one_second; + lat_pub->publish(lat_msg); + lon_pub->publish(lon_msg); + + test_utils::waitForMessage( + node, this, received_combined_command, std::chrono::seconds{1LL}, + false); + // Ensure combined command was not published + ASSERT_FALSE(received_combined_command); +} + +TEST_F(FakeNodeFixture, TestLatlonTimeout) +{ + // Data to test + ControlCommand::SharedPtr cmd_msg; + bool received_combined_command = false; + // Node + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("timeout_thr_sec", 0.5); + std::shared_ptr node = std::make_shared(node_options); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr lat_pub = + this->create_publisher( + "latlon_muxer/input/lateral/control_cmd"); + rclcpp::Publisher::SharedPtr lon_pub = + this->create_publisher( + "latlon_muxer/input/longitudinal/control_cmd"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "latlon_muxer/output/control_cmd", *node, + [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) { + cmd_msg = msg; received_combined_command = true; + }); + // Publish messages + LateralCommand lat_msg; + LongitudinalCommand lon_msg; + // Generate a timeout of the longitudinal message + lat_msg.stamp = node->now() - one_second; + lon_msg.stamp = node->now() - one_second; + lat_pub->publish(lat_msg); + lon_pub->publish(lon_msg); + + test_utils::waitForMessage( + node, this, received_combined_command, std::chrono::seconds{1LL}, + false); + // Ensure combined command was not published + ASSERT_FALSE(received_combined_command); +} diff --git a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp new file mode 100644 index 0000000000000..836b6bc938f10 --- /dev/null +++ b/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp @@ -0,0 +1,470 @@ +// 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 + +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "fake_test_node/fake_test_node.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "trajectory_follower_test_utils.hpp" + +using LongitudinalController = + autoware::motion::control::trajectory_follower_nodes::LongitudinalController; +using LongitudinalCommand = autoware_auto_control_msgs::msg::LongitudinalCommand; +using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using VehicleOdometry = nav_msgs::msg::Odometry; + +using FakeNodeFixture = autoware::tools::testing::FakeTestNode; + +std::shared_ptr makeLongitudinalNode() +{ + // Pass default parameter file to the node + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + rclcpp::NodeOptions node_options; + node_options.arguments( + {"--ros-args", "--params-file", share_dir + "/param/longitudinal_controller_defaults.yaml", + "--params-file", share_dir + "/param/test_vehicle_info.yaml"}); + std::shared_ptr node = std::make_shared( + node_options); + + // Enable all logging in the node + auto ret = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) {std::cout << "Failed to set logging severerity to DEBUG\n";} + return node; +} + + +TEST_F(FakeNodeFixture, longitudinal_keep_velocity) { + // Data to test + LongitudinalCommand::SharedPtr cmd_msg; + bool received_longitudinal_command = false; + + // Node + std::shared_ptr node = makeLongitudinalNode(); + + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr odom_pub = this->create_publisher( + "longitudinal_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher( + "longitudinal_controller/input/current_trajectory"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { + cmd_msg = msg;received_longitudinal_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Already running at target vel + Non stopping trajectory -> no change in velocity + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish non stopping trajectory + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + + ASSERT_TRUE(received_longitudinal_command); + EXPECT_DOUBLE_EQ(cmd_msg->speed, 1.0); + EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0); + + // Generate another control message + received_longitudinal_command = false; + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + ASSERT_TRUE(received_longitudinal_command); + EXPECT_DOUBLE_EQ(cmd_msg->speed, 1.0); + EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0); +} + +TEST_F(FakeNodeFixture, longitudinal_slow_down) { + // Data to test + LongitudinalCommand::SharedPtr cmd_msg; + bool received_longitudinal_command = false; + + // Node + std::shared_ptr node = makeLongitudinalNode(); + + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr odom_pub = this->create_publisher( + "longitudinal_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher( + "longitudinal_controller/input/current_trajectory"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { + cmd_msg = msg;received_longitudinal_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Already running at target vel + Non stopping trajectory -> no change in velocity + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish non stopping trajectory + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.5; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.5; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.5; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + + ASSERT_TRUE(received_longitudinal_command); + EXPECT_LT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_LT(cmd_msg->acceleration, 0.0f); + + // Generate another control message + received_longitudinal_command = false; + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + ASSERT_TRUE(received_longitudinal_command); + EXPECT_LT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_LT(cmd_msg->acceleration, 0.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_accelerate) { + // Data to test + LongitudinalCommand::SharedPtr cmd_msg; + bool received_longitudinal_command = false; + + // Node + std::shared_ptr node = makeLongitudinalNode(); + + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr odom_pub = this->create_publisher( + "longitudinal_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher( + "longitudinal_controller/input/current_trajectory"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { + cmd_msg = msg;received_longitudinal_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Below target vel + Non stopping trajectory -> accelerate + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.5; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish non stopping trajectory + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + + ASSERT_TRUE(received_longitudinal_command); + EXPECT_GT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_GT(cmd_msg->acceleration, 0.0f); + + // Generate another control message + received_longitudinal_command = false; + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + ASSERT_TRUE(received_longitudinal_command); + EXPECT_GT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_GT(cmd_msg->acceleration, 0.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_stopped) { + // Data to test + LongitudinalCommand::SharedPtr cmd_msg; + bool received_longitudinal_command = false; + + // Node + std::shared_ptr node = makeLongitudinalNode(); + + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr odom_pub = this->create_publisher( + "longitudinal_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher( + "longitudinal_controller/input/current_trajectory"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { + cmd_msg = msg;received_longitudinal_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Below target vel + Non stopping trajectory -> accelerate + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish stopping trajectory + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + + ASSERT_TRUE(received_longitudinal_command); + EXPECT_DOUBLE_EQ(cmd_msg->speed, 0.0f); + EXPECT_LT(cmd_msg->acceleration, 0.0f); // when stopped negative acceleration to brake +} + +TEST_F(FakeNodeFixture, longitudinal_reverse) { + // Data to test + LongitudinalCommand::SharedPtr cmd_msg; + bool received_longitudinal_command = false; + + // Node + std::shared_ptr node = makeLongitudinalNode(); + + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr odom_pub = this->create_publisher( + "longitudinal_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher( + "longitudinal_controller/input/current_trajectory"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { + cmd_msg = msg;received_longitudinal_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Below target vel + Non stopping trajectory -> accelerate + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish reverse + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = -1.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = -1.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = -1.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + + ASSERT_TRUE(received_longitudinal_command); + EXPECT_LT(cmd_msg->speed, 0.0f); + EXPECT_GT(cmd_msg->acceleration, 0.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_emergency) { + // Data to test + LongitudinalCommand::SharedPtr cmd_msg; + bool received_longitudinal_command = false; + + // Node + std::shared_ptr node = makeLongitudinalNode(); + + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr odom_pub = this->create_publisher( + "longitudinal_controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher( + "longitudinal_controller/input/current_trajectory"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { + cmd_msg = msg;received_longitudinal_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Below target vel + Non stopping trajectory -> accelerate + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish trajectory starting away from the current ego pose + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 10.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_longitudinal_command); + + ASSERT_TRUE(received_longitudinal_command); + // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) + EXPECT_DOUBLE_EQ(cmd_msg->speed, 0.0f); + EXPECT_LT(cmd_msg->acceleration, 0.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_set_param_smoke_test) +{ + // Node + std::shared_ptr node = makeLongitudinalNode(); + + // give the node some time to initialize completely + std::this_thread::sleep_for(std::chrono::milliseconds{100LL}); + + // Change some parameter value + auto result = node->set_parameter(rclcpp::Parameter("kp", 1.0)); + EXPECT_TRUE(result.successful); +} diff --git a/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp b/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp new file mode 100644 index 0000000000000..3cea0b22ebf3e --- /dev/null +++ b/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp @@ -0,0 +1,85 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 TRAJECTORY_FOLLOWER_TEST_UTILS_HPP_ +#define TRAJECTORY_FOLLOWER_TEST_UTILS_HPP_ + +#include +#include + +#include "fake_test_node/fake_test_node.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "tf2_ros/static_transform_broadcaster.h" + +namespace test_utils +{ +using FakeNodeFixture = autoware::tools::testing::FakeTestNode; + +inline void waitForMessage( + const std::shared_ptr & node, FakeNodeFixture * fixture, + const bool & received_flag, + const std::chrono::duration max_wait_time = std::chrono::seconds{10LL}, + const bool fail_on_timeout = true) +{ + const auto dt{std::chrono::milliseconds{100LL}}; + auto time_passed{std::chrono::milliseconds{0LL}}; + while (!received_flag) { + rclcpp::spin_some(node); + rclcpp::spin_some(fixture->get_fake_node()); + std::this_thread::sleep_for(dt); + time_passed += dt; + if (time_passed > max_wait_time) { + if (fail_on_timeout) { + throw std::runtime_error( + std::string( + "Did not receive a message soon enough")); + } else { + break; + } + } + } +} + +inline geometry_msgs::msg::TransformStamped getDummyTransform() +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.transform.translation.x = 0.0; + transform_stamped.transform.translation.y = 0.0; + transform_stamped.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, 0.0); + transform_stamped.transform.rotation.x = q.x(); + transform_stamped.transform.rotation.y = q.y(); + transform_stamped.transform.rotation.z = q.z(); + transform_stamped.transform.rotation.w = q.w(); + transform_stamped.header.frame_id = "map"; + transform_stamped.child_frame_id = "base_link"; + return transform_stamped; +} + +// TODO(Horibe): modify the controller nodes so that they does not publish topics when data is not ready. +// then, remove this function. +template +inline void spinWhile(T &node){ + for (size_t i = 0; i < 10; i++) { + rclcpp::spin_some(node); + const auto dt{std::chrono::milliseconds{100LL}}; + std::this_thread::sleep_for(dt); + } +} +} // namespace test_utils + +#endif // TRAJECTORY_FOLLOWER_TEST_UTILS_HPP_