diff --git a/control/trajectory_follower/CMakeLists.txt b/control/trajectory_follower/CMakeLists.txt new file mode 100644 index 0000000000000..f3385f6e7b225 --- /dev/null +++ b/control/trajectory_follower/CMakeLists.txt @@ -0,0 +1,121 @@ +# 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) + +# require that dependencies from package.xml be available +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies(REQUIRED + ${${PROJECT_NAME}_BUILD_DEPENDS} + ${${PROJECT_NAME}_BUILDTOOL_DEPENDS} +) + +# lateral_controller +set(LATERAL_CONTROLLER_LIB lateral_controller_lib) +set(LATERAL_CONTROLLER_LIB_SRC + src/interpolate.cpp + src/lowpass_filter.cpp + src/mpc.cpp + src/mpc_trajectory.cpp + src/mpc_utils.cpp + src/qp_solver/qp_solver_osqp.cpp + src/qp_solver/qp_solver_unconstr_fast.cpp + src/vehicle_model/vehicle_model_bicycle_dynamics.cpp + src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp + src/vehicle_model/vehicle_model_bicycle_kinematics.cpp + src/vehicle_model/vehicle_model_interface.cpp +) + +set(LATERAL_CONTROLLER_LIB_HEADERS + include/trajectory_follower/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_interface.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_no_delay.hpp + include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp + include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp +) + +# generate library +ament_auto_add_library(${LATERAL_CONTROLLER_LIB} SHARED + ${LATERAL_CONTROLLER_LIB_SRC} + ${LATERAL_CONTROLLER_LIB_HEADERS} +) +autoware_set_compile_options(${LATERAL_CONTROLLER_LIB}) +target_compile_options(${LATERAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-cast) + +# longitudinal_controller +set(LONGITUDINAL_CONTROLLER_LIB longitudinal_controller_lib) +set(LONGITUDINAL_CONTROLLER_LIB_SRC + src/pid.cpp + src/smooth_stop.cpp + src/longitudinal_controller_utils.cpp +) + +set(LONGITUDINAL_CONTROLLER_LIB_HEADERS + include/trajectory_follower/debug_values.hpp + include/trajectory_follower/pid.hpp + include/trajectory_follower/smooth_stop.hpp + include/trajectory_follower/longitudinal_controller_utils.hpp +) + +# generate library +ament_auto_add_library(${LONGITUDINAL_CONTROLLER_LIB} SHARED + ${LONGITUDINAL_CONTROLLER_LIB_SRC} + ${LONGITUDINAL_CONTROLLER_LIB_HEADERS} +) +autoware_set_compile_options(${LONGITUDINAL_CONTROLLER_LIB}) +target_compile_options(${LONGITUDINAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-cast) + +# Testing +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Unit tests + set(TEST_LAT_SOURCES + test/test_mpc.cpp + test/test_mpc_trajectory.cpp + test/test_mpc_utils.cpp + test/test_interpolate.cpp + test/test_lowpass_filter.cpp + ) + set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) + ament_add_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) + autoware_set_compile_options(${TEST_LATERAL_CONTROLLER_EXE}) + target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${LATERAL_CONTROLLER_LIB}) + + set(TEST_LON_SOURCES + test/test_debug_values.cpp + test/test_pid.cpp + test/test_smooth_stop.cpp + test/test_longitudinal_controller_utils.cpp + ) + set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller) + ament_add_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) + autoware_set_compile_options(${TEST_LONGITUDINAL_CONTROLLER_EXE}) + target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${LONGITUDINAL_CONTROLLER_LIB}) +endif() + +# ament package generation and installing +ament_auto_package(INSTALL_TO_SHARE +) diff --git a/control/trajectory_follower/design/trajectory_follower-design.md b/control/trajectory_follower/design/trajectory_follower-design.md new file mode 100644 index 0000000000000..93a27f168ace8 --- /dev/null +++ b/control/trajectory_follower/design/trajectory_follower-design.md @@ -0,0 +1,21 @@ +Trajectory Follower {#trajectory_follower-package-design} +=========== + +This is the design document for the `trajectory_follower` package. + + +# Purpose / Use cases + + +This package provides the library code used by the nodes of the `trajectory_follower_nodes` package. +Mainly, it implements two algorithms: +- Model-Predictive Control (MPC) for the computation of lateral steering commands. + - @subpage trajectory_follower-mpc-design +- PID control for the computation of velocity and acceleration commands. + - @subpage trajectory_follower-pid-design + +# Related issues + +- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057 +- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058 diff --git a/control/trajectory_follower/design/trajectory_follower-mpc-design.md b/control/trajectory_follower/design/trajectory_follower-mpc-design.md new file mode 100644 index 0000000000000..b0dc982fe099e --- /dev/null +++ b/control/trajectory_follower/design/trajectory_follower-mpc-design.md @@ -0,0 +1,65 @@ +MPC (Trajectory Follower) {#trajectory_follower-mpc-design} +=========== + +This is the design document for the MPC implemented in the `trajectory_follower` package. + +# Purpose / Use cases + + +Model Predictive Control (MPC) is used by the `trajectory_follower` +to calculate the lateral commands corresponding to a steering angle and a steering rate. + +This implementation differs from the one in the `mpc_controller` package in several aspects. +- This is a linear MPC that only computes the steering command whereas +the `mpc_controller` uses a non-linear MPC which calculates coupled steering and velocity commands. +- The optimization problem solved by the linear MPC is simpler, making it less likely to fail. +- Tuning of the linear MPC is easier. + +# Design + + +MPC uses predictions of the vehicle's motion to optimize the immediate control command. + +Different vehicle models are implemented: +- `kinematics` : bicycle kinematics model with steering 1st-order delay. +- `kinematics_no_delay` : bicycle kinematics model without steering delay. +- `dynamics` : bicycle dynamics model considering slip angle. + +The `kinematics` model is being used by default. Please see the reference [1] for more details. + + +For the optimization, a Quadratric Programming (QP) solver is used +with two options are currently implemented: +- `unconstraint` : use least square method to solve unconstraint QP with eigen. +- `unconstraint_fast` : similar to unconstraint. This is faster, but lower accuracy for optimization. + +## Filtering + +Filtering is required for good noise reduction. +A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as input of the MPC as well as for +the output steering angle. +Other filtering methods can be considered as long as the noise reduction performances are good +enough. +The moving average filter for example is not suited and can yield worse results than without any +filtering. + +## Inputs / Outputs / API + + +The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm. +Once a vehicle model, a QP solver, and the reference trajectory to follow have been set +(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command +can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`. + +# References / External links + +- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", +Robotics Institute, Carnegie Mellon University, February 2009. + +# Related issues + +- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057 +- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058 diff --git a/control/trajectory_follower/design/trajectory_follower-pid-design.md b/control/trajectory_follower/design/trajectory_follower-pid-design.md new file mode 100644 index 0000000000000..de6df3a4ed527 --- /dev/null +++ b/control/trajectory_follower/design/trajectory_follower-pid-design.md @@ -0,0 +1,88 @@ +PID (Trajectory Follower) {#trajectory_follower-pid-design} +=========== + +This is the design document for the PID implemented in the `trajectory_follower` package. + +# Purpose / Use cases + + +PID control is used by the `trajectory_follower` +to calculate longitudinal commands corresponding to a velocity and an acceleration. + +# Design + + +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. + +@image html images/trajectory_follower-pid-diagram.svg "PID controller diagram" + +## 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. + +@image html images/trajectory_follower-pid-states-diagram.svg "State Transitions" + +## 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. + +## 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) + +## Inputs / Outputs / API + + +The `PIDController` class is straightforward to use. +First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components. +Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function. + +# Related issues + +- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058 diff --git a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp b/control/trajectory_follower/include/trajectory_follower/debug_values.hpp new file mode 100644 index 0000000000000..61ba471b0c8bd --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/debug_values.hpp @@ -0,0 +1,106 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ +#define TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ + +#include + +#include "common/types.hpp" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +/// Debug Values used for debugging or controller tuning +class TRAJECTORY_FOLLOWER_PUBLIC DebugValues +{ +public: + /// Types of debug values + enum class TYPE + { + DT = 0, + CURRENT_VEL = 1, + TARGET_VEL = 2, + TARGET_ACC = 3, + NEAREST_VEL = 4, + NEAREST_ACC = 5, + SHIFT = 6, + PITCH_LPF_RAD = 7, + PITCH_RAW_RAD = 8, + PITCH_LPF_DEG = 9, + PITCH_RAW_DEG = 10, + ERROR_VEL = 11, + ERROR_VEL_FILTERED = 12, + CONTROL_STATE = 13, + ACC_CMD_PID_APPLIED = 14, + ACC_CMD_ACC_LIMITED = 15, + ACC_CMD_JERK_LIMITED = 16, + ACC_CMD_SLOPE_APPLIED = 17, + ACC_CMD_PUBLISHED = 18, + ACC_CMD_FB_P_CONTRIBUTION = 19, + ACC_CMD_FB_I_CONTRIBUTION = 20, + ACC_CMD_FB_D_CONTRIBUTION = 21, + FLAG_STOPPING = 22, + FLAG_EMERGENCY_STOP = 23, + PREDICTED_VEL = 24, + CALCULATED_ACC = 25, + PITCH_RAW_TRAJ_RAD = 26, + PITCH_RAW_TRAJ_DEG = 27, + STOP_DIST = 28, + SIZE // this is the number of enum elements + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + size_t getValuesIdx(const TYPE type) const {return static_cast(type);} + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> getValues() const {return m_values;} + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void setValues(const TYPE type, const float64_t value) + { + m_values.at(static_cast(type)) = value; + } + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void setValues(const size_t type, const float64_t value) {m_values.at(type) = value;} + +private: + std::array(TYPE::SIZE)> m_values; +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp b/control/trajectory_follower/include/trajectory_follower/interpolate.hpp new file mode 100644 index 0000000000000..99fc437d57f48 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/interpolate.hpp @@ -0,0 +1,59 @@ +// Copyright 2018-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__INTERPOLATE_HPP_ +#define TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_ + +#include +#include +#include + +#include "common/types.hpp" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +/** + * @brief linearly interpolate the given values assuming a base indexing and a new desired indexing + * @param [in] base_index indexes for each base value + * @param [in] base_value values for each base index + * @param [in] return_index desired interpolated indexes + * @param [out] return_value resulting interpolated values + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index, std::vector & return_value); +/** + * @brief linearly interpolate the given values assuming a base indexing and a new desired index + * @param [in] base_index indexes for each base value + * @param [in] base_value values for each base index + * @param [in] return_index desired interpolated index + * @param [out] return_value resulting interpolated value + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate( + const std::vector & base_index, const std::vector & base_value, + const float64_t & return_index, float64_t & return_value); +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__INTERPOLATE_HPP__ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp new file mode 100644 index 0000000000000..854f85b758f74 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -0,0 +1,184 @@ +// Copyright 2018-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__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ + +#include +#include // NOLINT +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "common/types.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry/common_2d.hpp" +#include "motion_common/motion_common.hpp" +#include "motion_common/trajectory_common.hpp" +#include "tf2/utils.h" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +namespace longitudinal_utils +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Quaternion; +namespace motion_common = ::motion::motion_common; +namespace trajectory_common = ::autoware::motion::motion_common; + +/** + * @brief check if trajectory is invalid or not + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj); + +/** + * @brief calculate distance to stopline from current vehicle position where velocity is 0 + */ +TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( + const Point & current_pos, + const Trajectory & traj); + +/** + * @brief calculate pitch angle from estimated current pose + */ +TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByPose(const Quaternion & quaternion); + +/** + * @brief calculate pitch angle from trajectory on map + * NOTE: there is currently no z information so this always returns 0.0 + * @param [in] trajectory input trajectory + * @param [in] closest_idx nearest index to current vehicle position + * @param [in] wheel_base length of wheel base + */ +TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByTraj( + const Trajectory & trajectory, const size_t closest_idx, + const float64_t wheel_base); + +/** + * @brief calculate elevation angle + */ +TRAJECTORY_FOLLOWER_PUBLIC float64_t calcElevationAngle( + const TrajectoryPoint & p_from, + const TrajectoryPoint & p_to); + +/** + * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for delayed time + */ +TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay( + const Pose & current_pose, const float64_t delay_time, const float64_t current_vel); + +/** + * @brief apply linear interpolation to orientation + * @param [in] o_from first orientation + * @param [in] o_to second orientation + * @param [in] ratio ratio between o_from and o_to for interpolation + */ +TRAJECTORY_FOLLOWER_PUBLIC Quaternion lerpOrientation( + const Quaternion & o_from, + const Quaternion & o_to, + const float64_t ratio); + +/** + * @brief apply linear interpolation to trajectory point that is nearest to a certain point + * @param [in] points trajectory points + * @param [in] point Interpolated point is nearest to this point. + */ +template +TRAJECTORY_FOLLOWER_PUBLIC +TrajectoryPoint lerpTrajectoryPoint(const T & points, const Point & point) +{ + TrajectoryPoint interpolated_point; + + const size_t nearest_seg_idx = trajectory_common::findNearestSegmentIndex(points, point); + + const float64_t len_to_interpolated = + trajectory_common::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point); + const float64_t len_segment = + trajectory_common::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1); + const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); + + { + const size_t i = nearest_seg_idx; + + interpolated_point.pose.position.x = motion_common::interpolate( + points.at(i).pose.position.x, points.at( + i + 1).pose.position.x, interpolate_ratio); + interpolated_point.pose.position.y = motion_common::interpolate( + points.at(i).pose.position.y, points.at( + i + 1).pose.position.y, interpolate_ratio); + interpolated_point.pose.orientation = lerpOrientation( + points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); + interpolated_point.longitudinal_velocity_mps = + motion_common::interpolate( + points.at(i).longitudinal_velocity_mps, points.at( + i + 1).longitudinal_velocity_mps, interpolate_ratio); + interpolated_point.lateral_velocity_mps = + motion_common::interpolate( + points.at(i).lateral_velocity_mps, points.at( + i + 1).lateral_velocity_mps, interpolate_ratio); + interpolated_point.acceleration_mps2 = + motion_common::interpolate( + points.at(i).acceleration_mps2, points.at( + i + 1).acceleration_mps2, interpolate_ratio); + interpolated_point.heading_rate_rps = + motion_common::interpolate( + points.at(i).heading_rate_rps, points.at( + i + 1).heading_rate_rps, interpolate_ratio); + } + + return interpolated_point; +} + +/** + * @brief limit variable whose differential is within a certain value + * @param [in] input_val current value + * @param [in] prev_val previous value + * @param [in] dt time between current and previous one + * @param [in] lim_val limitation value for differential + */ +TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter( + const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t lim_val); + +/** + * @brief limit variable whose differential is within a certain value + * @param [in] input_val current value + * @param [in] prev_val previous value + * @param [in] dt time between current and previous one + * @param [in] max_val maximum value for differential + * @param [in] min_val minimum value for differential + */ +TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter( + const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t max_val, + const float64_t min_val); +} // namespace longitudinal_utils +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp b/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp new file mode 100644 index 0000000000000..a717ddb4bb36b --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp @@ -0,0 +1,163 @@ +// Copyright 2018-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__LOWPASS_FILTER_HPP_ +#define TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ + +#include +#include +#include +#include + +#include "common/types.hpp" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; + +/** + * @brief Simple filter with gain on the first derivative of the value + */ +class LowpassFilter1d +{ +private: + float64_t m_x; //!< @brief current filtered value + float64_t m_gain; //!< @brief gain value of first-order low-pass filter + +public: + /** + * @brief constructor with initial value and first-order gain + * @param [in] x initial value + * @param [in] gain first-order gain + */ + LowpassFilter1d(const float64_t x, const float64_t gain) + : m_x(x), m_gain(gain) {} + + /** + * @brief set the current value of the filter + * @param [in] x new value + */ + void reset(const float64_t x) {m_x = x;} + + /** + * @brief get the current value of the filter + */ + float64_t getValue() const {return m_x;} + + /** + * @brief filter a new value + * @param [in] u new value + */ + float64_t filter(const float64_t u) + { + const float64_t ret = m_gain * m_x + (1.0 - m_gain) * u; + m_x = ret; + return ret; + } +}; + + +/** + * @brief 2nd-order Butterworth Filter + * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930. + */ +class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter +{ +private: + float64_t m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_a0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + float64_t m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + +public: + /** + * @brief constructor with initialization + * @param [in] dt sampling time + * @param [in] f_cutoff_hz cutoff frequency [Hz] + */ + explicit Butterworth2dFilter(float64_t dt = 0.01, float64_t f_cutoff_hz = 5.0); + + /** + * @brief destructor + */ + ~Butterworth2dFilter(); + + /** + * @brief constructor + * @param [in] dt sampling time + * @param [in] f_cutoff_hz cutoff frequency [Hz] + */ + void initialize(const float64_t & dt, const float64_t & f_cutoff_hz); + + /** + * @brief filtering (call this function at each sampling time with input) + * @param [in] u scalar input for filter + * @return filtered scalar value + */ + float64_t filter(const float64_t & u); + + /** + * @brief filtering for time-series data + * @param [in] t time-series data for input vector + * @param [out] u object vector + */ + void filt_vector(const std::vector & t, std::vector & u) const; + + /** + * @brief filtering for time-series data from both forward-backward direction for zero phase delay + * @param [in] t time-series data for input vector + * @param [out] u object vector + */ + void filtfilt_vector( + const std::vector & t, + std::vector & u) const; // filtering forward and backward direction + + /** + * @brief get filter coefficients + * @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2]. + */ + void getCoefficients(std::vector & coeffs) const; +}; + +/** + * @brief Move Average Filter + */ +namespace MoveAverageFilter +{ +/** + * @brief filtering vector + * @param [in] num index distance for moving average filter + * @param [out] u object vector + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t filt_vector(const int64_t num, std::vector & u); +} // namespace MoveAverageFilter +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp new file mode 100644 index 0000000000000..91a4a2e39552e --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp @@ -0,0 +1,455 @@ +// Copyright 2018-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__MPC_HPP_ +#define TRAJECTORY_FOLLOWER__MPC_HPP_ + +#include +#include +#include +#include + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "trajectory_follower/interpolate.hpp" +#include "trajectory_follower/lowpass_filter.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 "trajectory_follower/visibility_control.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 "common/types.hpp" +#include "geometry/common_2d.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "helper_functions/angle_utils.hpp" +#include "motion_common/motion_common.hpp" +#include "osqp_interface/osqp_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +struct MPCParam +{ +//!< @brief prediction horizon step + int64_t prediction_horizon; +//!< @brief prediction horizon sampling time + float64_t prediction_dt; +//!< @brief threshold that feed-forward angle becomes zero + float64_t zero_ff_steer_deg; +//!< @brief delay time for steering input to be compensated + float64_t input_delay; +//!< @brief for trajectory velocity calculation + float64_t acceleration_limit; +//!< @brief for trajectory velocity calculation + float64_t velocity_time_constant; +//!< @brief time constant for steer model + float64_t steer_tau; +// for weight matrix Q +//!< @brief lateral error weight + float64_t weight_lat_error; +//!< @brief heading error weight + float64_t weight_heading_error; +//!< @brief heading error * velocity weight + float64_t weight_heading_error_squared_vel; +//!< @brief terminal lateral error weight + float64_t weight_terminal_lat_error; +//!< @brief terminal heading error weight + float64_t weight_terminal_heading_error; +//!< @brief lateral error weight in matrix Q in low curvature point + float64_t low_curvature_weight_lat_error; +//!< @brief heading error weight in matrix Q in low curvature point + float64_t low_curvature_weight_heading_error; +//!< @brief heading error * velocity weight in matrix Q in low curvature point + float64_t low_curvature_weight_heading_error_squared_vel; +// for weight matrix R +//!< @brief steering error weight + float64_t weight_steering_input; +//!< @brief steering error * velocity weight + float64_t weight_steering_input_squared_vel; +//!< @brief lateral jerk weight + float64_t weight_lat_jerk; +//!< @brief steering rate weight + float64_t weight_steer_rate; +//!< @brief steering angle acceleration weight + float64_t weight_steer_acc; +//!< @brief steering error weight in matrix R in low curvature point + float64_t low_curvature_weight_steering_input; +//!< @brief steering error * velocity weight in matrix R in low curvature point + float64_t low_curvature_weight_steering_input_squared_vel; +//!< @brief lateral jerk weight in matrix R in low curvature point + float64_t low_curvature_weight_lat_jerk; +//!< @brief steering rate weight in matrix R in low curvature point + float64_t low_curvature_weight_steer_rate; +//!< @brief steering angle acceleration weight in matrix R in low curvature + float64_t low_curvature_weight_steer_acc; +//!< @brief threshold of curvature to use "low curvature" parameter + float64_t low_curvature_thresh_curvature; +}; +/** + * MPC problem data + */ +struct MPCData +{ + int64_t nearest_idx; + float64_t nearest_time; + geometry_msgs::msg::Pose nearest_pose; + float64_t steer; + float64_t predicted_steer; + float64_t lateral_err; + float64_t yaw_err; +}; +/** + * Matrices used for MPC optimization + */ +struct MPCMatrix +{ + Eigen::MatrixXd Aex; + Eigen::MatrixXd Bex; + Eigen::MatrixXd Wex; + Eigen::MatrixXd Cex; + Eigen::MatrixXd Qex; + Eigen::MatrixXd R1ex; + Eigen::MatrixXd R2ex; + Eigen::MatrixXd Uref_ex; +}; +/** + * MPC-based waypoints follower class + * @brief calculate control command to follow reference waypoints + */ +class TRAJECTORY_FOLLOWER_PUBLIC MPC +{ +private: + //!< @brief ROS logger used for debug logging + rclcpp::Logger m_logger = rclcpp::get_logger("mpc_logger"); + //!< @brief ROS clock + rclcpp::Clock::SharedPtr m_clock = std::make_shared(RCL_ROS_TIME); + + //!< @brief vehicle model type for MPC + std::string m_vehicle_model_type; + //!< @brief vehicle model for MPC + std::shared_ptr m_vehicle_model_ptr; + //!< @brief qp solver for MPC + std::shared_ptr m_qpsolver_ptr; + //!< @brief lowpass filter for steering command + trajectory_follower::Butterworth2dFilter m_lpf_steering_cmd; + //!< @brief lowpass filter for lateral error + trajectory_follower::Butterworth2dFilter m_lpf_lateral_error; + //!< @brief lowpass filter for heading error + trajectory_follower::Butterworth2dFilter m_lpf_yaw_error; + + //!< @brief raw output computed two iterations ago + float64_t m_raw_steer_cmd_pprev = 0.0; + //!< @brief previous lateral error for derivative + float64_t m_lateral_error_prev = 0.0; + //!< @brief previous lateral error for derivative + float64_t m_yaw_error_prev = 0.0; + //!< @brief previous predicted steering + std::shared_ptr m_steer_prediction_prev; + //!< @brief previous computation time + rclcpp::Time m_time_prev = rclcpp::Time(0, 0, RCL_ROS_TIME); + //!< @brief sign of previous target speed to calculate curvature when the target speed is 0. + float64_t m_sign_vx = 0.0; + //!< @brief buffer of sent command + std::vector m_ctrl_cmd_vec; + + /** + * @brief get variables for mpc calculation + */ + bool8_t getData( + const trajectory_follower::MPCTrajectory & traj, + const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, + const geometry_msgs::msg::Pose & current_pose, + MPCData * data); + /** + * @brief calculate predicted steering + */ + float64_t calcSteerPrediction(); + /** + * @brief get the sum of all steering commands over the given time range + */ + float64_t getSteerCmdSum( + const rclcpp::Time & t_start, const rclcpp::Time & t_end, + const float64_t time_constant) const; + /** + * @brief set the reference trajectory to follow + */ + void storeSteerCmd(const float64_t steer); + /** + * @brief reset previous result of MPC + */ + void resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer); + /** + * @brief set initial condition for mpc + * @param [in] data mpc data + */ + Eigen::VectorXd getInitialState(const MPCData & data); + /** + * @brief update status for delay compensation + * @param [in] traj MPCTrajectory to follow + * @param [in] start_time start time for update + * @param [out] x updated state at delayed_time + */ + bool8_t updateStateForDelayCompensation( + const trajectory_follower::MPCTrajectory & traj, const float64_t & start_time, + Eigen::VectorXd * x); + /** + * @brief generate MPC matrix with trajectory and vehicle model + * @param [in] reference_trajectory used for linearization around reference trajectory + */ + MPCMatrix generateMPCMatrix(const trajectory_follower::MPCTrajectory & reference_trajectory); + /** + * @brief generate MPC matrix with trajectory and vehicle model + * @param [in] mpc_matrix parameters matrix to use for optimization + * @param [in] x0 initial state vector + * @param [out] Uex optimized input vector + */ + bool8_t executeOptimization( + const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, Eigen::VectorXd * Uex); + /** + * @brief resample trajectory with mpc resampling time + */ + bool8_t resampleMPCTrajectoryByTime( + float64_t start_time, const trajectory_follower::MPCTrajectory & input, + trajectory_follower::MPCTrajectory * output) const; + /** + * @brief apply velocity dynamics filter with v0 from closest index + */ + trajectory_follower::MPCTrajectory applyVelocityDynamicsFilter( + const trajectory_follower::MPCTrajectory & trajectory, + const geometry_msgs::msg::Pose & current_pose, const float64_t v0) const; + /** + * @brief get total prediction time of mpc + */ + float64_t getPredictionTime() const; + /** + * @brief add weights related to lateral_jerk, steering_rate, steering_acc into R + */ + void addSteerWeightR(Eigen::MatrixXd * R) const; + /** + * @brief add weights related to lateral_jerk, steering_rate, steering_acc into f + */ + void addSteerWeightF(Eigen::MatrixXd * f) const; + /** + * @brief check if the matrix has invalid value + */ + bool8_t isValid(const MPCMatrix & m) const; + /** + * @brief return true if the given curvature is considered low + */ + inline bool8_t isLowCurvature(const float64_t curvature) + { + return std::fabs(curvature) < m_param.low_curvature_thresh_curvature; + } + /** + * @brief return the weight of the lateral error for the given curvature + */ + inline float64_t getWeightLatError(const float64_t curvature) + { + return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_error : + m_param.weight_lat_error; + } + /** + * @brief return the weight of the heading error for the given curvature + */ + inline float64_t getWeightHeadingError(const float64_t curvature) + { + return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error : + m_param.weight_heading_error; + } + /** + * @brief return the squared velocity weight of the heading error for the given curvature + */ + inline float64_t getWeightHeadingErrorSqVel(const float64_t curvature) + { + return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error_squared_vel : + m_param.weight_heading_error_squared_vel; + } + /** + * @brief return the weight of the steer input for the given curvature + */ + inline float64_t getWeightSteerInput(const float64_t curvature) + { + return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input : + m_param.weight_steering_input; + } + /** + * @brief return the squared velocity weight of the steer input for the given curvature + */ + inline float64_t getWeightSteerInputSqVel(const float64_t curvature) + { + return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input_squared_vel : + m_param.weight_steering_input_squared_vel; + } + /** + * @brief return the weight of the lateral jerk for the given curvature + */ + inline float64_t getWeightLatJerk(const float64_t curvature) + { + return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_jerk : + m_param.weight_lat_jerk; + } + /** + * @brief return the weight of the steering rate for the given curvature + */ + inline float64_t getWeightSteerRate(const float64_t curvature) + { + return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_rate : + m_param.weight_steer_rate; + } + /** + * @brief return the weight of the steering acceleration for the given curvature + */ + inline float64_t getWeightSteerAcc(const float64_t curvature) + { + return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_acc : + m_param.weight_steer_acc; + } + +public: + //!< @brief reference trajectory to be followed + trajectory_follower::MPCTrajectory m_ref_traj; + //!< @brief MPC design parameter + MPCParam m_param; + //!< @brief mpc_output buffer for delay time compensation + std::deque m_input_buffer; + //!< @brief mpc raw output in previous period + float64_t m_raw_steer_cmd_prev = 0.0; + /* parameters for control*/ + //!< @brief use stop cmd when lateral error exceeds this [m] + float64_t m_admissible_position_error; + //!< @brief use stop cmd when yaw error exceeds this [rad] + float64_t m_admissible_yaw_error_rad; + //!< @brief steering command limit [rad] + float64_t m_steer_lim; + //!< @brief steering rate limit [rad/s] + float64_t m_steer_rate_lim; + //!< @brief control frequency [s] + float64_t m_ctrl_period; + /* parameters for path smoothing */ + //!< @brief flag to use predicted steer, not measured steer. + bool8_t m_use_steer_prediction; + + /** + * @brief constructor + */ + MPC() = default; + /** + * @brief calculate control command by MPC algorithm + * @param [in] current_steer current steering of the vehicle + * @param [in] current_velocity current velocity of the vehicle [m/s] + * @param [in] current_pose current pose of the vehicle + * @param [out] ctrl_cmd control command calculated with mpc algorithm + * @param [out] predicted_traj predicted MPC trajectory + * @param [out] diagnostic diagnostic msg to be filled-out + */ + bool8_t calculateMPC( + const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, + const float64_t current_velocity, + const geometry_msgs::msg::Pose & current_pose, + autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd, + autoware_auto_planning_msgs::msg::Trajectory & predicted_traj, + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic + ); + /** + * @brief set the reference trajectory to follow + */ + void setReferenceTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, + const float64_t traj_resample_dist, + const bool8_t enable_path_smoothing, + const int64_t path_filter_moving_ave_num, + const int64_t curvature_smoothing_num_traj, + const int64_t curvature_smoothing_num_ref_steer, + const geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr); + /** + * @brief set the vehicle model of this MPC + */ + inline void setVehicleModel( + std::shared_ptr vehicle_model_ptr, + const std::string & vehicle_model_type) + { + m_vehicle_model_ptr = vehicle_model_ptr; + m_vehicle_model_type = vehicle_model_type; + } + /** + * @brief set the QP solver of this MPC + */ + inline void setQPSolver(std::shared_ptr qpsolver_ptr) + { + m_qpsolver_ptr = qpsolver_ptr; + } + /** + * @brief initialize low pass filters + */ + inline void initializeLowPassFilters( + const float64_t steering_lpf_cutoff_hz, + const float64_t error_deriv_lpf_cutoff_hz) + { + m_lpf_steering_cmd.initialize(m_ctrl_period, steering_lpf_cutoff_hz); + m_lpf_lateral_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz); + m_lpf_yaw_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz); + } + /** + * @brief return true if the vehicle model of this MPC is set + */ + inline bool8_t hasVehicleModel() const + { + return m_vehicle_model_ptr != nullptr; + } + /** + * @brief return true if the QP solver of this MPC is set + */ + inline bool8_t hasQPSolver() const + { + return m_qpsolver_ptr != nullptr; + } + /** + * @brief set the RCLCPP logger to use for logging + */ + inline void setLogger(rclcpp::Logger logger) + { + m_logger = logger; + } + /** + * @brief set the RCLCPP clock to use for time keeping + */ + inline void setClock(rclcpp::Clock::SharedPtr clock) + { + m_clock = clock; + } +}; // class MPC +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__MPC_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp new file mode 100644 index 0000000000000..6b11e1085ad17 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp @@ -0,0 +1,77 @@ +// Copyright 2018-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__MPC_TRAJECTORY_HPP_ +#define TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ + +#include +#include + +#include "common/types.hpp" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +/** + * Trajectory class for mpc follower + * @brief calculate control command to follow reference waypoints + */ +class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory +{ +public: + std::vector x; //!< @brief x position x vector + std::vector y; //!< @brief y position y vector + std::vector z; //!< @brief z position z vector + std::vector yaw; //!< @brief yaw pose yaw vector + std::vector vx; //!< @brief vx velocity vx vector + std::vector k; //!< @brief k curvature k vector + std::vector smooth_k; //!< @brief k smoothed-curvature k vector + std::vector relative_time; //!< @brief relative_time duration time from start point + + /** + * @brief push_back for all values + */ + void push_back( + const float64_t & xp, const float64_t & yp, const float64_t & zp, const float64_t & yawp, + const float64_t & vxp, const float64_t & kp, const float64_t & smooth_kp, const float64_t & tp); + /** + * @brief clear for all values + */ + void clear(); + + /** + * @brief check size of MPCTrajectory + * @return size, or 0 if the size for each components are inconsistent + */ + size_t size() const; + /** + * @return true if the compensents sizes are all 0 or are inconsistent + */ + inline bool empty() const + { + return size() == 0; + } +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp new file mode 100644 index 0000000000000..ec00273225e3a --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -0,0 +1,202 @@ +// Copyright 2018-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__MPC_UTILS_HPP_ +#define TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ + +#include +#include +#include + +#include "trajectory_follower/interpolate.hpp" +#include "trajectory_follower/mpc_trajectory.hpp" +#include "trajectory_follower/visibility_control.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "geometry/common_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "helper_functions/angle_utils.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/linear_interpolation.hpp" +#include "motion_common/motion_common.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +namespace MPCUtils +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +/** + * @brief convert from yaw to ros-Quaternion + * @param [in] yaw input yaw angle + * @return quaternion + */ +TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion getQuaternionFromYaw( + const float64_t & yaw); +/** + * @brief convert Euler angle vector including +-2pi to 0 jump to continuous series data + * @param [inout] a input angle vector + */ +TRAJECTORY_FOLLOWER_PUBLIC void convertEulerAngleToMonotonic(std::vector * a); +/** + * @brief calculate the lateral error of the given pose relative to the given reference pose + * @param [in] ego_pose pose to check for error + * @param [in] ref_pose reference pose + * @return lateral distance between the two poses + */ +TRAJECTORY_FOLLOWER_PUBLIC float64_t calcLateralError( + const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & ref_pose); +/** + * @brief convert the given Trajectory msg to a MPCTrajectory object + * @param [in] input trajectory to convert + * @param [out] output resulting MPCTrajectory + * @return true if the conversion was successful + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToMPCTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output); +/** + * @brief convert the given MPCTrajectory to a Trajectory msg + * @param [in] input MPCTrajectory to convert + * @param [out] output resulting Trajectory msg + * @return true if the conversion was successful + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToAutowareTrajectory( + const MPCTrajectory & input, autoware_auto_planning_msgs::msg::Trajectory & output); +/** + * @brief calculate the arc length at each point of the given trajectory + * @param [in] trajectory trajectory for which to calculate the arc length + * @param [out] arclength the cummulative arc length at each point of the trajectory + */ +TRAJECTORY_FOLLOWER_PUBLIC void calcMPCTrajectoryArclength( + const MPCTrajectory & trajectory, std::vector * arclength); +/** + * @brief resample the given trajectory with the given fixed interval + * @param [in] input trajectory to resample + * @param [in] resample_interval_dist the desired distance between two successive trajectory points + * @param [out] output the resampled trajectory + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t resampleMPCTrajectoryByDistance( + const MPCTrajectory & input, const float64_t resample_interval_dist, MPCTrajectory * output); +/** + * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired indexing + * @param [in] in_index indexes for each trajectory point + * @param [in] in_traj MPCTrajectory to interpolate + * @param [in] out_index desired interpolated indexes + * @param [out] out_traj resulting interpolated MPCTrajectory + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpMPCTrajectory( + const std::vector & in_index, const MPCTrajectory & in_traj, + const std::vector & out_index, MPCTrajectory * out_traj); +/** + * @brief fill the relative_time field of the given MPCTrajectory + * @param [in] traj MPCTrajectory for which to fill in the relative_time + * @return true if the calculation was successful + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcMPCTrajectoryTime(MPCTrajectory & traj); +/** + * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing + * @param [in] start_idx index of the trajectory point from which to start smoothing + * @param [in] start_vel initial velocity to set at the start_idx + * @param [in] acc_lim limit on the acceleration + * @param [in] tau constant to control the smoothing (high-value = very smooth) + * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity + */ +TRAJECTORY_FOLLOWER_PUBLIC void dynamicSmoothingVelocity( + const size_t start_idx, const float64_t start_vel, const float64_t acc_lim, const float64_t tau, + MPCTrajectory & traj); +/** + * @brief calculate yaw angle in MPCTrajectory from xy vector + * @param [inout] traj object trajectory + * @param [in] nearest_idx trajectory index nearest to ego + * @param [in] ego_yaw yaw of the ego vehicle + */ +TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY( + MPCTrajectory * traj, const int64_t nearest_idx, const float64_t ego_yaw); +/** + * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 points when num = 1) + * @param [in] curvature_smoothing_num_traj index distance for 3 points for curvature calculation + * @param [in] curvature_smoothing_num_ref_steer index distance for 3 points for smoothed curvature calculation + * @param [inout] traj object trajectory + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcTrajectoryCurvature( + const size_t curvature_smoothing_num_traj, + const size_t curvature_smoothing_num_ref_steer, + MPCTrajectory * traj); +/** + * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 points when num = 1) + * @param [in] curvature_smoothing_num index distance for 3 points for curvature calculation + * @param [in] traj input trajectory + * @return vector of curvatures at each point of the given trajectory + */ +TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( + const size_t curvature_smoothing_num, const MPCTrajectory & traj); +/** + * @brief calculate nearest pose on MPCTrajectory with linear interpolation + * @param [in] traj reference trajectory + * @param [in] self_pose object pose + * @param [out] nearest_pose nearest pose on path + * @param [out] nearest_index path index of nearest pose + * @param [out] nearest_time time of nearest pose on trajectory + * @param [out] logger to output the reason for failure + * @param [in] clock to throttle log output + * @return false when nearest pose couldn't find for some reasons + */ +TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcNearestPoseInterp( + const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, + geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time, + const rclcpp::Logger & logger, rclcpp::Clock & clock); +/** + * @brief calculate the index of the trajectory point nearest to the given pose + * @param [in] traj trajectory to search for the point nearest to the pose + * @param [in] self_pose pose for which to search the nearest trajectory point + * @return index of the input trajectory nearest to the pose + */ +TRAJECTORY_FOLLOWER_PUBLIC int64_t calcNearestIndex( + const MPCTrajectory & traj, + const geometry_msgs::msg::Pose & self_pose); +/** + * @brief calculate the index of the trajectory point nearest to the given pose + * @param [in] traj trajectory to search for the point nearest to the pose + * @param [in] self_pose pose for which to search the nearest trajectory point + * @return index of the input trajectory nearest to the pose + */ +TRAJECTORY_FOLLOWER_PUBLIC int64_t calcNearestIndex( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const geometry_msgs::msg::Pose & self_pose); +/** + * @brief calculate distance to stopped point + */ +TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( + const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, + const int64_t origin); +} // namespace MPCUtils +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/pid.hpp b/control/trajectory_follower/include/trajectory_follower/pid.hpp new file mode 100644 index 0000000000000..1260016da84bf --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/pid.hpp @@ -0,0 +1,109 @@ +// Copyright 2018-2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_FOLLOWER__PID_HPP_ +#define TRAJECTORY_FOLLOWER__PID_HPP_ + +#include + +#include "common/types.hpp" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +/// @brief implementation of a PID controller +class TRAJECTORY_FOLLOWER_PUBLIC PIDController +{ +public: + PIDController(); + + /** + * @brief calculate the output of this PID + * @param [in] error previous error + * @param [in] dt time step [s] + * @param [in] is_integrated if true, will use the integral component for calculation + * @param [out] pid_contributions values of the proportional, integral, and derivative components + * @return PID output + * @throw std::runtime_error if gains or limits have not been set + */ + float64_t calculate( + const float64_t error, const float64_t dt, const bool8_t is_integrated, + std::vector & pid_contributions); + /** + * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms + * @param [in] kp proportional coefficient + * @param [in] ki integral coefficient + * @param [in] kd derivative coefficient + */ + void setGains(const float64_t kp, const float64_t ki, const float64_t kd); + /** + * @brief set limits on the total, proportional, integral, and derivative components + * @param [in] max_ret maximum return value of this PID + * @param [in] min_ret minimum return value of this PID + * @param [in] max_ret_p maximum value of the proportional component + * @param [in] min_ret_p minimum value of the proportional component + * @param [in] max_ret_i maximum value of the integral component + * @param [in] min_ret_i minimum value of the integral component + * @param [in] max_ret_d maximum value of the derivative component + * @param [in] min_ret_d minimum value of the derivative component + */ + void setLimits( + const float64_t max_ret, const float64_t min_ret, const float64_t max_ret_p, + const float64_t min_ret_p, + const float64_t max_ret_i, const float64_t min_ret_i, const float64_t max_ret_d, + const float64_t min_ret_d); + /** + * @brief reset this PID to its initial state + */ + void reset(); + +private: + // PID parameters + struct Params + { + float64_t kp; + float64_t ki; + float64_t kd; + float64_t max_ret_p; + float64_t min_ret_p; + float64_t max_ret_i; + float64_t min_ret_i; + float64_t max_ret_d; + float64_t min_ret_d; + float64_t max_ret; + float64_t min_ret; + }; + Params m_params; + + // state variables + float64_t m_error_integral; + float64_t m_prev_error; + bool8_t m_is_first_time; + bool8_t m_is_gains_set; + bool8_t m_is_limits_set; +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__PID_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp new file mode 100644 index 0000000000000..4881f84198178 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp @@ -0,0 +1,63 @@ +// Copyright 2018-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__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ + +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Dense" +#include "eigen3/Eigen/LU" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::bool8_t; +/// Interface for solvers of Quadratic Programming (QP) problems +class TRAJECTORY_FOLLOWER_PUBLIC QPSolverInterface +{ +public: + /** + * @brief destructor + */ + virtual ~QPSolverInterface() = default; + + /** + * @brief solve QP problem : minimize J = u' * h_mat * u + f_vec' * u without constraint + * @param [in] h_mat parameter matrix in object function + * @param [in] f_vec parameter matrix in object function + * @param [in] a parameter matrix for constraint lb_a < a*u < ub_a + * @param [in] lb parameter matrix for constraint lb < u < ub + * @param [in] ub parameter matrix for constraint lb < u < ub + * @param [in] lb_a parameter matrix for constraint lb_a < a*u < ub_a + * @param [in] ub_a parameter matrix for constraint lb_a < a*u < ub_a + * @param [out] u optimal variable vector + * @return ture if the problem was solved + */ + virtual bool8_t solve( + const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, + const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, + const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0; +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp new file mode 100644 index 0000000000000..6d9eef7821f17 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp @@ -0,0 +1,75 @@ +// Copyright 2018-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__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ + +#include "trajectory_follower/qp_solver/qp_solver_interface.hpp" + +#include "common/types.hpp" +#include "eigen3/Eigen/Dense" +#include "osqp_interface/osqp_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +/// Solver for QP problems using the OSQP library +class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface +{ +public: + /** + * @brief constructor + */ + explicit QPSolverOSQP(const rclcpp::Logger & logger); + + /** + * @brief destructor + */ + virtual ~QPSolverOSQP() = default; + + /** + * @brief solve QP problem : minimize j = u' * h_mat * u + f_vec' * u without constraint + * @param [in] h_mat parameter matrix in object function + * @param [in] f_vec parameter matrix in object function + * @param [in] a parameter matrix for constraint lb_a < a*u < ub_a (not used here) + * @param [in] lb parameter matrix for constraint lb < U < ub (not used here) + * @param [in] ub parameter matrix for constraint lb < U < ub (not used here) + * @param [in] lb_a parameter matrix for constraint lb_a < a*u < ub_a (not used here) + * @param [in] ub_a parameter matrix for constraint lb_a < a*u < ub_a (not used here) + * @param [out] u optimal variable vector + * @return true if the problem was solved + */ + bool8_t solve( + const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, + const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, + const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; + +private: + autoware::common::osqp::OSQPInterface osqpsolver_; + rclcpp::Logger logger_; +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp new file mode 100644 index 0000000000000..c52c25dbf2879 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp @@ -0,0 +1,75 @@ +// Copyright 2018-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__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ + +#include "trajectory_follower/qp_solver/qp_solver_interface.hpp" + +#include + +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Dense" +#include "eigen3/Eigen/LU" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::bool8_t; +/** + * Solver for QP problems using least square decomposition + * implement with Eigen's standard Cholesky decomposition (LLT) + */ +class TRAJECTORY_FOLLOWER_PUBLIC QPSolverEigenLeastSquareLLT : public QPSolverInterface +{ +public: + /** + * @brief constructor + */ + QPSolverEigenLeastSquareLLT(); + + /** + * @brief destructor + */ + ~QPSolverEigenLeastSquareLLT() = default; + + /** + * @brief solve QP problem : minimize j = u' * h_mat * u + f_vec' * u without constraint + * @param [in] h_mat parameter matrix in object function + * @param [in] f_vec parameter matrix in object function + * @param [in] a parameter matrix for constraint lb_a < a*u < ub_a (not used here) + * @param [in] lb parameter matrix for constraint lb < U < ub (not used here) + * @param [in] ub parameter matrix for constraint lb < U < ub (not used here) + * @param [in] lb_a parameter matrix for constraint lb_a < a*u < ub_a (not used here) + * @param [in] ub_a parameter matrix for constraint lb_a < a*u < ub_a (not used here) + * @param [out] u optimal variable vector + * @return true if the problem was solved + */ + bool8_t solve( + const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, + const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, + const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp b/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp new file mode 100644 index 0000000000000..5d639cbe23696 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp @@ -0,0 +1,127 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ +#define TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ + +#include +#include +#include // NOLINT +#include +#include +#include + +#include "common/types.hpp" +#include "rclcpp/rclcpp.hpp" +#include "trajectory_follower/visibility_control.hpp" + + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +/** + * @brief Smooth stop class to implement vehicle specific deceleration profiles + */ +class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop +{ +public: + /** + * @brief initialize the state of the smooth stop + * @param [in] pred_vel_in_target predicted ego velocity when the stop command will be executed + * @param [in] pred_stop_dist predicted stop distance when the stop command will be executed + */ + void init(const float64_t pred_vel_in_target, const float64_t pred_stop_dist); + + /** + * @brief set the parameters of this smooth stop + * @param [in] max_strong_acc maximum strong acceleration value [m/s²] + * @param [in] min_strong_acc minumum strong acceleration value [m/s²] + * @param [in] weak_acc weak acceleration value [m/s²] + * @param [in] weak_stop_acc weak stopping acceleration value [m/s²] + * @param [in] strong_stop_acc strong stopping acceleration value [m/s²] + * @param [in] min_fast_vel minumum velocity to consider ego to be running fast [m/s] + * @param [in] min_running_vel minimum velocity to consider ego to be running [m/s] + * @param [in] min_running_acc minimum acceleration to consider ego to be running [m/s] + * @param [in] weak_stop_time time allowed for stopping with a weak acceleration [s] + * @param [in] weak_stop_dist distance to the stop point bellow which a weak accel is applied [m] + * @param [in] strong_stop_dist distance to the stop point bellow which a strong accel is applied [m] + */ + void setParams( + float64_t max_strong_acc, float64_t min_strong_acc, float64_t weak_acc, float64_t weak_stop_acc, + float64_t strong_stop_acc, float64_t min_fast_vel, float64_t min_running_vel, + float64_t min_running_acc, + float64_t weak_stop_time, float64_t weak_stop_dist, float64_t strong_stop_dist); + + /** + * @brief predict time when car stops by fitting some latest observed velocity history + * with linear function (v = at + b) + * @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, float64_t[m/s]) pairs + * @throw std::runtime_error if parameters have not been set + */ + std::experimental::optional calcTimeToStop( + const std::vector> & vel_hist) const; + + /** + * @brief calculate accel command while stopping + * Decrease velocity with m_strong_acc, + * then loose brake pedal with m_params.weak_acc to stop smoothly + * If the car is still running, input m_params.weak_stop_acc + * and then m_params.strong_stop_acc in steps not to exceed stopline too much + * @param [in] stop_dist distance left to travel before stopping [m] + * @param [in] current_vel current velocity of ego [m/s] + * @param [in] current_acc current acceleration of ego [m/s²] + * @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, float64_t[m/s]) pairs + * @param [in] delay_time assumed time delay when the stop command will actually be executed + * @throw std::runtime_error if parameters have not been set + */ + float64_t calculate( + const float64_t stop_dist, const float64_t current_vel, const float64_t current_acc, + const std::vector> & vel_hist, const float64_t delay_time); + +private: + struct Params + { + float64_t max_strong_acc; + float64_t min_strong_acc; + float64_t weak_acc; + float64_t weak_stop_acc; + float64_t strong_stop_acc; + + float64_t min_fast_vel; + float64_t min_running_vel; + float64_t min_running_acc; + float64_t weak_stop_time; + + float64_t weak_stop_dist; + float64_t strong_stop_dist; + }; + Params m_params; + + float64_t m_strong_acc; + rclcpp::Time m_weak_acc_time; + bool8_t m_is_set_params = false; +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp new file mode 100644 index 0000000000000..7313361572ac7 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -0,0 +1,121 @@ +// Copyright 2018-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. + +/* + * Representation + * e : lateral error + * de : derivative of lateral error + * th : heading angle error + * dth : derivative of heading angle error + * steer : steering angle (input) + * v : velocity + * m : mass + * Iz : inertia + * lf : length from center to front tire + * lr : length from center to rear tire + * cf : front tire cornering power + * cr : rear tire cornering power + * k : curvature on reference trajectory point + * + * State & Input + * x = [e, de, th, dth]^T + * u = steer + * + * Linearized model around reference point (v=vr) + * [0, 1, 0, 0] [ 0] [ 0] dx/dt = [0, + * -(cf+cr)/m/vr, (cf+cr)/m, (lr*cr-lf*cf)/m/vr] * x + [ cf/m] * u + [(lr*cr-lf*cf)/m/vr*k - vr*k] [0, + * 0, 0, 1] [ 0] [ 0] [0, + * (lr*cr-lf*cf)/Iz/vr, (lf*cf-lr*cr)/Iz, -(lf^2*cf+lr^2*cr)/Iz/vr] [lf*cf/Iz] [ -(lf^2*cf+lr^2*cr)/Iz/vr] + * + * Reference : Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", Robotics + * Institute, Carnegie Mellon University, February 2009. + */ + +#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ + +#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" + +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +/** + * Vehicle model class of bicycle dynamics + * @brief calculate model-related values + */ +class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInterface +{ +public: + /** + * @brief constructor with parameter initialization + * @param [in] wheelbase wheelbase length [m] + * @param [in] mass_fl mass applied to front left tire [kg] + * @param [in] mass_fr mass applied to front right tire [kg] + * @param [in] mass_rl mass applied to rear left tire [kg] + * @param [in] mass_rr mass applied to rear right tire [kg] + * @param [in] cf front cornering power [N/rad] + * @param [in] cr rear cornering power [N/rad] + */ + DynamicsBicycleModel( + const float64_t wheelbase, const float64_t mass_fl, const float64_t mass_fr, + const float64_t mass_rl, const float64_t mass_rr, + const float64_t cf, const float64_t cr); + + /** + * @brief destructor + */ + ~DynamicsBicycleModel() = default; + + /** + * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk + * @param [in] a_d coefficient matrix + * @param [in] b_d coefficient matrix + * @param [in] c_d coefficient matrix + * @param [in] w_d coefficient matrix + * @param [in] dt Discretization time [s] + */ + void calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & w_d, Eigen::MatrixXd & c_d, + const float64_t dt) override; + + /** + * @brief calculate reference input + * @param [out] u_ref input + */ + void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; + +private: + float64_t m_lf; //!< @brief length from center of mass to front wheel [m] + float64_t m_lr; //!< @brief length from center of mass to rear wheel [m] + float64_t m_mass; //!< @brief total mass of vehicle [kg] + float64_t m_iz; //!< @brief moment of inertia [kg * m2] + float64_t m_cf; //!< @brief front cornering power [N/rad] + float64_t m_cr; //!< @brief rear cornering power [N/rad] +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp new file mode 100644 index 0000000000000..3470c9ad583c9 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -0,0 +1,108 @@ +// Copyright 2018-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. + +/* + * Representation + * e : lateral error + * th : heading angle error + * steer : steering angle + * steer_d: desired steering angle (input) + * v : velocity + * W : wheelbase length + * tau : time constant for steering dynamics + * + * State & Input + * x = [e, th, steer]^T + * u = steer_d + * + * Nonlinear model + * dx1/dt = v * sin(x2) + * dx2/dt = v * tan(x3) / W + * dx3/dt = -(x3 - u) / tau + * + * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) + * [0, vr, 0] [ 0] [ 0] + * dx/dt = [0, 0, vr/W/cos(steer_r)^2] * x + [ 0] * u + [-vr*steer_r/W/cos(steer_r)^2] + * [0, 0, 1/tau] [1/tau] [ 0] + * + */ + +#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ + +#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" + +#include "trajectory_follower/visibility_control.hpp" + +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +/** + * Vehicle model class of bicycle kinematics + * @brief calculate model-related values + */ +class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInterface +{ +public: + /** + * @brief constructor with parameter initialization + * @param [in] wheelbase wheelbase length [m] + * @param [in] steer_lim steering angle limit [rad] + * @param [in] steer_tau steering time constant for 1d-model [s] + */ + KinematicsBicycleModel( + const float64_t wheelbase, const float64_t steer_lim, const float64_t steer_tau); + + /** + * @brief destructor + */ + ~KinematicsBicycleModel() = default; + + /** + * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk + * @param [out] a_d coefficient matrix + * @param [out] b_d coefficient matrix + * @param [out] c_d coefficient matrix + * @param [out] w_d coefficient matrix + * @param [in] dt Discretization time [s] + */ + void calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const float64_t dt) override; + + /** + * @brief calculate reference input + * @param [out] u_ref input + */ + void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; + +private: + float64_t m_steer_lim; //!< @brief steering angle limit [rad] + float64_t m_steer_tau; //!< @brief steering time constant for 1d-model [s] +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp new file mode 100644 index 0000000000000..9436b22096b7b --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -0,0 +1,101 @@ +// Copyright 2018-2021 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. + +/* + * Representation + * e : lateral error + * th : heading angle error + * steer : steering angle (input) + * v : velocity + * W : wheelbase length + * tau : time constant for steering dynamics + * + * State & Input + * x = [e, th]^T + * u = steer + * + * Nonlinear model + * dx1/dt = v * sin(x2) + * dx2/dt = v * tan(u) / W + * + * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) + * dx/dt = [0, vr] * x + [ 0] * u + [ 0] + * [0, 0] [vr/W/cos(steer_r)^2] [-vr*steer_r/W/cos(steer_r)^2] + * + */ + +#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ + +#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" + +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +/** + * Vehicle model class of bicycle kinematics without steering delay + * @brief calculate model-related values + */ +class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleModelInterface +{ +public: + /** + * @brief constructor with parameter initialization + * @param [in] wheelbase wheelbase length [m] + * @param [in] steer_lim steering angle limit [rad] + */ + KinematicsBicycleModelNoDelay(const float64_t wheelbase, const float64_t steer_lim); + + /** + * @brief destructor + */ + ~KinematicsBicycleModelNoDelay() = default; + + /** + * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk + * @param [out] a_d coefficient matrix + * @param [out] b_d coefficient matrix + * @param [out] c_d coefficient matrix + * @param [out] w_d coefficient matrix + * @param [in] dt Discretization time [s] + */ + void calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const float64_t dt) override; + + /** + * @brief calculate reference input + * @param [out] u_ref input + */ + void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; + +private: + float64_t m_steer_lim; //!< @brief steering angle limit [rad] +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp new file mode 100644 index 0000000000000..6ad22852c2e8a --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp @@ -0,0 +1,118 @@ +// Copyright 2018-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__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ + +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "trajectory_follower/visibility_control.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::float64_t; +/** + * Vehicle model class + * @brief calculate model-related values + */ +class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface +{ +protected: + const int64_t m_dim_x; //!< @brief dimension of state x + const int64_t m_dim_u; //!< @brief dimension of input u + const int64_t m_dim_y; //!< @brief dimension of output y + float64_t m_velocity; //!< @brief vehicle velocity [m/s] + float64_t m_curvature; //!< @brief curvature on the linearized point on path + float64_t m_wheelbase; //!< @brief wheelbase of the vehicle [m] + +public: + /** + * @brief constructor + * @param [in] dim_x dimension of state x + * @param [in] dim_u dimension of input u + * @param [in] dim_y dimension of output y + * @param [in] wheelbase wheelbase of the vehicle [m] + */ + VehicleModelInterface(int64_t dim_x, int64_t dim_u, int64_t dim_y, float64_t wheelbase); + + /** + * @brief destructor + */ + virtual ~VehicleModelInterface() = default; + + /** + * @brief get state x dimension + * @return state dimension + */ + int64_t getDimX(); + + /** + * @brief get input u dimension + * @return input dimension + */ + int64_t getDimU(); + + /** + * @brief get output y dimension + * @return output dimension + */ + int64_t getDimY(); + + /** + * @brief get wheelbase of the vehicle + * @return wheelbase value [m] + */ + float64_t getWheelbase(); + + /** + * @brief set velocity + * @param [in] velocity vehicle velocity + */ + void setVelocity(const float64_t velocity); + + /** + * @brief set curvature + * @param [in] curvature curvature on the linearized point on path + */ + void setCurvature(const float64_t curvature); + + /** + * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk + * @param [out] a_d coefficient matrix + * @param [out] b_d coefficient matrix + * @param [out] c_d coefficient matrix + * @param [out] w_d coefficient matrix + * @param [in] dt Discretization time [s] + */ + virtual void calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const float64_t dt) = 0; + + /** + * @brief calculate reference input + * @param [out] u_ref input + */ + virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0; +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware +#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp b/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp new file mode 100644 index 0000000000000..655d7b5160c9c --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/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__VISIBILITY_CONTROL_HPP_ +#define TRAJECTORY_FOLLOWER__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__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml new file mode 100644 index 0000000000000..f94336c6e31a5 --- /dev/null +++ b/control/trajectory_follower/package.xml @@ -0,0 +1,38 @@ + + + + trajectory_follower + 1.0.0 + Library for generating lateral and longitudinal controls following a trajectory + Maxime CLEMENT + Apache 2.0 + + ament_cmake_auto + autoware_auto_cmake + + autoware_auto_common + autoware_auto_planning_msgs + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + autoware_auto_system_msgs + autoware_auto_geometry + eigen + geometry_msgs + interpolation + motion_common + osqp_interface + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + + ament_cmake_gtest + + + eigen + + + ament_cmake + + diff --git a/control/trajectory_follower/src/interpolate.cpp b/control/trajectory_follower/src/interpolate.cpp new file mode 100644 index 0000000000000..4abff5c4ddfe3 --- /dev/null +++ b/control/trajectory_follower/src/interpolate.cpp @@ -0,0 +1,139 @@ +// Copyright 2018-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 "trajectory_follower/interpolate.hpp" + +#include +#include +#include + +/* + * linear interpolation + */ + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +namespace +{ +bool8_t isIncrease(const std::vector & x) +{ + for (size_t i = 0; i < x.size() - 1; ++i) { + if (x.at(i) > x.at(i + 1)) {return false;} + } + return true; +} + +bool8_t isValidInput( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index) +{ + if (base_index.empty() || base_value.empty() || return_index.empty()) { + std::cerr << "mpc bad index : some vector is empty. base_index: " << base_index.size() << + ", base_value: " << base_value.size() << ", return_index: " << return_index.size() << + std::endl; + return false; + } + if (!isIncrease(base_index)) { + std::cerr << "mpc bad index : base_index is not monotonically increasing. base_index = [" << + base_index.front() << ", " << base_index.back() << "]" << std::endl; + return false; + } + if (!isIncrease(return_index)) { + std::cerr << "mpc bad index : base_index is not monotonically increasing. return_index = [" << + return_index.front() << ", " << return_index.back() << "]" << std::endl; + return false; + } + if (return_index.front() < base_index.front()) { + std::cerr << "mpc bad index : return_index.front() < base_index.front()" << std::endl; + return false; + } + if (base_index.back() < return_index.back()) { + std::cerr << "mpc bad index : base_index.back() < return_index.back()" << std::endl; + return false; + } + if (base_index.size() != base_value.size()) { + std::cerr << "mpc bad index : base_index.size() != base_value.size()" << std::endl; + return false; + } + + return true; +} +} // namespace + +bool8_t linearInterpolate( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index, std::vector & return_value) +{ + // check if inputs are valid + if (!isValidInput(base_index, base_value, return_index)) { + std::cerr << "[mpc linear interpolate] invalid input. interpolation failed." << std::endl; + return false; + } + + // calculate linear interpolation + size_t i = 0; + const size_t base_size = base_index.size(); + for (const auto idx : return_index) { + if (base_index.at(i) == idx) { + return_value.push_back(base_value.at(i)); + continue; + } + while (base_index.at(i) < idx) { + ++i; + if (i <= 0 || base_size - 1 < i) {break;} + } + + if (i <= 0 || base_size - 1 < i) { + std::cerr << "mpc LinearInterpolate : undesired condition. skip index. (i = " << i << + ", base_size = " << base_size << "), idx = " << idx << + ", base_index.at(i) = " << base_index.at(i) << std::endl; + continue; + } + + const float64_t dist_base_idx = base_index.at(i) - base_index.at(i - 1); + const float64_t dist_to_forward = base_index.at(i) - idx; + const float64_t dist_to_backward = idx - base_index.at(i - 1); + + const float64_t value = + (dist_to_backward * base_value.at(i) + dist_to_forward * base_value.at(i - 1)) / + dist_base_idx; + return_value.push_back(value); + } + return true; +} + +bool8_t linearInterpolate( + const std::vector & base_index, const std::vector & base_value, + const float64_t & return_index, float64_t & return_value) +{ + std::vector return_index_v; + return_index_v.push_back(return_index); + + std::vector return_value_v; + if (!linearInterpolate(base_index, base_value, return_index_v, return_value_v)) { + return false; + } + return_value = return_value_v.at(0); + return true; +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp new file mode 100644 index 0000000000000..18d3cbe047aee --- /dev/null +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -0,0 +1,187 @@ +// Copyright 2018-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 "trajectory_follower/longitudinal_controller_utils.hpp" + +#include +#include // NOLINT +#include + +#include "motion_common/motion_common.hpp" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +namespace longitudinal_utils +{ + +bool isValidTrajectory(const Trajectory & traj) +{ + for (const auto & p : traj.points) { + if ( + !isfinite(p.pose.position.x) || !isfinite(p.pose.position.y) || + !isfinite(p.pose.position.z) || !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.acceleration_mps2) || + !isfinite(p.heading_rate_rps)) + { + return false; + } + } + + // when trajectory is empty + if (traj.points.empty()) { + return false; + } + + return true; +} + +float64_t calcStopDistance( + const Point & current_pos, const Trajectory & traj) +{ + const std::experimental::optional stop_idx_opt = + trajectory_common::searchZeroVelocityIndex(traj.points); + + // If no zero velocity point, return the length between current_pose to the end of trajectory. + if (!stop_idx_opt) { + return trajectory_common::calcSignedArcLength(traj.points, current_pos, traj.points.size() - 1); + } + + return trajectory_common::calcSignedArcLength(traj.points, current_pos, *stop_idx_opt); +} + +float64_t getPitchByPose(const Quaternion & quaternion_msg) +{ + float64_t roll, pitch, yaw; + tf2::Quaternion quaternion; + tf2::fromMsg(quaternion_msg, quaternion); + tf2::Matrix3x3{quaternion}.getRPY(roll, pitch, yaw); + + return pitch; +} + +float64_t getPitchByTraj( + const Trajectory & trajectory, const size_t nearest_idx, const float64_t wheel_base) +{ + using autoware::common::geometry::distance_2d; + // cannot calculate pitch + if (trajectory.points.size() <= 1) { + return 0.0; + } + + for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { + const float64_t dist = + distance_2d(trajectory.points.at(nearest_idx), trajectory.points.at(i)); + if (dist > wheel_base) { + // calculate pitch from trajectory between rear wheel (nearest) and front center (i) + return calcElevationAngle( + trajectory.points.at(nearest_idx), trajectory.points.at(i)); + } + } + + // close to goal + for (size_t i = trajectory.points.size() - 1; i > 0; --i) { + const float64_t dist = + distance_2d(trajectory.points.back(), trajectory.points.at(i)); + + if (dist > wheel_base) { + // calculate pitch from trajectory + // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) + return calcElevationAngle( + trajectory.points.at(i), trajectory.points.back()); + } + } + + // calculate pitch from trajectory between the beginning and end of trajectory + return calcElevationAngle( + trajectory.points.at(0), + trajectory.points.back()); +} + +float64_t calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) +{ + const float64_t dx = p_from.pose.position.x - p_to.pose.position.x; + const float64_t dy = p_from.pose.position.y - p_to.pose.position.y; + const float64_t dz = p_from.pose.position.z - p_to.pose.position.z; + + const float64_t dxy = std::max(std::hypot(dx, dy), std::numeric_limits::epsilon()); + const float64_t pitch = std::atan2(dz, dxy); + + return pitch; +} + +Pose calcPoseAfterTimeDelay( + const Pose & current_pose, const float64_t delay_time, const float64_t current_vel) +{ + // simple linear prediction + const float64_t yaw = ::motion::motion_common::to_angle(current_pose.orientation); + const float64_t running_distance = delay_time * current_vel; + const float64_t dx = running_distance * std::cos(yaw); + const float64_t dy = running_distance * std::sin(yaw); + + auto pred_pose = current_pose; + pred_pose.position.x += dx; + pred_pose.position.y += dy; + return pred_pose; +} + +float64_t lerp(const float64_t v_from, const float64_t v_to, const float64_t ratio) +{ + return v_from + (v_to - v_from) * ratio; +} + +Quaternion lerpOrientation( + const Quaternion & o_from, const Quaternion & o_to, const float64_t ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} + +float64_t applyDiffLimitFilter( + const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t max_val, + const float64_t min_val) +{ + const float64_t diff_raw = (input_val - prev_val) / dt; + const float64_t diff = std::min(std::max(diff_raw, min_val), max_val); + const float64_t filtered_val = prev_val + diff * dt; + return filtered_val; +} + +float64_t applyDiffLimitFilter( + const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t lim_val) +{ + const float64_t max_val = std::fabs(lim_val); + const float64_t min_val = -max_val; + return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); +} +} // namespace longitudinal_utils +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/lowpass_filter.cpp b/control/trajectory_follower/src/lowpass_filter.cpp new file mode 100644 index 0000000000000..4098a16a61eba --- /dev/null +++ b/control/trajectory_follower/src/lowpass_filter.cpp @@ -0,0 +1,151 @@ +// Copyright 2018-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 "trajectory_follower/lowpass_filter.hpp" + +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +Butterworth2dFilter::Butterworth2dFilter(float64_t dt, float64_t f_cutoff_hz) +{ + initialize(dt, f_cutoff_hz); +} + +Butterworth2dFilter::~Butterworth2dFilter() {} + +void Butterworth2dFilter::initialize(const float64_t & dt, const float64_t & f_cutoff_hz) +{ + m_y1 = 0.0; + m_y2 = 0.0; + m_u2 = 0.0; + m_u1 = 0.0; + + /* 2d butterworth lowpass filter with bi-linear transformation */ + float64_t wc = 2.0 * M_PI * f_cutoff_hz; + float64_t n = 2 / dt; + m_a0 = n * n + sqrt(2) * wc * n + wc * wc; + m_a1 = 2 * wc * wc - 2 * n * n; + m_a2 = n * n - sqrt(2) * wc * n + wc * wc; + m_b0 = wc * wc; + m_b1 = 2 * m_b0; + m_b2 = m_b0; +} + +float64_t Butterworth2dFilter::filter(const float64_t & u0) +{ + float64_t y0 = (m_b2 * m_u2 + m_b1 * m_u1 + m_b0 * u0 - m_a2 * m_y2 - m_a1 * m_y1) / m_a0; + m_y2 = m_y1; + m_y1 = y0; + m_u2 = m_u1; + m_u1 = u0; + return y0; +} + +void Butterworth2dFilter::filt_vector( + const std::vector & t, + std::vector & u) const +{ + u.resize(t.size()); + float64_t y1 = t.at(0); + float64_t y2 = t.at(0); + float64_t u2 = t.at(0); + float64_t u1 = t.at(0); + float64_t y0 = 0.0; + float64_t u0 = 0.0; + for (size_t i = 0; i < t.size(); ++i) { + u0 = t.at(i); + y0 = (m_b2 * u2 + m_b1 * u1 + m_b0 * u0 - m_a2 * y2 - m_a1 * y1) / m_a0; + y2 = y1; + y1 = y0; + u2 = u1; + u1 = u0; + u.at(i) = y0; + } +} + +// filtering forward and backward direction +void Butterworth2dFilter::filtfilt_vector( + const std::vector & t, + std::vector & u) const +{ + std::vector t_fwd(t); + std::vector t_rev(t); + + // forward filtering + filt_vector(t, t_fwd); + + // backward filtering + std::reverse(t_rev.begin(), t_rev.end()); + filt_vector(t, t_rev); + std::reverse(t_rev.begin(), t_rev.end()); + + // merge + u.clear(); + for (size_t i = 0; i < t.size(); ++i) { + u.push_back((t_fwd[i] + t_rev[i]) * 0.5); + } +} + +void Butterworth2dFilter::getCoefficients(std::vector & coeffs) const +{ + coeffs.clear(); + coeffs.push_back(m_a0); + coeffs.push_back(m_a1); + coeffs.push_back(m_a2); + coeffs.push_back(m_b0); + coeffs.push_back(m_b1); + coeffs.push_back(m_b2); +} + +namespace MoveAverageFilter +{ +bool8_t filt_vector(const int64_t num, std::vector & u) +{ + if (static_cast(u.size()) < num) { + return false; + } + std::vector filtered_u(u); + for (int64_t i = 0; i < static_cast(u.size()); ++i) { + float64_t tmp = 0.0; + int64_t num_tmp = 0; + float64_t count = 0; + if (i - num < 0) { + num_tmp = i; + } else if (i + num > static_cast(u.size()) - 1) { + num_tmp = static_cast(u.size()) - i - 1; + } else { + num_tmp = num; + } + + for (int64_t j = -num_tmp; j <= num_tmp; ++j) { + tmp += u[static_cast(i + j)]; + ++count; + } + filtered_u[static_cast(i)] = tmp / count; + } + u = filtered_u; + return true; +} +} // namespace MoveAverageFilter +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp new file mode 100644 index 0000000000000..3a34cef476f98 --- /dev/null +++ b/control/trajectory_follower/src/mpc.cpp @@ -0,0 +1,833 @@ +// Copyright 2018-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/mpc.hpp" + +#define DEG2RAD 3.1415926535 / 180.0 +#define RAD2DEG 180.0 / 3.1415926535 + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using namespace std::chrono_literals; +using ::motion::motion_common::to_angle; + +bool8_t MPC::calculateMPC( + const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, + const float64_t current_velocity, + const geometry_msgs::msg::Pose & current_pose, + autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd, + autoware_auto_planning_msgs::msg::Trajectory & predicted_traj, + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) +{ + /* recalculate velocity from ego-velocity with dynamics */ + trajectory_follower::MPCTrajectory reference_trajectory = + applyVelocityDynamicsFilter(m_ref_traj, current_pose, current_velocity); + + MPCData mpc_data; + if (!getData(reference_trajectory, current_steer, current_pose, &mpc_data)) { + RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "fail to get Data."); + return false; + } + + /* define initial state for error dynamics */ + Eigen::VectorXd x0 = getInitialState(mpc_data); + + /* delay compensation */ + + if (!updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, &x0)) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + m_logger, *m_clock, 1000 /*ms*/, + "updateStateForDelayCompensation failed. stop computation."); + return false; + } + + /* resample ref_traj with mpc sampling time */ + trajectory_follower::MPCTrajectory mpc_resampled_ref_traj; + const float64_t mpc_start_time = mpc_data.nearest_time + m_param.input_delay; + if (!resampleMPCTrajectoryByTime(mpc_start_time, reference_trajectory, &mpc_resampled_ref_traj)) { + RCLCPP_WARN_THROTTLE( + m_logger, *m_clock, + 1000 /*ms*/, "trajectory resampling failed."); + return false; + } + + /* generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex */ + MPCMatrix mpc_matrix = generateMPCMatrix(mpc_resampled_ref_traj); + + /* solve quadratic optimization */ + Eigen::VectorXd Uex; + if (!executeOptimization(mpc_matrix, x0, &Uex)) { + RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "optimization failed."); + return false; + } + + /* apply saturation and filter */ + const float64_t u_saturated = std::max(std::min(Uex(0), m_steer_lim), -m_steer_lim); + const float64_t u_filtered = m_lpf_steering_cmd.filter(u_saturated); + + /* set control command */ + { + const auto & dt = m_param.prediction_dt; + ctrl_cmd.steering_tire_angle = static_cast(u_filtered); + ctrl_cmd.steering_tire_rotation_rate = static_cast((Uex(1) - Uex(0)) / dt); + } + + storeSteerCmd(u_filtered); + + /* save input to buffer for delay compensation*/ + m_input_buffer.push_back(ctrl_cmd.steering_tire_angle); + m_input_buffer.pop_front(); + m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; + m_raw_steer_cmd_prev = Uex(0); + + /* calculate predicted trajectory */ + Eigen::VectorXd Xex = mpc_matrix.Aex * x0 + mpc_matrix.Bex * Uex + mpc_matrix.Wex; + trajectory_follower::MPCTrajectory mpc_predicted_traj; + const auto & traj = mpc_resampled_ref_traj; + for (size_t i = 0; i < static_cast(m_param.prediction_horizon); ++i) { + const int64_t DIM_X = m_vehicle_model_ptr->getDimX(); + const float64_t lat_error = Xex(static_cast(i) * DIM_X); + const float64_t yaw_error = Xex(static_cast(i) * DIM_X + 1); + const float64_t x = traj.x[i] - std::sin(traj.yaw[i]) * lat_error; + const float64_t y = traj.y[i] + std::cos(traj.yaw[i]) * lat_error; + const float64_t z = traj.z[i]; + const float64_t yaw = traj.yaw[i] + yaw_error; + const float64_t vx = traj.vx[i]; + const float64_t k = traj.k[i]; + const float64_t smooth_k = traj.smooth_k[i]; + const float64_t relative_time = traj.relative_time[i]; + mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time); + } + trajectory_follower::MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj); + + /* prepare diagnostic message */ + const float64_t nearest_k = reference_trajectory.k[static_cast(mpc_data.nearest_idx)]; + const float64_t nearest_smooth_k = + reference_trajectory.smooth_k[static_cast(mpc_data.nearest_idx)]; + const float64_t steer_cmd = ctrl_cmd.steering_tire_angle; + const float64_t wb = m_vehicle_model_ptr->getWheelbase(); + + typedef decltype (diagnostic.diag_array.data) ::value_type DiagnosticValueType; + auto append_diag_data = [&](const auto & val) -> void { + diagnostic.diag_array.data.push_back(static_cast(val)); + }; + // [0] final steering command (MPC + LPF) + append_diag_data(steer_cmd); + // [1] mpc calculation result + append_diag_data(Uex(0)); + // [2] feedforward steering value + append_diag_data(mpc_matrix.Uref_ex(0)); + // [3] feedforward steering value raw + append_diag_data(std::atan(nearest_smooth_k * wb)); + // [4] current steering angle + append_diag_data(mpc_data.steer); + // [5] lateral error + append_diag_data(mpc_data.lateral_err); + // [6] current_pose yaw + append_diag_data(to_angle(current_pose.orientation)); + // [7] nearest_pose yaw + append_diag_data(to_angle(mpc_data.nearest_pose.orientation)); + // [8] yaw error + append_diag_data(mpc_data.yaw_err); + // [9] reference velocity + append_diag_data(reference_trajectory.vx[static_cast(mpc_data.nearest_idx)]); + // [10] measured velocity + append_diag_data(current_velocity); + // [11] angvel from steer command + append_diag_data(current_velocity * tan(steer_cmd) / wb); + // [12] angvel from measured steer + append_diag_data(current_velocity * tan(mpc_data.steer) / wb); + // [13] angvel from path curvature + append_diag_data(current_velocity * nearest_smooth_k); + // [14] nearest path curvature (used for feedforward) + append_diag_data(nearest_smooth_k); + // [15] nearest path curvature (not smoothed) + append_diag_data(nearest_k); + // [16] predicted steer + append_diag_data(mpc_data.predicted_steer); + // [17] angvel from predicted steer + append_diag_data(current_velocity * tan(mpc_data.predicted_steer) / wb); + + return true; +} + +void MPC::setReferenceTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, + const float64_t traj_resample_dist, + const bool8_t enable_path_smoothing, + const int64_t path_filter_moving_ave_num, + const int64_t curvature_smoothing_num_traj, + const int64_t curvature_smoothing_num_ref_steer, + const geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr) +{ + trajectory_follower::MPCTrajectory mpc_traj_raw; // received raw trajectory + trajectory_follower::MPCTrajectory mpc_traj_resampled; // resampled trajectory + trajectory_follower::MPCTrajectory mpc_traj_smoothed; // smooth filtered trajectory + + /* resampling */ + trajectory_follower::MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw); + if (!trajectory_follower::MPCUtils::resampleMPCTrajectoryByDistance( + mpc_traj_raw, traj_resample_dist, &mpc_traj_resampled)) + { + RCLCPP_WARN(m_logger, "[setReferenceTrajectory] spline error when resampling by distance"); + return; + } + + /* path smoothing */ + mpc_traj_smoothed = mpc_traj_resampled; + const int64_t mpc_traj_resampled_size = static_cast(mpc_traj_resampled.size()); + if (enable_path_smoothing && mpc_traj_resampled_size > 2 * path_filter_moving_ave_num) { + if ( + !trajectory_follower::MoveAverageFilter::filt_vector( + path_filter_moving_ave_num, + mpc_traj_smoothed.x) || + !trajectory_follower::MoveAverageFilter::filt_vector( + path_filter_moving_ave_num, + mpc_traj_smoothed.y) || + !trajectory_follower::MoveAverageFilter::filt_vector( + path_filter_moving_ave_num, + mpc_traj_smoothed.yaw) || + !trajectory_follower::MoveAverageFilter::filt_vector( + path_filter_moving_ave_num, + mpc_traj_smoothed.vx)) + { + RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering."); + mpc_traj_smoothed = mpc_traj_resampled; + } + } + + /* calculate yaw angle */ + if (current_pose_ptr) { + const int64_t nearest_idx = + MPCUtils::calcNearestIndex(mpc_traj_smoothed, current_pose_ptr->pose); + const float64_t ego_yaw = tf2::getYaw(current_pose_ptr->pose.orientation); + trajectory_follower::MPCUtils::calcTrajectoryYawFromXY( + &mpc_traj_smoothed, nearest_idx, + ego_yaw); + trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); + } + + /* calculate curvature */ + trajectory_follower::MPCUtils::calcTrajectoryCurvature( + static_cast( + curvature_smoothing_num_traj), + static_cast( + curvature_smoothing_num_ref_steer), + &mpc_traj_smoothed); + + /* add end point with vel=0 on traj for mpc prediction */ + { + auto & t = mpc_traj_smoothed; + const float64_t t_ext = 100.0; // extra time to prevent mpc calcul failure due to short time + const float64_t t_end = t.relative_time.back() + getPredictionTime() + t_ext; + const float64_t v_end = 0.0; + t.vx.back() = v_end; // set for end point + t.push_back( + t.x.back(), t.y.back(), t.z.back(), t.yaw.back(), v_end, t.k.back(), t.smooth_k.back(), + t_end); + } + + if (!mpc_traj_smoothed.size()) { + RCLCPP_DEBUG(m_logger, "path callback: trajectory size is undesired."); + return; + } + + m_ref_traj = mpc_traj_smoothed; +} + +void MPC::resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer) +{ + m_raw_steer_cmd_prev = current_steer.steering_tire_angle; + m_raw_steer_cmd_pprev = current_steer.steering_tire_angle; +} + +bool8_t MPC::getData( + const trajectory_follower::MPCTrajectory & traj, + const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, + const geometry_msgs::msg::Pose & current_pose, + MPCData * data) +{ + static constexpr auto duration = 5000 /*ms*/; + size_t nearest_idx; + if (!trajectory_follower::MPCUtils::calcNearestPoseInterp( + traj, current_pose, &(data->nearest_pose), &(nearest_idx), + &(data->nearest_time), m_logger, *m_clock)) + { + // reset previous MPC result + // Note: When a large deviation from the trajectory occurs, the optimization stops and + // the vehicle will return to the path by re-planning the trajectory or external operation. + // After the recovery, the previous value of the optimization may deviate greatly from + // the actual steer angle, and it may make the optimization result unstable. + resetPrevResult(current_steer); + RCLCPP_WARN_SKIPFIRST_THROTTLE( + m_logger, *m_clock, duration, + "calculateMPC: error in calculating nearest pose. stop mpc."); + return false; + } + + /* get data */ + data->nearest_idx = static_cast(nearest_idx); + data->steer = static_cast(current_steer.steering_tire_angle); + data->lateral_err = trajectory_follower::MPCUtils::calcLateralError( + current_pose, + data->nearest_pose); + data->yaw_err = autoware::common::helper_functions::wrap_angle( + to_angle(current_pose.orientation) - + to_angle(data->nearest_pose.orientation)); + + /* get predicted steer */ + if (!m_steer_prediction_prev) { + m_steer_prediction_prev = std::make_shared(current_steer.steering_tire_angle); + } + data->predicted_steer = calcSteerPrediction(); + *m_steer_prediction_prev = data->predicted_steer; + + /* check error limit */ + const float64_t dist_err = autoware::common::geometry::distance_2d( + current_pose.position, + data->nearest_pose.position); + if (dist_err > m_admissible_position_error) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + m_logger, *m_clock, duration, "position error is over limit. error = %fm, limit: %fm", + dist_err, m_admissible_position_error); + return false; + } + + /* check yaw error limit */ + if (std::fabs(data->yaw_err) > m_admissible_yaw_error_rad) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + m_logger, *m_clock, duration, "yaw error is over limit. error = %f deg, limit %f deg", + RAD2DEG * data->yaw_err, RAD2DEG * m_admissible_yaw_error_rad); + return false; + } + + /* check trajectory time length */ + auto end_time = data->nearest_time + m_param.input_delay + getPredictionTime(); + if (end_time > traj.relative_time.back()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + m_logger, *m_clock, 1000 /*ms*/, "path is too short for prediction."); + return false; + } + return true; +} + +float64_t MPC::calcSteerPrediction() +{ + auto t_start = m_time_prev; + auto t_end = m_clock->now(); + m_time_prev = t_end; + + const float64_t duration = (t_end - t_start).seconds(); + const float64_t time_constant = m_param.steer_tau; + + const float64_t initial_response = std::exp(-duration / time_constant) * + (*m_steer_prediction_prev); + + if (m_ctrl_cmd_vec.size() <= 2) {return initial_response;} + + return initial_response + getSteerCmdSum(t_start, t_end, time_constant); +} + +float64_t MPC::getSteerCmdSum( + const rclcpp::Time & t_start, const rclcpp::Time & t_end, const float64_t time_constant) const +{ + if (m_ctrl_cmd_vec.size() <= 2) {return 0.0;} + + // Find first index of control command container + size_t idx = 1; + while (t_start > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) { + if ((idx + 1) >= m_ctrl_cmd_vec.size()) {return 0.0;} + ++idx; + } + + // Compute steer command input response + float64_t steer_sum = 0.0; + auto t = t_start; + while (t_end > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) { + const float64_t duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds(); + t = rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp); + steer_sum += + (1 - std::exp(-duration / time_constant)) * + static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); + ++idx; + if (idx >= m_ctrl_cmd_vec.size()) {break;} + } + + const float64_t duration = (t_end - t).seconds(); + steer_sum += + (1 - std::exp(-duration / time_constant)) * + static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); + + return steer_sum; +} + +void MPC::storeSteerCmd(const float64_t steer) +{ + const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_param.input_delay); + autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + cmd.stamp = time_delayed; + cmd.steering_tire_angle = static_cast(steer); + + // store published ctrl cmd + m_ctrl_cmd_vec.emplace_back(cmd); + + if (m_ctrl_cmd_vec.size() <= 2) { + return; + } + + // remove unused ctrl cmd + constexpr float64_t store_time = 0.3; + if ( + (time_delayed - m_ctrl_cmd_vec.at(1).stamp).seconds() > + m_param.input_delay + store_time) + { + m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); + } +} + +bool8_t MPC::resampleMPCTrajectoryByTime( + float64_t ts, const trajectory_follower::MPCTrajectory & input, + trajectory_follower::MPCTrajectory * output) const +{ + std::vector mpc_time_v; + for (float64_t i = 0; i < static_cast(m_param.prediction_horizon); ++i) { + mpc_time_v.push_back(ts + i * m_param.prediction_dt); + } + if (!trajectory_follower::MPCUtils::linearInterpMPCTrajectory( + input.relative_time, input, + mpc_time_v, output)) + { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + m_logger, *m_clock, + 1000 /*ms*/, + "calculateMPC: mpc resample error. stop mpc calculation. check code!"); + return false; + } + return true; +} + +Eigen::VectorXd MPC::getInitialState(const MPCData & data) +{ + const int64_t DIM_X = m_vehicle_model_ptr->getDimX(); + Eigen::VectorXd x0 = Eigen::VectorXd::Zero(DIM_X); + + const auto & lat_err = data.lateral_err; + const auto & steer = m_use_steer_prediction ? data.predicted_steer : data.steer; + const auto & yaw_err = data.yaw_err; + + if (m_vehicle_model_type == "kinematics") { + x0 << lat_err, yaw_err, steer; + } else if (m_vehicle_model_type == "kinematics_no_delay") { + x0 << lat_err, yaw_err; + } else if (m_vehicle_model_type == "dynamics") { + float64_t dlat = (lat_err - m_lateral_error_prev) / m_ctrl_period; + float64_t dyaw = (yaw_err - m_yaw_error_prev) / m_ctrl_period; + m_lateral_error_prev = lat_err; + m_yaw_error_prev = yaw_err; + dlat = m_lpf_lateral_error.filter(dlat); + dyaw = m_lpf_yaw_error.filter(dyaw); + x0 << lat_err, dlat, yaw_err, dyaw; + RCLCPP_DEBUG(m_logger, "(before lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw); + RCLCPP_DEBUG(m_logger, "(after lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw); + } else { + RCLCPP_ERROR(m_logger, "vehicle_model_type is undefined"); + } + return x0; +} + +bool8_t MPC::updateStateForDelayCompensation( + const trajectory_follower::MPCTrajectory & traj, const float64_t & start_time, + Eigen::VectorXd * x) +{ + const int64_t DIM_X = m_vehicle_model_ptr->getDimX(); + const int64_t DIM_U = m_vehicle_model_ptr->getDimU(); + const int64_t DIM_Y = m_vehicle_model_ptr->getDimY(); + + Eigen::MatrixXd Ad(DIM_X, DIM_X); + Eigen::MatrixXd Bd(DIM_X, DIM_U); + Eigen::MatrixXd Wd(DIM_X, 1); + Eigen::MatrixXd Cd(DIM_Y, DIM_X); + + Eigen::MatrixXd x_curr = *x; + float64_t mpc_curr_time = start_time; + for (uint64_t i = 0; i < m_input_buffer.size(); ++i) { + float64_t k = 0.0; + float64_t v = 0.0; + if ( + !trajectory_follower::linearInterpolate( + traj.relative_time, traj.k, + mpc_curr_time, k) || + !trajectory_follower::linearInterpolate( + traj.relative_time, traj.vx, + mpc_curr_time, v)) + { + RCLCPP_ERROR( + m_logger, + "mpc resample error at delay compensation, stop mpc calculation. check code!"); + return false; + } + + /* get discrete state matrix A, B, C, W */ + m_vehicle_model_ptr->setVelocity(v); + m_vehicle_model_ptr->setCurvature(k); + m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, m_ctrl_period); + Eigen::MatrixXd ud = Eigen::MatrixXd::Zero(DIM_U, 1); + ud(0, 0) = m_input_buffer.at(i); // for steering input delay + x_curr = Ad * x_curr + Bd * ud + Wd; + mpc_curr_time += m_ctrl_period; + } + *x = x_curr; + return true; +} + +trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( + const trajectory_follower::MPCTrajectory & input, + const geometry_msgs::msg::Pose & current_pose, + const float64_t v0) const +{ + int64_t nearest_idx = + trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose); + if (nearest_idx < 0) {return input;} + + const float64_t acc_lim = m_param.acceleration_limit; + const float64_t tau = m_param.velocity_time_constant; + + trajectory_follower::MPCTrajectory output = input; + trajectory_follower::MPCUtils::dynamicSmoothingVelocity( + static_cast(nearest_idx), v0, + acc_lim, tau, output); + const float64_t t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time + const float64_t t_end = output.relative_time.back() + getPredictionTime() + t_ext; + const float64_t v_end = 0.0; + output.vx.back() = v_end; // set for end point + output.push_back( + output.x.back(), output.y.back(), output.z.back(), output.yaw.back(), v_end, output.k.back(), + output.smooth_k.back(), t_end); + return output; +} + +/* + * predict equation: Xec = Aex * x0 + Bex * Uex + Wex + * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex + * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) + */ +MPCMatrix MPC::generateMPCMatrix( + const trajectory_follower::MPCTrajectory & reference_trajectory) +{ + using Eigen::MatrixXd; + + const int64_t N = m_param.prediction_horizon; + const float64_t DT = m_param.prediction_dt; + const int64_t DIM_X = m_vehicle_model_ptr->getDimX(); + const int64_t DIM_U = m_vehicle_model_ptr->getDimU(); + const int64_t DIM_Y = m_vehicle_model_ptr->getDimY(); + + MPCMatrix m; + m.Aex = MatrixXd::Zero(DIM_X * N, DIM_X); + m.Bex = MatrixXd::Zero(DIM_X * N, DIM_U * N); + m.Wex = MatrixXd::Zero(DIM_X * N, 1); + m.Cex = MatrixXd::Zero(DIM_Y * N, DIM_X * N); + m.Qex = MatrixXd::Zero(DIM_Y * N, DIM_Y * N); + m.R1ex = MatrixXd::Zero(DIM_U * N, DIM_U * N); + m.R2ex = MatrixXd::Zero(DIM_U * N, DIM_U * N); + m.Uref_ex = MatrixXd::Zero(DIM_U * N, 1); + + /* weight matrix depends on the vehicle model */ + MatrixXd Q = MatrixXd::Zero(DIM_Y, DIM_Y); + MatrixXd R = MatrixXd::Zero(DIM_U, DIM_U); + MatrixXd Q_adaptive = MatrixXd::Zero(DIM_Y, DIM_Y); + MatrixXd R_adaptive = MatrixXd::Zero(DIM_U, DIM_U); + + MatrixXd Ad(DIM_X, DIM_X); + MatrixXd Bd(DIM_X, DIM_U); + MatrixXd Wd(DIM_X, 1); + MatrixXd Cd(DIM_Y, DIM_X); + MatrixXd Uref(DIM_U, 1); + + constexpr float64_t ep = 1.0e-3; // large enough to ignore velocity noise + + /* predict dynamics for N times */ + for (int64_t i = 0; i < N; ++i) { + const float64_t ref_vx = reference_trajectory.vx[static_cast(i)]; + const float64_t ref_vx_squared = ref_vx * ref_vx; + + // curvature will be 0 when vehicle stops + const float64_t ref_k = reference_trajectory.k[static_cast(i)] * m_sign_vx; + const float64_t ref_smooth_k = reference_trajectory.smooth_k[static_cast(i)] * + m_sign_vx; + + /* get discrete state matrix A, B, C, W */ + m_vehicle_model_ptr->setVelocity(ref_vx); + m_vehicle_model_ptr->setCurvature(ref_k); + m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT); + + Q = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R = Eigen::MatrixXd::Zero(DIM_U, DIM_U); + Q(0, 0) = getWeightLatError(ref_k); + Q(1, 1) = getWeightHeadingError(ref_k); + R(0, 0) = getWeightSteerInput(ref_k); + + Q_adaptive = Q; + R_adaptive = R; + if (i == N - 1) { + Q_adaptive(0, 0) = m_param.weight_terminal_lat_error; + Q_adaptive(1, 1) = m_param.weight_terminal_heading_error; + } + Q_adaptive(1, 1) += ref_vx_squared * getWeightHeadingErrorSqVel(ref_k); + R_adaptive(0, 0) += ref_vx_squared * getWeightSteerInputSqVel(ref_k); + + /* update mpc matrix */ + int64_t idx_x_i = i * DIM_X; + int64_t idx_x_i_prev = (i - 1) * DIM_X; + int64_t idx_u_i = i * DIM_U; + int64_t idx_y_i = i * DIM_Y; + if (i == 0) { + m.Aex.block(0, 0, DIM_X, DIM_X) = Ad; + m.Bex.block(0, 0, DIM_X, DIM_U) = Bd; + m.Wex.block(0, 0, DIM_X, 1) = Wd; + } else { + m.Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * m.Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X); + for (int64_t j = 0; j < i; ++j) { + int64_t idx_u_j = j * DIM_U; + m.Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) = + Ad * m.Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U); + } + m.Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * m.Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd; + } + m.Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd; + m.Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd; + m.Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive; + m.R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive; + + /* get reference input (feed-forward) */ + m_vehicle_model_ptr->setCurvature(ref_smooth_k); + m_vehicle_model_ptr->calculateReferenceInput(Uref); + if (std::fabs(Uref(0, 0)) < DEG2RAD * m_param.zero_ff_steer_deg) { + Uref(0, 0) = 0.0; // ignore curvature noise + } + m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; + } + + /* add lateral jerk : weight for (v * {u(i) - u(i-1)} )^2 */ + for (int64_t i = 0; i < N - 1; ++i) { + const float64_t ref_vx = reference_trajectory.vx[static_cast(i)]; + m_sign_vx = ref_vx > ep ? 1 : (ref_vx < -ep ? -1 : m_sign_vx); + const float64_t ref_k = reference_trajectory.k[static_cast(i)] * m_sign_vx; + const float64_t j = ref_vx * ref_vx * getWeightLatJerk(ref_k) / (DT * DT); + const Eigen::Matrix2d J = (Eigen::Matrix2d() << j, -j, -j, j).finished(); + m.R2ex.block(i, i, 2, 2) += J; + } + + addSteerWeightR(&m.R1ex); + + return m; +} + +/* + * solve quadratic optimization. + * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex + * , Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) + * constraint matrix : lb < U < ub, lbA < A*U < ubA + * current considered constraint + * - steering limit + * - steering rate limit + * + * (1)lb < u < ub && (2)lbA < Au < ubA --> (3)[lb, lbA] < [I, A]u < [ub, ubA] + * (1)lb < u < ub ... + * [-u_lim] < [ u0 ] < [u_lim] + * [-u_lim] < [ u1 ] < [u_lim] + * ~~~ + * [-u_lim] < [ uN ] < [u_lim] (*N... DIM_U) + * (2)lbA < Au < ubA ... + * [prev_u0 - au_lim*ctp] < [ u0 ] < [prev_u0 + au_lim*ctp] (*ctp ... ctrl_period) + * [ -au_lim * dt ] < [u1 - u0] < [ au_lim * dt ] + * [ -au_lim * dt ] < [u2 - u1] < [ au_lim * dt ] + * ~~~ + * [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U) + */ +bool8_t MPC::executeOptimization( + const MPCMatrix & m, const Eigen::VectorXd & x0, Eigen::VectorXd * Uex) +{ + using Eigen::MatrixXd; + using Eigen::VectorXd; + + if (!isValid(m)) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + m_logger, *m_clock, 1000 /*ms*/, "model matrix is invalid. stop MPC."); + return false; + } + + const int64_t DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU(); + + // cost function: 1/2 * Uex' * H * Uex + f' * Uex, H = B' * C' * Q * C * B + R + const MatrixXd CB = m.Cex * m.Bex; + const MatrixXd QCB = m.Qex * CB; + // MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; // This calculation is heavy. looking for a good way. //NOLINT + MatrixXd H = MatrixXd::Zero(DIM_U_N, DIM_U_N); + H.triangularView() = CB.transpose() * QCB; + H.triangularView() += m.R1ex + m.R2ex; + H.triangularView() = H.transpose(); + MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex; + addSteerWeightF(&f); + + MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N); + for (int64_t i = 1; i < DIM_U_N; i++) { + A(i, i - 1) = -1.0; + } + + VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim); // min steering angle + VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle + VectorXd lbA = VectorXd::Constant(DIM_U_N, -m_steer_rate_lim * m_param.prediction_dt); + VectorXd ubA = VectorXd::Constant(DIM_U_N, m_steer_rate_lim * m_param.prediction_dt); + lbA(0, 0) = m_raw_steer_cmd_prev - m_steer_rate_lim * m_ctrl_period; + ubA(0, 0) = m_raw_steer_cmd_prev + m_steer_rate_lim * m_ctrl_period; + + auto t_start = std::chrono::system_clock::now(); + bool8_t solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, *Uex); + auto t_end = std::chrono::system_clock::now(); + if (!solve_result) { + RCLCPP_WARN_SKIPFIRST_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "qp solver error"); + return false; + } + + { + auto t = std::chrono::duration_cast(t_end - t_start).count(); + RCLCPP_DEBUG( + m_logger, "qp solver calculation time = %ld [ms]", t); + } + + if (Uex->array().isNaN().any()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + m_logger, *m_clock, 1000 /*ms*/, "model Uex includes NaN, stop MPC."); + return false; + } + return true; +} + +void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr) const +{ + const int64_t N = m_param.prediction_horizon; + const float64_t DT = m_param.prediction_dt; + + auto & R = *R_ptr; + + /* add steering rate : weight for (u(i) - u(i-1) / dt )^2 */ + { + const float64_t steer_rate_r = m_param.weight_steer_rate / (DT * DT); + const Eigen::Matrix2d D = steer_rate_r * (Eigen::Matrix2d() << 1.0, -1.0, -1.0, 1.0).finished(); + for (int64_t i = 0; i < N - 1; ++i) { + R.block(i, i, 2, 2) += D; + } + if (N > 1) { + // steer rate i = 0 + R(0, 0) += m_param.weight_steer_rate / (m_ctrl_period * m_ctrl_period); + } + } + + /* add steering acceleration : weight for { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2 */ + { + const float64_t w = m_param.weight_steer_acc; + const float64_t steer_acc_r = w / std::pow(DT, 4); + const float64_t steer_acc_r_cp1 = w / (std::pow(DT, 3) * m_ctrl_period); + const float64_t steer_acc_r_cp2 = w / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2)); + const float64_t steer_acc_r_cp4 = w / std::pow(m_ctrl_period, 4); + const Eigen::Matrix3d D = + steer_acc_r * + (Eigen::Matrix3d() << 1.0, -2.0, 1.0, -2.0, 4.0, -2.0, 1.0, -2.0, 1.0).finished(); + for (int64_t i = 1; i < N - 1; ++i) { + R.block(i - 1, i - 1, 3, 3) += D; + } + if (N > 1) { + // steer acc i = 1 + R(0, 0) += steer_acc_r * 1.0 + steer_acc_r_cp2 * 1.0 + steer_acc_r_cp1 * 2.0; + R(1, 0) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0; + R(0, 1) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0; + R(1, 1) += steer_acc_r * 1.0; + // steer acc i = 0 + R(0, 0) += steer_acc_r_cp4 * 1.0; + } + } +} + +void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr) const +{ + if (f_ptr->rows() < 2) { + return; + } + + const float64_t DT = m_param.prediction_dt; + auto & f = *f_ptr; + + // steer rate for i = 0 + f(0, 0) += -2.0 * m_param.weight_steer_rate / (std::pow(DT, 2)) * 0.5; + + // const float64_t steer_acc_r = m_param.weight_steer_acc / std::pow(DT, 4); + const float64_t steer_acc_r_cp1 = m_param.weight_steer_acc / (std::pow(DT, 3) * m_ctrl_period); + const float64_t steer_acc_r_cp2 = + m_param.weight_steer_acc / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2)); + const float64_t steer_acc_r_cp4 = m_param.weight_steer_acc / std::pow(m_ctrl_period, 4); + + // steer acc i = 0 + f(0, 0) += ((-2.0 * m_raw_steer_cmd_prev + m_raw_steer_cmd_pprev) * steer_acc_r_cp4) * 0.5; + + // steer acc for i = 1 + f(0, 0) += (-2.0 * m_raw_steer_cmd_prev * (steer_acc_r_cp1 + steer_acc_r_cp2)) * 0.5; + f(0, 1) += (2.0 * m_raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5; +} + +float64_t MPC::getPredictionTime() const +{ + return static_cast(m_param.prediction_horizon - 1) * m_param.prediction_dt + + m_param.input_delay + + m_ctrl_period; +} + +bool8_t MPC::isValid(const MPCMatrix & m) const +{ + if ( + m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() || + m.Wex.array().isNaN().any() || m.Qex.array().isNaN().any() || m.R1ex.array().isNaN().any() || + m.R2ex.array().isNaN().any() || m.Uref_ex.array().isNaN().any()) + { + return false; + } + + if ( + m.Aex.array().isInf().any() || m.Bex.array().isInf().any() || m.Cex.array().isInf().any() || + m.Wex.array().isInf().any() || m.Qex.array().isInf().any() || m.R1ex.array().isInf().any() || + m.R2ex.array().isInf().any() || m.Uref_ex.array().isInf().any()) + { + return false; + } + + return true; +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/mpc_trajectory.cpp b/control/trajectory_follower/src/mpc_trajectory.cpp new file mode 100644 index 0000000000000..cc1c51130c489 --- /dev/null +++ b/control/trajectory_follower/src/mpc_trajectory.cpp @@ -0,0 +1,68 @@ +// Copyright 2018-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 "trajectory_follower/mpc_trajectory.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +void MPCTrajectory::push_back( + const float64_t & xp, const float64_t & yp, const float64_t & zp, const float64_t & yawp, + const float64_t & vxp, + const float64_t & kp, const float64_t & smooth_kp, const float64_t & tp) +{ + x.push_back(xp); + y.push_back(yp); + z.push_back(zp); + yaw.push_back(yawp); + vx.push_back(vxp); + k.push_back(kp); + smooth_k.push_back(smooth_kp); + relative_time.push_back(tp); +} + +void MPCTrajectory::clear() +{ + x.clear(); + y.clear(); + z.clear(); + yaw.clear(); + vx.clear(); + k.clear(); + smooth_k.clear(); + relative_time.clear(); +} + +size_t MPCTrajectory::size() const +{ + if ( + x.size() == y.size() && x.size() == z.size() && x.size() == yaw.size() && + x.size() == vx.size() && x.size() == k.size() && x.size() == smooth_k.size() && + x.size() == relative_time.size()) + { + return x.size(); + } else { + std::cerr << "[MPC trajectory] trajectory size is inappropriate" << std::endl; + return 0; + } +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp new file mode 100644 index 0000000000000..3d2cdacd6ddd5 --- /dev/null +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -0,0 +1,493 @@ +// Copyright 2018-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 "trajectory_follower/mpc_utils.hpp" + +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +namespace MPCUtils +{ +using autoware::common::geometry::distance_2d; + +geometry_msgs::msg::Quaternion getQuaternionFromYaw(const float64_t & yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); +} + +void convertEulerAngleToMonotonic(std::vector * a) +{ + if (!a) { + return; + } + for (uint64_t i = 1; i < a->size(); ++i) { + const float64_t da = a->at(i) - a->at(i - 1); + a->at(i) = a->at(i - 1) + autoware::common::helper_functions::wrap_angle(da); + } +} + +float64_t calcLateralError( + const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & ref_pose) +{ + const float64_t err_x = ego_pose.position.x - ref_pose.position.x; + const float64_t err_y = ego_pose.position.y - ref_pose.position.y; + const float64_t ref_yaw = ::motion::motion_common::to_angle(ref_pose.orientation); + const float64_t lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y; + return lat_err; +} + +void calcMPCTrajectoryArclength( + const MPCTrajectory & trajectory, std::vector * arclength) +{ + float64_t dist = 0.0; + arclength->clear(); + arclength->push_back(dist); + for (uint64_t i = 1; i < trajectory.size(); ++i) { + const float64_t dx = trajectory.x.at(i) - trajectory.x.at(i - 1); + const float64_t dy = trajectory.y.at(i) - trajectory.y.at(i - 1); + dist += std::sqrt(dx * dx + dy * dy); + arclength->push_back(dist); + } +} + +bool8_t resampleMPCTrajectoryByDistance( + const MPCTrajectory & input, const float64_t resample_interval_dist, MPCTrajectory * output) +{ + if (!output) { + return false; + } + if (input.empty()) { + *output = input; + return true; + } + std::vector input_arclength; + calcMPCTrajectoryArclength(input, &input_arclength); + + if (input_arclength.empty()) { + return false; + } + + std::vector output_arclength; + for (float64_t s = 0; s < input_arclength.back(); s += resample_interval_dist) { + output_arclength.push_back(s); + } + + std::vector input_yaw = input.yaw; + convertEulerAngleToMonotonic(&input_yaw); + + output->x = interpolation::slerp(input_arclength, input.x, output_arclength); + output->y = interpolation::slerp(input_arclength, input.y, output_arclength); + output->z = interpolation::slerp(input_arclength, input.z, output_arclength); + output->yaw = interpolation::slerp(input_arclength, input.yaw, output_arclength); + output->vx = interpolation::lerp(input_arclength, input.vx, output_arclength); + output->k = interpolation::slerp(input_arclength, input.k, output_arclength); + output->smooth_k = interpolation::slerp(input_arclength, input.smooth_k, output_arclength); + output->relative_time = interpolation::lerp( + input_arclength, input.relative_time, + output_arclength); + + return true; +} + +bool8_t linearInterpMPCTrajectory( + const std::vector & in_index, const MPCTrajectory & in_traj, + const std::vector & out_index, MPCTrajectory * out_traj) +{ + if (!out_traj) { + return false; + } + + if (in_traj.empty()) { + *out_traj = in_traj; + return true; + } + + std::vector in_traj_yaw = in_traj.yaw; + convertEulerAngleToMonotonic(&in_traj_yaw); + + if ( + !linearInterpolate(in_index, in_traj.x, out_index, out_traj->x) || + !linearInterpolate(in_index, in_traj.y, out_index, out_traj->y) || + !linearInterpolate(in_index, in_traj.z, out_index, out_traj->z) || + !linearInterpolate(in_index, in_traj_yaw, out_index, out_traj->yaw) || + !linearInterpolate(in_index, in_traj.vx, out_index, out_traj->vx) || + !linearInterpolate(in_index, in_traj.k, out_index, out_traj->k) || + !linearInterpolate(in_index, in_traj.smooth_k, out_index, out_traj->smooth_k) || + !linearInterpolate( + in_index, in_traj.relative_time, out_index, out_traj->relative_time)) + { + std::cerr << "linearInterpMPCTrajectory error!" << std::endl; + return false; + } + + if (out_traj->empty()) { + std::cerr << "[mpc util] linear interpolation error" << std::endl; + return false; + } + + return true; +} + +void calcTrajectoryYawFromXY( + MPCTrajectory * traj, const int64_t nearest_idx, + const float64_t ego_yaw) +{ + if (traj->yaw.size() < 3) { // at least 3 points are required to calculate yaw + return; + } + if (traj->yaw.size() != traj->vx.size()) { + RCLCPP_ERROR(rclcpp::get_logger("mpc_utils"), "trajectory size has no consistency."); + return; + } + + // calculate shift direction (forward or backward) + const int64_t upper_nearest_idx = + (static_cast(traj->x.size()) - 1 == nearest_idx) ? nearest_idx : nearest_idx + 1; + const float64_t dx = traj->x[static_cast(upper_nearest_idx)] - + traj->x[static_cast(upper_nearest_idx - 1)]; + const float64_t dy = traj->y[static_cast(upper_nearest_idx)] - + traj->y[static_cast(upper_nearest_idx - 1)]; + const bool forward_shift = + std::abs(autoware::common::helper_functions::wrap_angle(std::atan2(dy, dx) - ego_yaw)) < + M_PI / 2.0; + + // interpolate yaw + for (int64_t i = 1; i < static_cast(traj->yaw.size()) - 1; ++i) { + const float64_t dx = traj->x[static_cast(i + 1)] - traj->x[static_cast(i - 1)]; + const float64_t dy = traj->y[static_cast(i + 1)] - traj->y[static_cast(i - 1)]; + traj->yaw[static_cast(i)] = + forward_shift ? std::atan2(dy, dx) : std::atan2(dy, dx) + M_PI; + } + if (traj->yaw.size() > 1) { + traj->yaw[0] = traj->yaw[1]; + traj->yaw.back() = traj->yaw[traj->yaw.size() - 2]; + } +} + +bool8_t calcTrajectoryCurvature( + const size_t curvature_smoothing_num_traj, + const size_t curvature_smoothing_num_ref_steer, + MPCTrajectory * traj) +{ + if (!traj) { + return false; + } + + traj->k = calcTrajectoryCurvature(curvature_smoothing_num_traj, *traj); + traj->smooth_k = calcTrajectoryCurvature(curvature_smoothing_num_ref_steer, *traj); + return true; +} + +std::vector calcTrajectoryCurvature( + const size_t curvature_smoothing_num, const MPCTrajectory & traj) +{ + std::vector curvature_vec(traj.x.size()); + + /* calculate curvature by circle fitting from three points */ + geometry_msgs::msg::Point p1, p2, p3; + const size_t max_smoothing_num = static_cast( + std::floor(0.5 * (static_cast(traj.x.size() - 1)))); + const size_t L = std::min(curvature_smoothing_num, max_smoothing_num); + for (size_t i = L; i < traj.x.size() - L; ++i) { + const size_t curr_idx = i; + const size_t prev_idx = curr_idx - L; + const size_t next_idx = curr_idx + L; + p1.x = traj.x[prev_idx]; + p2.x = traj.x[curr_idx]; + p3.x = traj.x[next_idx]; + p1.y = traj.y[prev_idx]; + p2.y = traj.y[curr_idx]; + p3.y = traj.y[next_idx]; + const float64_t den = std::max( + distance_2d( + p1, + p2) * distance_2d(p2, p3) * distance_2d(p3, p1), + std::numeric_limits::epsilon()); + const float64_t curvature = + 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; + curvature_vec.at(curr_idx) = curvature; + } + + /* first and last curvature is copied from next value */ + for (size_t i = 0; i < std::min(L, traj.x.size()); ++i) { + curvature_vec.at(i) = curvature_vec.at(std::min(L, traj.x.size() - 1)); + curvature_vec.at( + traj.x.size() - i - + 1) = curvature_vec.at(std::max(traj.x.size() - L - 1, size_t(0))); + } + return curvature_vec; +} + +bool8_t convertToMPCTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output) +{ + output.clear(); + for (const autoware_auto_planning_msgs::msg::TrajectoryPoint & p : input.points) { + const float64_t x = p.pose.position.x; + const float64_t y = p.pose.position.y; + const float64_t z = 0.0; + const float64_t yaw = ::motion::motion_common::to_angle(p.pose.orientation); + const float64_t vx = p.longitudinal_velocity_mps; + const float64_t k = 0.0; + const float64_t t = 0.0; + output.push_back(x, y, z, yaw, vx, k, k, t); + } + calcMPCTrajectoryTime(output); + return true; +} + +bool8_t convertToAutowareTrajectory( + const MPCTrajectory & input, autoware_auto_planning_msgs::msg::Trajectory & output) +{ + output.points.clear(); + autoware_auto_planning_msgs::msg::TrajectoryPoint p; + using Real = decltype(p.pose.position.x); + for (size_t i = 0; i < input.size(); ++i) { + p.pose.position.x = static_cast(input.x.at(i)); + p.pose.position.y = static_cast(input.y.at(i)); + p.pose.position.z = static_cast(input.z.at(i)); + p.pose.orientation = ::motion::motion_common::from_angle(input.yaw.at(i)); + p.longitudinal_velocity_mps = + static_cast(input.vx.at(i)); + output.points.push_back(p); + } + return true; +} + +bool8_t calcMPCTrajectoryTime(MPCTrajectory & traj) +{ + float64_t t = 0.0; + traj.relative_time.clear(); + traj.relative_time.push_back(t); + for (size_t i = 0; i < traj.x.size() - 1; ++i) { + const float64_t dx = traj.x.at(i + 1) - traj.x.at(i); + const float64_t dy = traj.y.at(i + 1) - traj.y.at(i); + const float64_t dz = traj.z.at(i + 1) - traj.z.at(i); + const float64_t dist = std::sqrt(dx * dx + dy * dy + dz * dz); + const float64_t v = std::max(std::fabs(traj.vx.at(i)), 0.1); + t += (dist / v); + traj.relative_time.push_back(t); + } + return true; +} + +void dynamicSmoothingVelocity( + const size_t start_idx, const float64_t start_vel, const float64_t acc_lim, const float64_t tau, + MPCTrajectory & traj) +{ + float64_t curr_v = start_vel; + traj.vx.at(start_idx) = start_vel; + + for (size_t i = start_idx + 1; i < traj.size(); ++i) { + const float64_t ds = + std::hypot(traj.x.at(i) - traj.x.at(i - 1), traj.y.at(i) - traj.y.at(i - 1)); + const float64_t dt = ds / + std::max(std::fabs(curr_v), std::numeric_limits::epsilon()); + const float64_t a = tau / std::max(tau + dt, std::numeric_limits::epsilon()); + const float64_t updated_v = a * curr_v + (1.0 - a) * traj.vx.at(i); + const float64_t dv = std::max(-acc_lim * dt, std::min(acc_lim * dt, updated_v - curr_v)); + curr_v = curr_v + dv; + traj.vx.at(i) = curr_v; + } + calcMPCTrajectoryTime(traj); +} + +int64_t calcNearestIndex( + const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose) +{ + if (traj.empty()) { + return -1; + } + const float64_t my_yaw = ::motion::motion_common::to_angle(self_pose.orientation); + int64_t nearest_idx = -1; + float64_t min_dist_squared = std::numeric_limits::max(); + for (size_t i = 0; i < traj.size(); ++i) { + const float64_t dx = self_pose.position.x - traj.x[i]; + const float64_t dy = self_pose.position.y - traj.y[i]; + const float64_t dist_squared = dx * dx + dy * dy; + + /* ignore when yaw error is large, for crossing path */ + const float64_t err_yaw = autoware::common::helper_functions::wrap_angle(my_yaw - traj.yaw[i]); + if (std::fabs(err_yaw) > (M_PI / 3.0)) { + continue; + } + if (dist_squared < min_dist_squared) { + min_dist_squared = dist_squared; + nearest_idx = static_cast(i); + } + } + return nearest_idx; +} + +int64_t calcNearestIndex( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const geometry_msgs::msg::Pose & self_pose) +{ + if (traj.points.empty()) { + return -1; + } + const float64_t my_yaw = ::motion::motion_common::to_angle(self_pose.orientation); + int64_t nearest_idx = -1; + float64_t min_dist_squared = std::numeric_limits::max(); + for (size_t i = 0; i < traj.points.size(); ++i) { + const float64_t dx = + self_pose.position.x - static_cast(traj.points.at(i).pose.position.x); + const float64_t dy = + self_pose.position.y - static_cast(traj.points.at(i).pose.position.y); + const float64_t dist_squared = dx * dx + dy * dy; + + /* ignore when yaw error is large, for crossing path */ + const float64_t traj_yaw = + ::motion::motion_common::to_angle(traj.points.at(i).pose.orientation); + const float64_t err_yaw = autoware::common::helper_functions::wrap_angle(my_yaw - traj_yaw); + if (std::fabs(err_yaw) > (M_PI / 3.0)) { + continue; + } + if (dist_squared < min_dist_squared) { + min_dist_squared = dist_squared; + nearest_idx = static_cast(i); + } + } + return nearest_idx; +} + +bool8_t calcNearestPoseInterp( + const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, + geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time, + const rclcpp::Logger & logger, rclcpp::Clock & clock) +{ + if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) { + return false; + } + const int64_t nearest_idx = calcNearestIndex(traj, self_pose); + if (nearest_idx == -1) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, clock, 5000, "[calcNearestPoseInterp] fail to get nearest. traj.size = %zu", + traj.size()); + return false; + } + + const int64_t traj_size = static_cast(traj.size()); + + *nearest_index = static_cast(nearest_idx); + + if (traj.size() == 1) { + nearest_pose->position.x = traj.x[*nearest_index]; + nearest_pose->position.y = traj.y[*nearest_index]; + nearest_pose->orientation = getQuaternionFromYaw(traj.yaw[*nearest_index]); + *nearest_time = traj.relative_time[*nearest_index]; + return true; + } + + auto calcSquaredDist = + [](const geometry_msgs::msg::Pose & p, const MPCTrajectory & t, const size_t idx) { + const float64_t dx = p.position.x - t.x[idx]; + const float64_t dy = p.position.y - t.y[idx]; + return dx * dx + dy * dy; + }; + + /* get second nearest index = next to nearest_index */ + const size_t next = static_cast(std::min(nearest_idx + 1, traj_size - 1)); + const size_t prev = static_cast(std::max(nearest_idx - 1, int64_t(0))); + const float64_t dist_to_next = calcSquaredDist(self_pose, traj, next); + const float64_t dist_to_prev = calcSquaredDist(self_pose, traj, prev); + const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev; + + const float64_t a_sq = calcSquaredDist(self_pose, traj, *nearest_index); + const float64_t b_sq = calcSquaredDist(self_pose, traj, second_nearest_index); + const float64_t dx3 = traj.x[*nearest_index] - traj.x[second_nearest_index]; + const float64_t dy3 = traj.y[*nearest_index] - traj.y[second_nearest_index]; + const float64_t c_sq = dx3 * dx3 + dy3 * dy3; + + /* if distance between two points are too close */ + if (c_sq < 1.0E-5) { + nearest_pose->position.x = traj.x[*nearest_index]; + nearest_pose->position.y = traj.y[*nearest_index]; + nearest_pose->orientation = getQuaternionFromYaw(traj.yaw[*nearest_index]); + *nearest_time = traj.relative_time[*nearest_index]; + return true; + } + + /* linear interpolation */ + const float64_t alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0); + nearest_pose->position.x = + alpha * traj.x[*nearest_index] + (1 - alpha) * traj.x[second_nearest_index]; + nearest_pose->position.y = + alpha * traj.y[*nearest_index] + (1 - alpha) * traj.y[second_nearest_index]; + const float64_t tmp_yaw_err = + autoware::common::helper_functions::wrap_angle( + traj.yaw[*nearest_index] - + traj.yaw[second_nearest_index]); + const float64_t nearest_yaw = + autoware::common::helper_functions::wrap_angle( + traj.yaw[second_nearest_index] + alpha * tmp_yaw_err); + nearest_pose->orientation = getQuaternionFromYaw(nearest_yaw); + *nearest_time = alpha * traj.relative_time[*nearest_index] + + (1 - alpha) * traj.relative_time[second_nearest_index]; + return true; +} + +float64_t calcStopDistance( + const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, + const int64_t origin) +{ + constexpr float zero_velocity = std::numeric_limits::epsilon(); + const float origin_velocity = + current_trajectory.points.at(static_cast(origin)).longitudinal_velocity_mps; + float64_t stop_dist = 0.0; + + // search forward + if (std::fabs(origin_velocity) > zero_velocity) { + for (int64_t i = origin + 1; i < static_cast(current_trajectory.points.size()) - 1; + ++i) + { + const auto & p0 = current_trajectory.points.at(static_cast(i)); + const auto & p1 = current_trajectory.points.at(static_cast(i - 1)); + stop_dist += distance_2d(p0, p1); + if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) { + break; + } + } + return stop_dist; + } + + // search backward + for (int64_t i = origin - 1; 0 < i; --i) { + const auto & p0 = current_trajectory.points.at(static_cast(i)); + const auto & p1 = current_trajectory.points.at(static_cast(i + 1)); + if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) { + break; + } + stop_dist -= distance_2d(p0, p1); + } + return stop_dist; +} + +} // namespace MPCUtils +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/pid.cpp b/control/trajectory_follower/src/pid.cpp new file mode 100644 index 0000000000000..1d61114b05972 --- /dev/null +++ b/control/trajectory_follower/src/pid.cpp @@ -0,0 +1,110 @@ +// Copyright 2018-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 "trajectory_follower/pid.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +PIDController::PIDController() +: m_error_integral(0.0), m_prev_error(0.0), m_is_first_time(true), m_is_gains_set(false), + m_is_limits_set(false) {} + +float64_t PIDController::calculate( + const float64_t error, const float64_t dt, const bool8_t enable_integration, + std::vector & pid_contributions) +{ + if (!m_is_gains_set || !m_is_limits_set) { + throw std::runtime_error("Trying to calculate uninitialized PID"); + } + + const auto & p = m_params; + + float64_t ret_p = p.kp * error; + ret_p = std::min(std::max(ret_p, p.min_ret_p), p.max_ret_p); + + if (enable_integration) { + m_error_integral += error * dt; + m_error_integral = std::min(std::max(m_error_integral, p.min_ret_i / p.ki), p.max_ret_i / p.ki); + } + const float64_t ret_i = p.ki * m_error_integral; + + float64_t error_differential; + if (m_is_first_time) { + error_differential = 0; + m_is_first_time = false; + } else { + error_differential = (error - m_prev_error) / dt; + } + float64_t ret_d = p.kd * error_differential; + ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); + + m_prev_error = error; + + pid_contributions.resize(3); + pid_contributions.at(0) = ret_p; + pid_contributions.at(1) = ret_i; + pid_contributions.at(2) = ret_d; + + float64_t ret = ret_p + ret_i + ret_d; + ret = std::min(std::max(ret, p.min_ret), p.max_ret); + + return ret; +} + +void PIDController::setGains(const float64_t kp, const float64_t ki, const float64_t kd) +{ + m_params.kp = kp; + m_params.ki = ki; + m_params.kd = kd; + m_is_gains_set = true; +} + +void PIDController::setLimits( + const float64_t max_ret, const float64_t min_ret, const float64_t max_ret_p, + const float64_t min_ret_p, + const float64_t max_ret_i, const float64_t min_ret_i, const float64_t max_ret_d, + const float64_t min_ret_d) +{ + m_params.max_ret = max_ret; + m_params.min_ret = min_ret; + m_params.max_ret_p = max_ret_p; + m_params.min_ret_p = min_ret_p; + m_params.max_ret_d = max_ret_d; + m_params.min_ret_d = min_ret_d; + m_params.max_ret_i = max_ret_i; + m_params.min_ret_i = min_ret_i; + m_is_limits_set = true; +} + +void PIDController::reset() +{ + m_error_integral = 0.0; + m_prev_error = 0.0; + m_is_first_time = true; +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp b/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp new file mode 100644 index 0000000000000..609e28b62c568 --- /dev/null +++ b/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp @@ -0,0 +1,89 @@ +// Copyright 2018-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 "trajectory_follower/qp_solver/qp_solver_osqp.hpp" + +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) +: logger_{logger} {} +bool8_t QPSolverOSQP::solve( + const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, + const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, + const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) +{ + const Eigen::Index raw_a = a.rows(); + const Eigen::Index col_a = a.cols(); + const Eigen::Index dim_u = ub.size(); + Eigen::MatrixXd Identity = Eigen::MatrixXd::Identity(dim_u, dim_u); + + // convert matrix to vector for osqpsolver + std::vector f(&f_vec(0), f_vec.data() + f_vec.cols() * f_vec.rows()); + + std::vector lower_bound; + std::vector upper_bound; + + for (int64_t i = 0; i < dim_u; ++i) { + lower_bound.push_back(lb(i)); + upper_bound.push_back(ub(i)); + } + + for (int64_t i = 0; i < col_a; ++i) { + lower_bound.push_back(lb_a(i)); + upper_bound.push_back(ub_a(i)); + } + + Eigen::MatrixXd osqpA = Eigen::MatrixXd(dim_u + col_a, raw_a); + osqpA << Identity, a; + + /* execute optimization */ + auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound); + + std::vector U_osqp = std::get<0>(result); + u = + Eigen::Map>( + &U_osqp[0], + static_cast(U_osqp.size()), 1); + + const int64_t status_val = std::get<3>(result); + if (status_val != 1) { + // TODO(Horibe): Should return false and the failure must be handled in an appropriate way. + RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str()); + } + + // polish status: successful (1), unperformed (0), (-1) unsuccessful + int64_t status_polish = std::get<2>(result); + if (status_polish == -1) { + RCLCPP_WARN(logger_, "osqp status_polish = %ld (unsuccessful)", status_polish); + return false; + } + if (status_polish == 0) { + RCLCPP_WARN(logger_, "osqp status_polish = %ld (unperformed)", status_polish); + return true; + } + return true; +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp new file mode 100644 index 0000000000000..1e2c8b114c1f0 --- /dev/null +++ b/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp @@ -0,0 +1,40 @@ +// Copyright 2018-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 "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() {} +bool8_t QPSolverEigenLeastSquareLLT::solve( + const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & /*a*/, + const Eigen::VectorXd & /*lb*/, const Eigen::VectorXd & /*ub*/, const Eigen::VectorXd & /*lb_a*/, + const Eigen::VectorXd & /*ub_a*/, Eigen::VectorXd & u) +{ + if (std::fabs(h_mat.determinant()) < 1.0E-9) {return false;} + + u = -h_mat.llt().solve(f_vec); + + return true; +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/smooth_stop.cpp b/control/trajectory_follower/src/smooth_stop.cpp new file mode 100644 index 0000000000000..3864860eca3eb --- /dev/null +++ b/control/trajectory_follower/src/smooth_stop.cpp @@ -0,0 +1,174 @@ +// 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 // NOLINT +#include +#include +#include + +#include "trajectory_follower/smooth_stop.hpp" + + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +void SmoothStop::init(const float64_t pred_vel_in_target, const float64_t pred_stop_dist) +{ + m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + + // when distance to stopline is near the car + if (pred_stop_dist < std::numeric_limits::epsilon()) { + m_strong_acc = m_params.min_strong_acc; + return; + } + + m_strong_acc = -std::pow(pred_vel_in_target, 2) / (2 * pred_stop_dist); + m_strong_acc = + std::max(std::min(m_strong_acc, m_params.max_strong_acc), m_params.min_strong_acc); +} + +void SmoothStop::setParams( + float64_t max_strong_acc, float64_t min_strong_acc, float64_t weak_acc, float64_t weak_stop_acc, + float64_t strong_stop_acc, float64_t min_fast_vel, float64_t min_running_vel, + float64_t min_running_acc, + float64_t weak_stop_time, float64_t weak_stop_dist, float64_t strong_stop_dist) +{ + m_params.max_strong_acc = max_strong_acc; + m_params.min_strong_acc = min_strong_acc; + m_params.weak_acc = weak_acc; + m_params.weak_stop_acc = weak_stop_acc; + m_params.strong_stop_acc = strong_stop_acc; + + m_params.min_fast_vel = min_fast_vel; + m_params.min_running_vel = min_running_vel; + m_params.min_running_acc = min_running_acc; + m_params.weak_stop_time = weak_stop_time; + + m_params.weak_stop_dist = weak_stop_dist; + m_params.strong_stop_dist = strong_stop_dist; + + m_is_set_params = true; +} + +std::experimental::optional SmoothStop::calcTimeToStop( + const std::vector> & vel_hist) const +{ + if (!m_is_set_params) { + throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); + } + + // return when vel_hist is empty + const float64_t vel_hist_size = static_cast(vel_hist.size()); + if (vel_hist_size == 0.0) { + return {}; + } + + // calculate some variables for fitting + const rclcpp::Time current_ros_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + float64_t mean_t = 0.0; + float64_t mean_v = 0.0; + float64_t sum_tv = 0.0; + float64_t sum_tt = 0.0; + for (const auto & vel : vel_hist) { + const float64_t t = (vel.first - current_ros_time).seconds(); + const float64_t v = vel.second; + + mean_t += t / vel_hist_size; + mean_v += v / vel_hist_size; + sum_tv += t * v; + sum_tt += t * t; + } + + // return when gradient a (of v = at + b) cannot be calculated. + // See the following calculation of a + if (std::abs(vel_hist_size * mean_t * mean_t - sum_tt) < + std::numeric_limits::epsilon()) + { + return {}; + } + + // calculate coefficients of linear function (v = at + b) + const float64_t a = + (vel_hist_size * mean_t * mean_v - sum_tv) / (vel_hist_size * mean_t * mean_t - sum_tt); + const float64_t b = mean_v - a * mean_t; + + // return when v is independent of time (v = b) + if (std::abs(a) < std::numeric_limits::epsilon()) { + return {}; + } + + // calculate time to stop by substituting v = 0 for v = at + b + const float64_t time_to_stop = -b / a; + if (time_to_stop > 0) { + return time_to_stop; + } + + return {}; +} + +float64_t SmoothStop::calculate( + const float64_t stop_dist, const float64_t current_vel, const float64_t current_acc, + const std::vector> & vel_hist, const float64_t delay_time) +{ + if (!m_is_set_params) { + throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); + } + + // predict time to stop + const auto time_to_stop = calcTimeToStop(vel_hist); + + // calculate some flags + const bool8_t is_fast_vel = std::abs(current_vel) > m_params.min_fast_vel; + const bool8_t is_running = std::abs(current_vel) > m_params.min_running_vel || + std::abs(current_acc) > m_params.min_running_acc; + + // when exceeding the stopline (stop_dist is negative in these cases.) + if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much + return m_params.strong_stop_acc; + } else if (stop_dist < m_params.weak_stop_dist) { // when exceeding the stopline a bit + return m_params.weak_stop_acc; + } + + // when the car is running + if (is_running) { + // when the car will not stop in a certain time + if (time_to_stop && *time_to_stop > m_params.weak_stop_time + delay_time) { + return m_strong_acc; + } else if (!time_to_stop && is_fast_vel) { + return m_strong_acc; + } + + m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + return m_params.weak_acc; + } + + // for 0.5 seconds after the car stopped + if ((rclcpp::Clock{RCL_ROS_TIME}.now() - m_weak_acc_time).seconds() < 0.5) { + return m_params.weak_acc; + } + + // when the car is not running + return m_params.strong_stop_acc; +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp new file mode 100644 index 0000000000000..a16d5694d0a89 --- /dev/null +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -0,0 +1,99 @@ +// Copyright 2018-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 "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp" + +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +DynamicsBicycleModel::DynamicsBicycleModel( + const float64_t wheelbase, const float64_t mass_fl, + const float64_t mass_fr, const float64_t mass_rl, + const float64_t mass_rr, const float64_t cf, const float64_t cr) +: VehicleModelInterface(/* dim_x */ 4, /* dim_u */ 1, /* dim_y */ 2, wheelbase) +{ + const float64_t mass_front = mass_fl + mass_fr; + const float64_t mass_rear = mass_rl + mass_rr; + + m_mass = mass_front + mass_rear; + m_lf = m_wheelbase * (1.0 - mass_front / m_mass); + m_lr = m_wheelbase * (1.0 - mass_rear / m_mass); + m_iz = m_lf * m_lf * mass_front + m_lr * m_lr * mass_rear; + m_cf = cf; + m_cr = cr; +} + +void DynamicsBicycleModel::calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const float64_t dt) +{ + /* + * x[k+1] = a_d*x[k] + b_d*u + w_d + */ + + const float64_t vel = std::max(m_velocity, 0.01); + + a_d = Eigen::MatrixXd::Zero(m_dim_x, m_dim_x); + a_d(0, 1) = 1.0; + a_d(1, 1) = -(m_cf + m_cr) / (m_mass * vel); + a_d(1, 2) = (m_cf + m_cr) / m_mass; + a_d(1, 3) = (m_lr * m_cr - m_lf * m_cf) / (m_mass * vel); + a_d(2, 3) = 1.0; + a_d(3, 1) = (m_lr * m_cr - m_lf * m_cf) / (m_iz * vel); + a_d(3, 2) = (m_lf * m_cf - m_lr * m_cr) / m_iz; + a_d(3, 3) = -(m_lf * m_lf * m_cf + m_lr * m_lr * m_cr) / (m_iz * vel); + + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x); + Eigen::MatrixXd a_d_inverse = (I - dt * 0.5 * a_d).inverse(); + + a_d = a_d_inverse * (I + dt * 0.5 * a_d); // bilinear discretization + + b_d = Eigen::MatrixXd::Zero(m_dim_x, m_dim_u); + b_d(0, 0) = 0.0; + b_d(1, 0) = m_cf / m_mass; + b_d(2, 0) = 0.0; + b_d(3, 0) = m_lf * m_cf / m_iz; + + w_d = Eigen::MatrixXd::Zero(m_dim_x, 1); + w_d(0, 0) = 0.0; + w_d(1, 0) = (m_lr * m_cr - m_lf * m_cf) / (m_mass * vel) - vel; + w_d(2, 0) = 0.0; + w_d(3, 0) = -(m_lf * m_lf * m_cf + m_lr * m_lr * m_cr) / (m_iz * vel); + + b_d = (a_d_inverse * dt) * b_d; + w_d = (a_d_inverse * dt * m_curvature * vel) * w_d; + + c_d = Eigen::MatrixXd::Zero(m_dim_y, m_dim_x); + c_d(0, 0) = 1.0; + c_d(1, 2) = 1.0; +} + +void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) +{ + const float64_t vel = std::max(m_velocity, 0.01); + const float64_t Kv = m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / + (2 * m_cr * m_wheelbase); + u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature; +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp new file mode 100644 index 0000000000000..b02ce12d4cca2 --- /dev/null +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -0,0 +1,74 @@ +// Copyright 2018-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 "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" + +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +KinematicsBicycleModel::KinematicsBicycleModel( + const float64_t wheelbase, const float64_t steer_lim, const float64_t steer_tau) +: VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2, wheelbase) +{ + m_steer_lim = steer_lim; + m_steer_tau = steer_tau; +} + +void KinematicsBicycleModel::calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const float64_t dt) +{ + auto sign = [](float64_t x) {return (x > 0.0) - (x < 0.0);}; + + /* Linearize delta around delta_r (reference delta) */ + float64_t delta_r = atan(m_wheelbase * m_curvature); + if (std::abs(delta_r) >= m_steer_lim) { + delta_r = m_steer_lim * static_cast(sign(delta_r)); + } + float64_t cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); + float64_t velocity = m_velocity; + if (std::abs(m_velocity) < 1e-04) {velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1);} + + a_d << 0.0, velocity, 0.0, 0.0, 0.0, velocity / m_wheelbase * cos_delta_r_squared_inv, 0.0, 0.0, + -1.0 / m_steer_tau; + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x); + a_d = (I - dt * 0.5 * a_d).inverse() * (I + dt * 0.5 * a_d); // bilinear discretization + + b_d << 0.0, 0.0, 1.0 / m_steer_tau; + b_d *= dt; + + c_d << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0; + + w_d << 0.0, + -velocity * m_curvature + + velocity / m_wheelbase * (tan(delta_r) - delta_r * cos_delta_r_squared_inv), + 0.0; + w_d *= dt; +} + +void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) +{ + u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp new file mode 100644 index 0000000000000..e9399953cce5c --- /dev/null +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -0,0 +1,67 @@ +// Copyright 2018-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 "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" + +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( + const float64_t wheelbase, const float64_t steer_lim) +: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2, wheelbase) +{ + m_steer_lim = steer_lim; +} + +void KinematicsBicycleModelNoDelay::calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const float64_t dt) +{ + auto sign = [](float64_t x) {return (x > 0.0) - (x < 0.0);}; + + /* Linearize delta around delta_r (reference delta) */ + float64_t delta_r = atan(m_wheelbase * m_curvature); + if (std::abs(delta_r) >= m_steer_lim) { + delta_r = m_steer_lim * static_cast(sign(delta_r)); + } + float64_t cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); + + a_d << 0.0, m_velocity, 0.0, 0.0; + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x); + a_d = I + a_d * dt; + + b_d << 0.0, m_velocity / m_wheelbase * cos_delta_r_squared_inv; + b_d *= dt; + + c_d << 1.0, 0.0, 0.0, 1.0; + + w_d << 0.0, -m_velocity / m_wheelbase * delta_r * cos_delta_r_squared_inv; + w_d *= dt; +} + +void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ref) +{ + u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); +} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp new file mode 100644 index 0000000000000..8344bd0d6f799 --- /dev/null +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp @@ -0,0 +1,40 @@ +// Copyright 2018-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 "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +VehicleModelInterface::VehicleModelInterface( + int64_t dim_x, int64_t dim_u, int64_t dim_y, + float64_t wheelbase) +: m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) +{ +} +int64_t VehicleModelInterface::getDimX() {return m_dim_x;} +int64_t VehicleModelInterface::getDimU() {return m_dim_u;} +int64_t VehicleModelInterface::getDimY() {return m_dim_y;} +float64_t VehicleModelInterface::getWheelbase() {return m_wheelbase;} +void VehicleModelInterface::setVelocity(const float64_t velocity) {m_velocity = velocity;} +void VehicleModelInterface::setCurvature(const float64_t curvature) {m_curvature = curvature;} +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/test/test_debug_values.cpp b/control/trajectory_follower/test/test_debug_values.cpp new file mode 100644 index 0000000000000..35eb7c248e305 --- /dev/null +++ b/control/trajectory_follower/test/test_debug_values.cpp @@ -0,0 +1,27 @@ +// 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 "gtest/gtest.h" +#include "trajectory_follower/debug_values.hpp" + +TEST(TestDebugValues, assign_and_get) { + using ::autoware::motion::control::trajectory_follower::DebugValues; + DebugValues debug; + + EXPECT_EQ(debug.getValues().size(), static_cast(DebugValues::TYPE::SIZE)); + debug.setValues(DebugValues::TYPE::DT, 42.0); + EXPECT_EQ(debug.getValues().at(debug.getValuesIdx(DebugValues::TYPE::DT)), 42.0); + debug.setValues(debug.getValuesIdx(DebugValues::TYPE::DT), 4.0); + EXPECT_EQ(debug.getValues().at(debug.getValuesIdx(DebugValues::TYPE::DT)), 4.0); +} diff --git a/control/trajectory_follower/test/test_interpolate.cpp b/control/trajectory_follower/test/test_interpolate.cpp new file mode 100644 index 0000000000000..59906bb21c751 --- /dev/null +++ b/control/trajectory_follower/test/test_interpolate.cpp @@ -0,0 +1,111 @@ +// 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 "common/types.hpp" +#include "gtest/gtest.h" +#include "trajectory_follower/interpolate.hpp" + +using autoware::common::types::float64_t; +TEST(TestInterpolate, Nominal) { + using autoware::motion::control::trajectory_follower::linearInterpolate; + + // Simple case + { + std::vector original_indexes = {1.0, 2.0, 3.0}; + std::vector original_values = {1.0, 2.0, 3.0}; + std::vector target_indexes = {1.5, 2.5}; + std::vector target_values; + + ASSERT_TRUE( + linearInterpolate( + original_indexes, original_values, target_indexes, + target_values)); + ASSERT_EQ(target_values.size(), target_indexes.size()); + for (size_t i = 0; i < target_values.size(); ++i) { + EXPECT_EQ(target_values[i], target_indexes[i]); + } + } + // Non regular indexes + { + std::vector original_indexes = {1.0, 1.5, 3.0}; + std::vector original_values = {1.0, 2.0, 3.5}; + std::vector target_indexes = {1.25, 2.5, 3.0}; + std::vector target_values; + + ASSERT_TRUE( + linearInterpolate( + original_indexes, original_values, target_indexes, + target_values)); + ASSERT_EQ(target_values.size(), target_indexes.size()); + EXPECT_EQ(target_values[0], 1.5); + EXPECT_EQ(target_values[1], 3.0); + EXPECT_EQ(target_values[2], 3.5); + } + // Single index query + { + std::vector original_indexes = {1.0, 1.5, 3.0}; + std::vector original_values = {1.0, 2.0, 3.5}; + float64_t target_index = 1.25; + float64_t target_value; + + ASSERT_TRUE( + linearInterpolate( + original_indexes, original_values, target_index, + target_value)); + EXPECT_EQ(target_value, 1.5); + } +} +TEST(TestInterpolate, Failure) { + using autoware::motion::control::trajectory_follower::linearInterpolate; + + std::vector target_values; + + // Non increasing indexes + ASSERT_FALSE( + linearInterpolate( + {1.0, 2.0, 1.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, + {1.0, 3.0}, target_values)); + ASSERT_FALSE( + linearInterpolate( + {1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, + {3.0, 1.0}, target_values)); + + // Target indexes out of range + ASSERT_FALSE( + linearInterpolate( + {1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, + {0.0, 3.0}, target_values)); + ASSERT_FALSE( + linearInterpolate( + {1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, + {1.0, 3.5}, target_values)); + + // Missmatched inputs + ASSERT_FALSE( + linearInterpolate( + {1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0}, + {1.0, 1.5}, target_values)); + ASSERT_FALSE( + linearInterpolate( + {1.0, 2.0, 3.0}, {1.0, 2.0, 3.0, 4.0}, + {1.0, 1.5}, target_values)); + + // Input size 0 + ASSERT_FALSE(linearInterpolate({}, {}, {1.0, 3.5}, target_values)); + + // Input size 1 + ASSERT_FALSE(linearInterpolate({1.5}, {1.5}, {1.0, 3.5}, target_values)); +} diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp new file mode 100644 index 0000000000000..54d065eeacd78 --- /dev/null +++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp @@ -0,0 +1,462 @@ +// 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 "gtest/gtest.h" +#include "trajectory_follower/longitudinal_controller_utils.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +namespace longitudinal_utils = ::autoware::motion::control::trajectory_follower::longitudinal_utils; + +TEST(TestLongitudinalControllerUtils, isValidTrajectory) { + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + Trajectory traj; + TrajectoryPoint point; + EXPECT_FALSE(longitudinal_utils::isValidTrajectory(traj)); + traj.points.push_back(point); + EXPECT_TRUE(longitudinal_utils::isValidTrajectory(traj)); + point.pose.position.x = std::numeric_limits::infinity(); + traj.points.push_back(point); + EXPECT_FALSE(longitudinal_utils::isValidTrajectory(traj)); +} + +TEST(TestLongitudinalControllerUtils, calcStopDistance) { + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using geometry_msgs::msg::Point; + Point current_pos; + current_pos.x = 0.0; + current_pos.y = 0.0; + Trajectory traj; + // empty trajectory : exception + EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, traj), std::invalid_argument); + // one point trajectory : exception + 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); + EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, traj), std::out_of_range); + traj.points.clear(); + // non stopping trajectory: stop distance = trajectory length + 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 = 1.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 2.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 2.0); + // stopping trajectory: stop distance = length until stopping point + point.pose.position.x = 3.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.0; + traj.points.push_back(point); + point.pose.position.x = 4.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 5.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.0; + traj.points.push_back(point); + EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 3.0); +} + +TEST(TestLongitudinalControllerUtils, getPitchByPose) { + tf2::Quaternion quaternion_tf; + quaternion_tf.setRPY(0.0, 0.0, 0.0); + EXPECT_EQ(longitudinal_utils::getPitchByPose(tf2::toMsg(quaternion_tf)), 0.0); + quaternion_tf.setRPY(0.0, 1.0, 0.0); + EXPECT_EQ(longitudinal_utils::getPitchByPose(tf2::toMsg(quaternion_tf)), 1.0); +} + +TEST(TestLongitudinalControllerUtils, getPitchByTraj) { + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + const double wheel_base = 0.9; + /** + * Trajectory: + * 1 X + * X + * 0 X X + * 0 1 2 3 + */ + Trajectory traj; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.pose.position.z = 0.0; + traj.points.push_back(point); + // non stopping trajectory: stop distance = trajectory length + point.pose.position.x = 1.0; + point.pose.position.y = 0.0; + point.pose.position.z = 1.0; + traj.points.push_back(point); + point.pose.position.x = 2.0; + point.pose.position.y = 0.0; + point.pose.position.z = 0.0; + traj.points.push_back(point); + point.pose.position.x = 3.0; + point.pose.position.y = 0.0; + point.pose.position.z = 0.5; + traj.points.push_back(point); + size_t closest_idx = 0; + EXPECT_DOUBLE_EQ( + std::abs( + longitudinal_utils::getPitchByTraj( + traj, closest_idx, + wheel_base)), M_PI_4); + closest_idx = 1; + EXPECT_DOUBLE_EQ( + std::abs( + longitudinal_utils::getPitchByTraj( + traj, closest_idx, + wheel_base)), M_PI_4); + closest_idx = 2; + EXPECT_DOUBLE_EQ( + std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), + std::atan2(0.5, 1)); + closest_idx = 3; + EXPECT_DOUBLE_EQ( + std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), + std::atan2(0.5, 1)); +} + +TEST(TestLongitudinalControllerUtils, calcElevationAngle) { + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + TrajectoryPoint p_from; + p_from.pose.position.x = 0.0; + p_from.pose.position.y = 0.0; + TrajectoryPoint p_to; + p_to.pose.position.x = 1.0; + p_to.pose.position.y = 0.0; + EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), 0.0); + p_to.pose.position.x = 1.0; + p_to.pose.position.z = 1.0; + EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4); + p_to.pose.position.x = -1.0; + p_to.pose.position.z = 1.0; + EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4); + p_to.pose.position.x = 0.0; + p_to.pose.position.z = 1.0; + EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_2); + p_to.pose.position.x = 1.0; + p_to.pose.position.z = -1.0; + EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); + p_to.pose.position.x = -1.0; + p_to.pose.position.z = -1.0; + EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); +} + +TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) { + using geometry_msgs::msg::Pose; + const double abs_err = 1e-7; + Pose current_pose; + current_pose.position.x = 0.0; + current_pose.position.y = 0.0; + current_pose.position.z = 0.0; + tf2::Quaternion quaternion_tf; + quaternion_tf.setRPY(0.0, 0.0, 0.0); + current_pose.orientation = tf2::toMsg(quaternion_tf); + + // With a delay and/or a velocity of 0.0 there is no change of position + double delay_time = 0.0; + double current_vel = 0.0; + Pose delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay( + current_pose, delay_time, + current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + + delay_time = 1.0; + current_vel = 0.0; + delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + + delay_time = 0.0; + current_vel = 1.0; + delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + + // With both delay and velocity: change of position + delay_time = 1.0; + current_vel = 1.0; + delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + + // Vary the yaw + quaternion_tf.setRPY(0.0, 0.0, M_PI); + current_pose.orientation = tf2::toMsg(quaternion_tf); + delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x - current_vel * delay_time, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + + quaternion_tf.setRPY(0.0, 0.0, M_PI_2); + current_pose.orientation = tf2::toMsg(quaternion_tf); + delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y + current_vel * delay_time, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + + quaternion_tf.setRPY(0.0, 0.0, -M_PI_2); + current_pose.orientation = tf2::toMsg(quaternion_tf); + delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y - current_vel * delay_time, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + + // Vary the pitch : no effect /!\ NOTE: bug with roll of +-PI/2 which rotates the yaw by PI + quaternion_tf.setRPY(0.0, M_PI_4, 0.0); + current_pose.orientation = tf2::toMsg(quaternion_tf); + delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + + // Vary the roll : no effect + quaternion_tf.setRPY(M_PI_2, 0.0, 0.0); + current_pose.orientation = tf2::toMsg(quaternion_tf); + delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); +} + +TEST(TestLongitudinalControllerUtils, lerpOrientation) { + geometry_msgs::msg::Quaternion result; + tf2::Quaternion o_from; + tf2::Quaternion o_to; + tf2::Quaternion o_result; + double roll; + double pitch; + double yaw; + double ratio; + + o_from.setRPY(0.0, 0.0, 0.0); + o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); + + ratio = 0.0; + result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + tf2::convert(result, o_result); + tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); + EXPECT_DOUBLE_EQ(roll, 0.0); + EXPECT_DOUBLE_EQ(pitch, 0.0); + EXPECT_DOUBLE_EQ(yaw, 0.0); + + ratio = 1.0; + result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + tf2::convert(result, o_result); + tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); + EXPECT_DOUBLE_EQ(roll, M_PI_4); + EXPECT_DOUBLE_EQ(pitch, M_PI_4); + EXPECT_DOUBLE_EQ(yaw, M_PI_4); + + ratio = 0.5; + o_to.setRPY(M_PI_4, 0.0, 0.0); + result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + tf2::convert(result, o_result); + tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); + EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); + EXPECT_DOUBLE_EQ(pitch, 0.0); + EXPECT_DOUBLE_EQ(yaw, 0.0); + + o_to.setRPY(0.0, M_PI_4, 0.0); + result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + tf2::convert(result, o_result); + tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); + EXPECT_DOUBLE_EQ(roll, 0.0); + EXPECT_DOUBLE_EQ(pitch, M_PI_4 / 2); + EXPECT_DOUBLE_EQ(yaw, 0.0); + + o_to.setRPY(0.0, 0.0, M_PI_4); + result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + tf2::convert(result, o_result); + tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); + EXPECT_DOUBLE_EQ(roll, 0.0); + EXPECT_DOUBLE_EQ(pitch, 0.0); + EXPECT_DOUBLE_EQ(yaw, M_PI_4 / 2); +} + +TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) { + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using geometry_msgs::msg::Point; + const double abs_err = 1e-15; + decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; + TrajectoryPoint p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 10.0; + p.acceleration_mps2 = 10.0; + points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 20.0; + p.acceleration_mps2 = 20.0; + points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 1.0; + p.longitudinal_velocity_mps = 30.0; + p.acceleration_mps2 = 30.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 1.0; + p.longitudinal_velocity_mps = 40.0; + p.acceleration_mps2 = 40.0; + points.push_back(p); + TrajectoryPoint result; + Point point; + + // Points on the trajectory gives back the original trajectory points values + point.x = 0.0; + point.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, point); + EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + EXPECT_NEAR(result.longitudinal_velocity_mps, 10.0, abs_err); + EXPECT_NEAR(result.acceleration_mps2, 10.0, abs_err); + + point.x = 1.0; + point.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, point); + EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + EXPECT_NEAR(result.longitudinal_velocity_mps, 20.0, abs_err); + EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err); + + point.x = 1.0; + point.y = 1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, point); + EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err); + EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err); + + point.x = 2.0; + point.y = 1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, point); + EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err); + EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err); + + // Interpolate between trajectory points + point.x = 0.5; + point.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, point); + EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); + point.x = 0.75; + point.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, point); + + EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + EXPECT_NEAR(result.longitudinal_velocity_mps, 17.5, abs_err); + EXPECT_NEAR(result.acceleration_mps2, 17.5, abs_err); + + // Interpolate away from the trajectory (interpolated point is projected) + point.x = 0.5; + point.y = -1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, point); + EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); + EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); + + // Ambiguous projections: possibility with the lowest index is used + point.x = 0.5; + point.y = 0.5; + result = longitudinal_utils::lerpTrajectoryPoint(points, point); + EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); + EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); +} + +TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) { + double dt = 1.0; + double max_val = 0.0; // cannot increase + double min_val = 0.0; // cannot decrease + double prev_val = 0.0; + + double input_val = 10.0; + EXPECT_DOUBLE_EQ( + longitudinal_utils::applyDiffLimitFilter( + input_val, prev_val, dt, max_val, + min_val), 0.0); + + max_val = 1.0; // can only increase by up to 1.0 at a time + EXPECT_DOUBLE_EQ( + longitudinal_utils::applyDiffLimitFilter( + input_val, prev_val, dt, max_val, + min_val), 1.0); + + input_val = -10.0; + EXPECT_DOUBLE_EQ( + longitudinal_utils::applyDiffLimitFilter( + input_val, prev_val, dt, max_val, + min_val), 0.0); + + min_val = -1.0; // can decrease by up to -1.0 at a time + EXPECT_DOUBLE_EQ( + longitudinal_utils::applyDiffLimitFilter( + input_val, prev_val, dt, max_val, + min_val), -1.0); + + dt = 5.0; // can now increase/decrease 5 times more + input_val = 10.0; + EXPECT_DOUBLE_EQ( + longitudinal_utils::applyDiffLimitFilter( + input_val, prev_val, dt, max_val, + min_val), 5.0); + input_val = -10.0; + EXPECT_DOUBLE_EQ( + longitudinal_utils::applyDiffLimitFilter( + input_val, prev_val, dt, max_val, + min_val), -5.0); + + dt = 1.0; + input_val = 100.0; + for (double prev = 0.0; prev < 100.0; ++prev) { + const double new_val = longitudinal_utils::applyDiffLimitFilter( + input_val, prev, dt, max_val, + min_val); + EXPECT_DOUBLE_EQ(new_val, prev + max_val); + prev = new_val; + } +} diff --git a/control/trajectory_follower/test/test_lowpass_filter.cpp b/control/trajectory_follower/test/test_lowpass_filter.cpp new file mode 100644 index 0000000000000..ca1f1f9896b08 --- /dev/null +++ b/control/trajectory_follower/test/test_lowpass_filter.cpp @@ -0,0 +1,113 @@ +// 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 "common/types.hpp" +#include "gtest/gtest.h" +#include "trajectory_follower/lowpass_filter.hpp" + +using autoware::common::types::float64_t; + +TEST(TestLowpassFilter, LowpassFilter1d) +{ + using autoware::motion::control::trajectory_follower::LowpassFilter1d; + + const float64_t epsilon = 1e-6; + LowpassFilter1d lowpass_filter_1d(0.0, 0.1); + + // initial state + EXPECT_NEAR(lowpass_filter_1d.getValue(), 0.0, epsilon); + + // random filter + EXPECT_NEAR(lowpass_filter_1d.filter(0.0), 0.0, epsilon); + EXPECT_NEAR(lowpass_filter_1d.filter(1.0), 0.9, epsilon); + EXPECT_NEAR(lowpass_filter_1d.filter(2.0), 1.89, epsilon); + EXPECT_NEAR(lowpass_filter_1d.getValue(), 1.89, epsilon); + + // reset + lowpass_filter_1d.reset(-1.1); + EXPECT_NEAR(lowpass_filter_1d.getValue(), -1.1, epsilon); + EXPECT_NEAR(lowpass_filter_1d.filter(0.0), -0.11, epsilon); + EXPECT_NEAR(lowpass_filter_1d.getValue(), -0.11, epsilon); +} +TEST(TestLowpassFilter, MoveAverageFilter) { + namespace MoveAverageFilter = autoware::motion::control::trajectory_follower::MoveAverageFilter; + + { // Fail case: window size higher than the vector size + const int64_t window_size = 5; + std::vector vec = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(MoveAverageFilter::filt_vector(window_size, vec)); + } + { + const int64_t window_size = 0; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + for (size_t i = 0; i < filtered_vec.size(); ++i) { + EXPECT_EQ(filtered_vec[i], original_vec[i]); + } + } + { + const int64_t window_size = 1; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 8.0 / 3); + EXPECT_EQ(filtered_vec[2], 13.0 / 3); + EXPECT_EQ(filtered_vec[3], original_vec[3]); + } + { + const int64_t window_size = 2; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 8.0 / 3); + EXPECT_EQ(filtered_vec[2], 21.0 / 5); + EXPECT_EQ(filtered_vec[3], 30.0 / 5); + EXPECT_EQ(filtered_vec[4], 23.0 / 3); + EXPECT_EQ(filtered_vec[5], original_vec[5]); + } +} +TEST(TestLowpassFilter, Butterworth2dFilter) { + using autoware::motion::control::trajectory_follower::Butterworth2dFilter; + const float64_t dt = 1.0; + const float64_t cutoff_hz = 1.0; + Butterworth2dFilter filter(dt, cutoff_hz); + for (float64_t i = 1.0; i < 10.0; ++i) { + EXPECT_LT(filter.filter(i), i); + } + + const std::vector original_vec = {1.0, 2.0, 3.0, 4.0}; + std::vector filtered_vec; + filter.filt_vector(original_vec, filtered_vec); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + for (size_t i = 1; i < filtered_vec.size(); ++i) { + EXPECT_LT(filtered_vec[i], original_vec[i]); + } + + filtered_vec.clear(); + filter.filtfilt_vector(original_vec, filtered_vec); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + + std::vector coefficients; + filter.getCoefficients(coefficients); + EXPECT_EQ(coefficients.size(), size_t(6)); +} diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/trajectory_follower/test/test_mpc.cpp new file mode 100644 index 0000000000000..52c9b60613082 --- /dev/null +++ b/control/trajectory_follower/test/test_mpc.cpp @@ -0,0 +1,500 @@ +// 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 "trajectory_follower/mpc.hpp" +#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" +#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" +#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.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_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "common/types.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "gtest/gtest.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +namespace +{ +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; +typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; +typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; +typedef autoware_auto_vehicle_msgs::msg::SteeringReport SteeringReport; +typedef geometry_msgs::msg::Pose Pose; +typedef geometry_msgs::msg::PoseStamped PoseStamped; +typedef autoware_auto_control_msgs::msg::AckermannLateralCommand AckermannLateralCommand; +typedef autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic Float32MultiArrayDiagnostic; + +class MPCTest : public ::testing::Test +{ +protected: + trajectory_follower::MPCParam param; + // Test inputs + Trajectory dummy_straight_trajectory; + Trajectory dummy_right_turn_trajectory; + SteeringReport neutral_steer; + Pose pose_zero; + PoseStamped::SharedPtr pose_zero_ptr; + float64_t default_velocity = 1.0; + rclcpp::Logger logger = rclcpp::get_logger("mpc_test_logger"); + // Vehicle model parameters + float64_t wheelbase = 2.7; + float64_t steer_limit = 1.0; + float64_t steer_tau = 0.1; + float64_t mass_fl = 600.0; + float64_t mass_fr = 600.0; + float64_t mass_rl = 600.0; + float64_t mass_rr = 600.0; + float64_t cf = 155494.663; + float64_t cr = 155494.663; + // Filters parameter + float64_t steering_lpf_cutoff_hz = 3.0; + float64_t error_deriv_lpf_cutoff_hz = 5.0; + // Test Parameters + float64_t admissible_position_error = 5.0; + float64_t admissible_yaw_error_rad = M_PI_2; + float64_t steer_lim = 0.610865; // 35 degrees + float64_t steer_rate_lim = 2.61799; // 150 degrees + float64_t ctrl_period = 0.03; + float64_t traj_resample_dist = 0.1; + int64_t path_filter_moving_ave_num = 35; + int64_t curvature_smoothing_num_traj = 1; + int64_t curvature_smoothing_num_ref_steer = 35; + bool8_t enable_path_smoothing = true; + bool8_t use_steer_prediction = true; + + void SetUp() override + { + param.prediction_horizon = 50; + param.prediction_dt = 0.1; + param.zero_ff_steer_deg = 0.5; + param.input_delay = 0.0; + param.acceleration_limit = 2.0; + param.velocity_time_constant = 0.3; + param.steer_tau = 0.1; + param.weight_lat_error = 1.0; + param.weight_heading_error = 1.0; + param.weight_heading_error_squared_vel = 1.0; + param.weight_terminal_lat_error = 1.0; + param.weight_terminal_heading_error = 0.1; + param.low_curvature_weight_lat_error = 0.1; + param.low_curvature_weight_heading_error = 0.0; + param.low_curvature_weight_heading_error_squared_vel = 0.3; + param.weight_steering_input = 1.0; + param.weight_steering_input_squared_vel = 0.25; + param.weight_lat_jerk = 0.0; + param.weight_steer_rate = 0.0; + param.weight_steer_acc = 0.000001; + param.low_curvature_weight_steering_input = 1.0; + param.low_curvature_weight_steering_input_squared_vel = 0.25; + param.low_curvature_weight_lat_jerk = 0.0; + param.low_curvature_weight_steer_rate = 0.0; + param.low_curvature_weight_steer_acc = 0.000001; + param.low_curvature_thresh_curvature = 0.0; + + TrajectoryPoint p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_straight_trajectory.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_straight_trajectory.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_straight_trajectory.points.push_back(p); + p.pose.position.x = 3.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_straight_trajectory.points.push_back(p); + p.pose.position.x = 4.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_straight_trajectory.points.push_back(p); + + p.pose.position.x = -1.0; + p.pose.position.y = -1.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_right_turn_trajectory.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_right_turn_trajectory.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = -1.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_right_turn_trajectory.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = -2.0; + p.longitudinal_velocity_mps = 1.0f; + dummy_right_turn_trajectory.points.push_back(p); + + neutral_steer.steering_tire_angle = 0.0; + pose_zero.position.x = 0.0; + pose_zero.position.y = 0.0; + pose_zero_ptr = std::make_shared(); + pose_zero_ptr->pose = pose_zero; + } + + void initializeMPC(trajectory_follower::MPC & mpc) + { + mpc.m_param = param; + mpc.m_admissible_position_error = admissible_position_error; + mpc.m_admissible_yaw_error_rad = admissible_yaw_error_rad; + mpc.m_steer_lim = steer_lim; + mpc.m_steer_rate_lim = steer_rate_lim; + mpc.m_ctrl_period = ctrl_period; + mpc.m_use_steer_prediction = use_steer_prediction; + // Init filters + mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + // Init trajectory + mpc.setReferenceTrajectory( + dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + pose_zero_ptr); + } +}; // class MPCTest + +/* cppcheck-suppress syntaxError */ +TEST_F(MPCTest, InitializeAndCalculate) { + trajectory_follower::MPC mpc; + EXPECT_FALSE(mpc.hasVehicleModel()); + EXPECT_FALSE(mpc.hasQPSolver()); + + const std::string vehicle_model_type = "kinematics"; + std::shared_ptr vehicle_model_ptr = + std::make_shared( + wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + ASSERT_TRUE(mpc.hasVehicleModel()); + + std::shared_ptr qpsolver_ptr = + std::make_shared(); + mpc.setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc.hasQPSolver()); + + // Init parameters and reference trajectory + initializeMPC(mpc); + + // Calculate MPC + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); +} + +TEST_F(MPCTest, InitializeAndCalculateRightTurn) { + trajectory_follower::MPC mpc; + EXPECT_FALSE(mpc.hasVehicleModel()); + EXPECT_FALSE(mpc.hasQPSolver()); + + const std::string vehicle_model_type = "kinematics"; + std::shared_ptr vehicle_model_ptr = + std::make_shared( + wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + ASSERT_TRUE(mpc.hasVehicleModel()); + + std::shared_ptr qpsolver_ptr = + std::make_shared(); + mpc.setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc.hasQPSolver()); + + // Init parameters and reference trajectory + initializeMPC(mpc); + mpc.setReferenceTrajectory( + dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + pose_zero_ptr); + + // Calculate MPC + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); +} + +TEST_F(MPCTest, OsqpCalculate) { + trajectory_follower::MPC mpc; + initializeMPC(mpc); + mpc.setReferenceTrajectory( + dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + pose_zero_ptr); + + const std::string vehicle_model_type = "kinematics"; + std::shared_ptr vehicle_model_ptr = + std::make_shared( + wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + ASSERT_TRUE(mpc.hasVehicleModel()); + + std::shared_ptr qpsolver_ptr = + std::make_shared(logger); + mpc.setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc.hasQPSolver()); + + // Calculate MPC + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + // with OSQP this function returns false despite finding correct solutions + EXPECT_FALSE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); +} + +TEST_F(MPCTest, OsqpCalculateRightTurn) { + trajectory_follower::MPC mpc; + initializeMPC(mpc); + mpc.setReferenceTrajectory( + dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + pose_zero_ptr); + + const std::string vehicle_model_type = "kinematics"; + std::shared_ptr vehicle_model_ptr = + std::make_shared( + wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + ASSERT_TRUE(mpc.hasVehicleModel()); + + std::shared_ptr qpsolver_ptr = + std::make_shared(logger); + mpc.setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc.hasQPSolver()); + + // Calculate MPC + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); +} + +TEST_F(MPCTest, KinematicsNoDelayCalculate) { + trajectory_follower::MPC mpc; + initializeMPC(mpc); + + const std::string vehicle_model_type = "kinematics_no_delay"; + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + ASSERT_TRUE(mpc.hasVehicleModel()); + + std::shared_ptr qpsolver_ptr = + std::make_shared(); + mpc.setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc.hasQPSolver()); + + // Init filters + mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + // Init trajectory + mpc.setReferenceTrajectory( + dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + pose_zero_ptr); + // Calculate MPC + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); +} + +TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { + trajectory_follower::MPC mpc; + initializeMPC(mpc); + mpc.setReferenceTrajectory( + dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + pose_zero_ptr); + + const std::string vehicle_model_type = "kinematics_no_delay"; + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + ASSERT_TRUE(mpc.hasVehicleModel()); + + std::shared_ptr qpsolver_ptr = + std::make_shared(); + mpc.setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc.hasQPSolver()); + + // Init filters + mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + + // Calculate MPC + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); +} + +TEST_F(MPCTest, DynamicCalculate) { + trajectory_follower::MPC mpc; + initializeMPC(mpc); + + const std::string vehicle_model_type = "dynamics"; + std::shared_ptr vehicle_model_ptr = + std::make_shared( + wheelbase, mass_fl, mass_fr, + mass_rl, mass_rr, cf, cr); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + ASSERT_TRUE(mpc.hasVehicleModel()); + + std::shared_ptr qpsolver_ptr = + std::make_shared(); + mpc.setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc.hasQPSolver()); + + // Calculate MPC + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); +} + +TEST_F(MPCTest, MultiSolveWithBuffer) { + trajectory_follower::MPC mpc; + const std::string vehicle_model_type = "kinematics"; + std::shared_ptr vehicle_model_ptr = + std::make_shared( + wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + std::shared_ptr qpsolver_ptr = + std::make_shared(); + mpc.setQPSolver(qpsolver_ptr); + + // Init parameters and reference trajectory + initializeMPC(mpc); + + mpc.m_input_buffer = {0.0, 0.0, 0.0}; + // Calculate MPC + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); + EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); + EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); + EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); + ASSERT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); + EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); + EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); + EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); +} + +TEST_F(MPCTest, FailureCases) { + trajectory_follower::MPC mpc; + const std::string vehicle_model_type = "kinematics"; + std::shared_ptr vehicle_model_ptr = + std::make_shared( + wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + std::shared_ptr qpsolver_ptr = + std::make_shared(); + mpc.setQPSolver(qpsolver_ptr); + + // Init parameters and reference trajectory + initializeMPC(mpc); + + // Calculate MPC with a pose too far from the trajectory + Pose pose_far; + pose_far.position.x = pose_zero.position.x - admissible_position_error - 1.0; + pose_far.position.y = pose_zero.position.y - admissible_position_error - 1.0; + AckermannLateralCommand ctrl_cmd; + Trajectory pred_traj; + Float32MultiArrayDiagnostic diag; + EXPECT_FALSE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_far, ctrl_cmd, pred_traj, + diag)); + + // Calculate MPC with a fast velocity to make the prediction go further than the reference path + EXPECT_FALSE( + mpc.calculateMPC(neutral_steer, default_velocity + 10.0, pose_far, ctrl_cmd, pred_traj, diag)); + + // Set a wrong vehicle model (not a failure but generates an error message) + const std::string wrong_vehicle_model_type = "wrong_model"; + vehicle_model_ptr = std::make_shared( + wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr, wrong_vehicle_model_type); + EXPECT_TRUE( + mpc.calculateMPC( + neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, + diag)); +} +} // namespace diff --git a/control/trajectory_follower/test/test_mpc_trajectory.cpp b/control/trajectory_follower/test/test_mpc_trajectory.cpp new file mode 100644 index 0000000000000..95835077567c8 --- /dev/null +++ b/control/trajectory_follower/test/test_mpc_trajectory.cpp @@ -0,0 +1,59 @@ +// 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 "common/types.hpp" +#include "gtest/gtest.h" +#include "trajectory_follower/mpc_trajectory.hpp" + +using autoware::common::types::float64_t; +TEST(TestMPCTrajectory, Nominal) { + typedef autoware::motion::control::trajectory_follower::MPCTrajectory MPCTrajectory; + + MPCTrajectory traj; + EXPECT_EQ(traj.size(), size_t(0)); + traj.clear(); + EXPECT_EQ(traj.size(), size_t(0)); + + traj.push_back(0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0); + ASSERT_EQ(traj.size(), size_t(1)); + EXPECT_EQ(traj.x[0], 0.0); + EXPECT_EQ(traj.y[0], 1.0); + EXPECT_EQ(traj.z[0], 2.0); + EXPECT_EQ(traj.yaw[0], 3.0); + EXPECT_EQ(traj.vx[0], 4.0); + EXPECT_EQ(traj.k[0], 5.0); + EXPECT_EQ(traj.smooth_k[0], 6.0); + EXPECT_EQ(traj.relative_time[0], 7.0); + + for (float64_t i = 1; i < 11.0; ++i) { + traj.push_back(i + 0.0, i + 1.0, i + 2.0, i + 3.0, i + 4.0, i + 5.0, i + 6.0, i + 7.0); + } + ASSERT_EQ(traj.size(), size_t(11)); + for (size_t i = 0; i < size_t(11); ++i) { + const float64_t j = static_cast(i); + EXPECT_EQ(traj.x[i], j); + EXPECT_EQ(traj.y[i], j + 1.0); + EXPECT_EQ(traj.z[i], j + 2.0); + EXPECT_EQ(traj.yaw[i], j + 3.0); + EXPECT_EQ(traj.vx[i], j + 4.0); + EXPECT_EQ(traj.k[i], j + 5.0); + EXPECT_EQ(traj.smooth_k[i], j + 6.0); + EXPECT_EQ(traj.relative_time[i], j + 7.0); + } + + traj.clear(); + EXPECT_EQ(traj.size(), size_t(0)); +} diff --git a/control/trajectory_follower/test/test_mpc_utils.cpp b/control/trajectory_follower/test/test_mpc_utils.cpp new file mode 100644 index 0000000000000..d01b26d958e24 --- /dev/null +++ b/control/trajectory_follower/test/test_mpc_utils.cpp @@ -0,0 +1,112 @@ +// 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/mpc_utils.hpp" +#include "trajectory_follower/mpc_trajectory.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "gtest/gtest.h" + +namespace +{ +namespace MPCUtils = ::autoware::motion::control::trajectory_follower::MPCUtils; +typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; +typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; +typedef geometry_msgs::msg::Pose Pose; + +TEST(TestMPCUtils, CalcNearestIndex) { + Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + Trajectory trajectory; + TrajectoryPoint p; + p.pose.position.x = -2.0; + p.pose.position.y = 1.0; + trajectory.points.push_back(p); + p.pose.position.x = -1.0; + p.pose.position.y = 1.0; + trajectory.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 1.0; + trajectory.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 1.0; + trajectory.points.push_back(p); + EXPECT_EQ(MPCUtils::calcNearestIndex(trajectory, pose), 2); +} + +/* cppcheck-suppress syntaxError */ +TEST(TestMPC, CalcStopDistance) { + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + + Trajectory trajectory_msg; + TrajectoryPoint p; + // Point 0 + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + trajectory_msg.points.push_back(p); + // Point 1 + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + trajectory_msg.points.push_back(p); + // Point 2 - STOP + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + trajectory_msg.points.push_back(p); + // Point 3 + p.pose.position.x = 3.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + trajectory_msg.points.push_back(p); + // Point 4 + p.pose.position.x = 4.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + trajectory_msg.points.push_back(p); + // Point 5 + p.pose.position.x = 5.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + trajectory_msg.points.push_back(p); + // Point 6 - STOP + p.pose.position.x = 6.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + trajectory_msg.points.push_back(p); + // Point 7 - STOP + p.pose.position.x = 7.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + trajectory_msg.points.push_back(p); + + EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 0), 2.0); + EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 1), 1.0); + EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 2), 0.0); + EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 3), 3.0); + EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 4), 2.0); + EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 5), 1.0); + EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 6), 0.0); + EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 7), -1.0); +} +} // namespace diff --git a/control/trajectory_follower/test/test_pid.cpp b/control/trajectory_follower/test/test_pid.cpp new file mode 100644 index 0000000000000..dc92912d2bb7a --- /dev/null +++ b/control/trajectory_follower/test/test_pid.cpp @@ -0,0 +1,68 @@ +// 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 "gtest/gtest.h" +#include "trajectory_follower/pid.hpp" + +TEST(TestPID, calculate_pid_output) { + using ::autoware::motion::control::trajectory_follower::PIDController; + std::vector contributions; + const double dt = 1.0; + double target = 10.0; + double current = 0.0; + bool enable_integration = false; + + PIDController pid; + + // Cannot calculate before initializing gains and limits + EXPECT_THROW(pid.calculate(0.0, dt, enable_integration, contributions), std::runtime_error); + + pid.setGains(1.0, 1.0, 1.0); + pid.setLimits(10.0, 0.0, 10.0, 0.0, 10.0, 0.0, 10.0, 0.0); + double error = target - current; + double prev_error = error; + while (current != target) { + current = pid.calculate(error, dt, enable_integration, contributions); + EXPECT_EQ(contributions[0], error); + EXPECT_EQ(contributions[1], 0.0); // integration is deactivated + EXPECT_EQ(contributions[2], error - prev_error); + prev_error = error; + error = target - current; + } + pid.reset(); + + pid.setGains(100.0, 100.0, 100.0); + pid.setLimits(10.0, -10.0, 10.0, -10.0, 10.0, -10.0, 10.0, -10.0); + enable_integration = true; + + // High errors to force each component to its upper limit + EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0); + for (double error = 100.0; error < 1000.0; error += 100.0) { + EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), 10.0); + EXPECT_EQ(contributions[0], 10.0); + EXPECT_EQ(contributions[1], 10.0); // integration is activated + EXPECT_EQ(contributions[2], 10.0); + } + + // Low errors to force each component to its lower limit + EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0); + for (double error = -100.0; error > -1000.0; error -= 100.0) { + EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), -10.0); + EXPECT_EQ(contributions[0], -10.0); + EXPECT_EQ(contributions[1], -10.0); // integration is activated + EXPECT_EQ(contributions[2], -10.0); + } +} diff --git a/control/trajectory_follower/test/test_smooth_stop.cpp b/control/trajectory_follower/test/test_smooth_stop.cpp new file mode 100644 index 0000000000000..43d289d08b77d --- /dev/null +++ b/control/trajectory_follower/test/test_smooth_stop.cpp @@ -0,0 +1,117 @@ +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "trajectory_follower/smooth_stop.hpp" + +TEST(TestSmoothStop, calculate_stopping_acceleration) { + using ::autoware::motion::control::trajectory_follower::SmoothStop; + using rclcpp::Time; + using rclcpp::Duration; + + const double max_strong_acc = -0.5; + const double min_strong_acc = -1.0; + const double weak_acc = -0.3; + const double weak_stop_acc = -0.8; + const double strong_stop_acc = -3.4; + const double max_fast_vel = 0.5; + const double min_running_vel = 0.01; + const double min_running_acc = 0.01; + const double weak_stop_time = 0.8; + const double weak_stop_dist = -0.3; + const double strong_stop_dist = -0.5; + + const double delay_time = 0.17; + + SmoothStop ss; + + // Cannot calculate before setting parameters + EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time), std::runtime_error); + + ss.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); + + double vel_in_target; + double stop_dist; + double current_vel; + double current_acc = 0.0; + const Time now = rclcpp::Clock{RCL_ROS_TIME}.now(); + const std::vector> velocity_history = { + {now - Duration(3, 0), 3.0}, {now - Duration(2, 0), 2.0}, {now - Duration(1, 0), 1.0}}; + double accel; + + // strong stop when the stop distance is below the threshold + vel_in_target = 5.0; + stop_dist = strong_stop_dist - 0.1; + current_vel = 2.0; + ss.init(vel_in_target, stop_dist); + accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + EXPECT_EQ(accel, strong_stop_acc); + + // weak stop when the stop distance is below the threshold (but not bellow the strong_stop_dist) + stop_dist = weak_stop_dist - 0.1; + current_vel = 2.0; + ss.init(vel_in_target, stop_dist); + accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + EXPECT_EQ(accel, weak_stop_acc); + + // if not running, weak accel for 0.5 seconds after the previous init or previous weak_acc + rclcpp::Rate rate_quart(1.0 / 0.25); + rclcpp::Rate rate_half(1.0 / 0.5); + stop_dist = 0.0; + current_vel = 0.0; + EXPECT_EQ( + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + rate_quart.sleep(); + EXPECT_EQ( + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + rate_half.sleep(); + EXPECT_NE( + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + + // strong stop when the car is not running (and is at least 0.5seconds after initialization) + EXPECT_EQ( + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), + strong_stop_acc); + + // accel between min/max_strong_acc when the car is running: + // not predicted to exceed the stop line and is predicted to stop after weak_stop_time + delay + stop_dist = 1.0; + current_vel = 1.0; + vel_in_target = 1.0; + ss.init(vel_in_target, stop_dist); + EXPECT_EQ( + ss.calculate( + stop_dist, current_vel, current_acc, velocity_history, + delay_time), max_strong_acc); + + vel_in_target = std::sqrt(2.0); + ss.init(vel_in_target, stop_dist); + EXPECT_EQ( + ss.calculate( + stop_dist, current_vel, current_acc, velocity_history, + delay_time), min_strong_acc); + + for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) { + ss.init(vel_in_target, stop_dist); + accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + EXPECT_GT(accel, min_strong_acc); + EXPECT_LT(accel, max_strong_acc); + } +}