diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt new file mode 100644 index 0000000000000..2167805d99c9b --- /dev/null +++ b/control/mpc_lateral_controller/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(mpc_lateral_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib) +ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED + src/mpc_lateral_controller.cpp + 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 +) + +if(BUILD_TESTING) + set(TEST_LAT_SOURCES + test/test_mpc.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_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) + target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${MPC_LAT_CON_LIB}) +endif() + +ament_auto_package(INSTALL_TO_SHARE + param +) diff --git a/control/trajectory_follower/design/mpc_lateral_controller-design.md b/control/mpc_lateral_controller/README.md similarity index 97% rename from control/trajectory_follower/design/mpc_lateral_controller-design.md rename to control/mpc_lateral_controller/README.md index 0883d160cb89b..7596f753c5edc 100644 --- a/control/trajectory_follower/design/mpc_lateral_controller-design.md +++ b/control/mpc_lateral_controller/README.md @@ -1,7 +1,7 @@ # MPC Lateral Controller This is the design document for the lateral controller node -in the `trajectory_follower_nodes` package. +in the `trajectory_follower_node` package. ## Purpose / Use cases @@ -58,7 +58,7 @@ The tracking is not accurate if the first point of the reference trajectory is a ### Inputs -Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) +Set the following from the [controller_node](../trajectory_follower_node/README.md) - `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry @@ -66,7 +66,7 @@ Set the following from the [controller_node](../../trajectory_follower_nodes/des ### Outputs -Return LateralOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) +Return LateralOutput which contains the following to the controller node - `autoware_auto_control_msgs/AckermannLateralCommand` - LateralSyncData diff --git a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp similarity index 82% rename from control/trajectory_follower/include/trajectory_follower/interpolate.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp index 52c26c9330e1a..7f83383a5d65d 100644 --- a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp @@ -12,10 +12,8 @@ // 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 "trajectory_follower/visibility_control.hpp" +#ifndef MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ +#define MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ #include #include @@ -27,7 +25,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** @@ -37,7 +35,7 @@ namespace trajectory_follower * @param [in] return_index desired interpolated indexes * @param [out] return_value resulting interpolated values */ -TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate( +bool linearInterpolate( const std::vector & base_index, const std::vector & base_value, const std::vector & return_index, std::vector & return_value); /** @@ -47,11 +45,11 @@ TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate( * @param [in] return_index desired interpolated index * @param [out] return_value resulting interpolated value */ -TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate( +bool linearInterpolate( const std::vector & base_index, const std::vector & base_value, const double & return_index, double & return_value); -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_ +#endif // MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp similarity index 73% rename from control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp index c006c50a6ea2c..e3bd55526adea 100644 --- a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp @@ -12,10 +12,8 @@ // 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 "trajectory_follower/visibility_control.hpp" +#ifndef MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ #include #include @@ -28,54 +26,13 @@ namespace motion { namespace control { -namespace trajectory_follower -{ - -/** - * @brief Simple filter with gain on the first derivative of the value - */ -class LowpassFilter1d +namespace mpc_lateral_controller { -private: - double m_x; //!< @brief current filtered value - double 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 double x, const double gain) : m_x(x), m_gain(gain) {} - - /** - * @brief set the current value of the filter - * @param [in] x new value - */ - void reset(const double x) { m_x = x; } - - /** - * @brief get the current value of the filter - */ - double getValue() const { return m_x; } - - /** - * @brief filter a new value - * @param [in] u new value - */ - double filter(const double u) - { - const double 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 +class Butterworth2dFilter { private: double m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time @@ -149,10 +106,10 @@ namespace MoveAverageFilter * @param [in] num index distance for moving average filter * @param [out] u object vector */ -TRAJECTORY_FOLLOWER_PUBLIC bool filt_vector(const int num, std::vector & u); +bool filt_vector(const int num, std::vector & u); } // namespace MoveAverageFilter -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ +#endif // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp similarity index 88% rename from control/trajectory_follower/include/trajectory_follower/mpc.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index c64c231144b80..5616f26bab641 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -12,26 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__MPC_HPP_ -#define TRAJECTORY_FOLLOWER__MPC_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ +#define MPC_LATERAL_CONTROLLER__MPC_HPP_ #include "geometry/common_2d.hpp" #include "helper_functions/angle_utils.hpp" +#include "mpc_lateral_controller/interpolate.hpp" +#include "mpc_lateral_controller/lowpass_filter.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" #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_planning_msgs/msg/trajectory.hpp" @@ -50,7 +49,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { struct MPCParam @@ -143,7 +142,7 @@ struct MPCMatrix * MPC-based waypoints follower class * @brief calculate control command to follow reference waypoints */ -class TRAJECTORY_FOLLOWER_PUBLIC MPC +class MPC { private: //!< @brief ROS logger used for debug logging @@ -154,15 +153,15 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC //!< @brief vehicle model type for MPC std::string m_vehicle_model_type; //!< @brief vehicle model for MPC - std::shared_ptr m_vehicle_model_ptr; + std::shared_ptr m_vehicle_model_ptr; //!< @brief qp solver for MPC - std::shared_ptr m_qpsolver_ptr; + std::shared_ptr m_qpsolver_ptr; //!< @brief lowpass filter for steering command - trajectory_follower::Butterworth2dFilter m_lpf_steering_cmd; + mpc_lateral_controller::Butterworth2dFilter m_lpf_steering_cmd; //!< @brief lowpass filter for lateral error - trajectory_follower::Butterworth2dFilter m_lpf_lateral_error; + mpc_lateral_controller::Butterworth2dFilter m_lpf_lateral_error; //!< @brief lowpass filter for heading error - trajectory_follower::Butterworth2dFilter m_lpf_yaw_error; + mpc_lateral_controller::Butterworth2dFilter m_lpf_yaw_error; //!< @brief raw output computed two iterations ago double m_raw_steer_cmd_pprev = 0.0; @@ -185,7 +184,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @brief get variables for mpc calculation */ bool getData( - const trajectory_follower::MPCTrajectory & traj, + const mpc_lateral_controller::MPCTrajectory & traj, const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, const geometry_msgs::msg::Pose & current_pose, MPCData * data); /** @@ -217,14 +216,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @param [out] x updated state at delayed_time */ bool updateStateForDelayCompensation( - const trajectory_follower::MPCTrajectory & traj, const double & start_time, + const mpc_lateral_controller::MPCTrajectory & traj, const double & 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, const double prediction_dt); + const mpc_lateral_controller::MPCTrajectory & reference_trajectory, const double prediction_dt); /** * @brief generate MPC matrix with trajectory and vehicle model * @param [in] mpc_matrix parameters matrix to use for optimization @@ -240,20 +239,20 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC */ bool resampleMPCTrajectoryByTime( const double start_time, const double prediction_dt, - const trajectory_follower::MPCTrajectory & input, - trajectory_follower::MPCTrajectory * output) const; + const mpc_lateral_controller::MPCTrajectory & input, + mpc_lateral_controller::MPCTrajectory * output) const; /** * @brief apply velocity dynamics filter with v0 from closest index */ - trajectory_follower::MPCTrajectory applyVelocityDynamicsFilter( - const trajectory_follower::MPCTrajectory & trajectory, + mpc_lateral_controller::MPCTrajectory applyVelocityDynamicsFilter( + const mpc_lateral_controller::MPCTrajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const double v0) const; /** * @brief get prediction delta time of mpc. * If trajectory length is shorter than min_prediction length, adjust delta time. */ double getPredictionDeltaTime( - const double start_time, const trajectory_follower::MPCTrajectory & input, + const double start_time, const mpc_lateral_controller::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose) const; /** * @brief add weights related to lateral_jerk, steering_rate, steering_acc into R @@ -349,7 +348,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC public: //!< @brief reference trajectory to be followed - trajectory_follower::MPCTrajectory m_ref_traj; + mpc_lateral_controller::MPCTrajectory m_ref_traj; //!< @brief MPC design parameter MPCParam m_param; //!< @brief mpc_output buffer for delay time compensation @@ -405,7 +404,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @brief set the vehicle model of this MPC */ inline void setVehicleModel( - std::shared_ptr vehicle_model_ptr, + std::shared_ptr vehicle_model_ptr, const std::string & vehicle_model_type) { m_vehicle_model_ptr = vehicle_model_ptr; @@ -414,7 +413,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief set the QP solver of this MPC */ - inline void setQPSolver(std::shared_ptr qpsolver_ptr) + inline void setQPSolver(std::shared_ptr qpsolver_ptr) { m_qpsolver_ptr = qpsolver_ptr; } @@ -445,9 +444,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC */ inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; } }; // class MPC -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__MPC_HPP_ +#endif // MPC_LATERAL_CONTROLLER__MPC_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp similarity index 83% rename from control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 8b764d3fb7b32..b2c18d0d7d721 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -12,26 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ -#define TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ - +#ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ + +#include "mpc_lateral_controller/interpolate.hpp" +#include "mpc_lateral_controller/lowpass_filter.hpp" +#include "mpc_lateral_controller/mpc.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "trajectory_follower/interpolate.hpp" -#include "trajectory_follower/lateral_controller_base.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/mpc.hpp" -#include "trajectory_follower/mpc_trajectory.hpp" -#include "trajectory_follower/mpc_utils.hpp" -#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" -#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "trajectory_follower_base/lateral_controller_base.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" @@ -55,12 +54,12 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; -class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralControllerBase +class MpcLateralController : public trajectory_follower::LateralControllerBase { public: /** @@ -112,7 +111,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController std::deque m_trajectory_buffer; // MPC object - trajectory_follower::MPC m_mpc; + mpc_lateral_controller::MPC m_mpc; //!< @brief measured kinematic state nav_msgs::msg::Odometry m_current_kinematic_state; @@ -139,12 +138,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController //!< initialize timer to work in real, simulation, and replay void initTimer(double period_s); - bool isReady(const InputData & input_data) override; + bool isReady(const trajectory_follower::InputData & input_data) override; /** * @brief compute control command for path follow with a constant control period */ - LateralOutput run(InputData const & input_data) override; + trajectory_follower::LateralOutput run( + trajectory_follower::InputData const & input_data) override; /** * @brief set m_current_trajectory with received message @@ -212,9 +212,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ +#endif // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp similarity index 90% rename from control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index 9016c0adbb689..5abb267eb9d9d 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -12,11 +12,10 @@ // 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_ +#ifndef MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#define MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ #include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include "trajectory_follower/visibility_control.hpp" #include "geometry_msgs/msg/point.hpp" @@ -29,14 +28,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Trajectory class for mpc follower * @brief calculate control command to follow reference waypoints */ -class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory +class MPCTrajectory { public: std::vector x; //!< @brief x position x vector @@ -97,8 +96,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory return points; } }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ +#endif // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp similarity index 83% rename from control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index f9f58deadd6bb..f72e7157d6a8f 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #include "eigen3/Eigen/Core" #include "geometry/common_2d.hpp" @@ -29,9 +29,8 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "trajectory_follower/interpolate.hpp" -#include "trajectory_follower/mpc_trajectory.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/interpolate.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -48,7 +47,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace MPCUtils { @@ -58,19 +57,19 @@ namespace MPCUtils * @param [in] yaw input yaw angle * @return quaternion */ -TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw); +geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & 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); +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 double calcLateralError( +double 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 @@ -78,7 +77,7 @@ TRAJECTORY_FOLLOWER_PUBLIC double calcLateralError( * @param [out] output resulting MPCTrajectory * @return true if the conversion was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool convertToMPCTrajectory( +bool convertToMPCTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output); /** * @brief convert the given MPCTrajectory to a Trajectory msg @@ -86,22 +85,21 @@ TRAJECTORY_FOLLOWER_PUBLIC bool convertToMPCTrajectory( * @param [out] output resulting Trajectory msg * @return true if the conversion was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool convertToAutowareTrajectory( +bool 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); +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 bool resampleMPCTrajectoryByDistance( +bool resampleMPCTrajectoryByDistance( const MPCTrajectory & input, const double resample_interval_dist, MPCTrajectory * output); /** * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired @@ -111,7 +109,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool resampleMPCTrajectoryByDistance( * @param [in] out_index desired interpolated indexes * @param [out] out_traj resulting interpolated MPCTrajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpMPCTrajectory( +bool linearInterpMPCTrajectory( const std::vector & in_index, const MPCTrajectory & in_traj, const std::vector & out_index, MPCTrajectory * out_traj); /** @@ -119,7 +117,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpMPCTrajectory( * @param [in] traj MPCTrajectory for which to fill in the relative_time * @return true if the calculation was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool calcMPCTrajectoryTime(MPCTrajectory & traj); +bool 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 @@ -128,7 +126,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcMPCTrajectoryTime(MPCTrajectory & traj); * @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( +void dynamicSmoothingVelocity( const size_t start_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj); /** @@ -136,8 +134,7 @@ TRAJECTORY_FOLLOWER_PUBLIC void dynamicSmoothingVelocity( * @param [inout] traj object trajectory * @param [in] shift is forward or not */ -TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY( - MPCTrajectory * traj, const bool is_forward_shift); +void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift); /** * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 * points when num = 1) @@ -146,7 +143,7 @@ TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY( * calculation * @param [inout] traj object trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature( +bool calcTrajectoryCurvature( const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer, MPCTrajectory * traj); /** @@ -156,7 +153,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature( * @param [in] traj input trajectory * @return vector of curvatures at each point of the given trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( +std::vector calcTrajectoryCurvature( const size_t curvature_smoothing_num, const MPCTrajectory & traj); /** * @brief calculate nearest pose on MPCTrajectory with linear interpolation @@ -169,7 +166,7 @@ TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( * @param [in] clock to throttle log output * @return false when nearest pose couldn't find for some reasons */ -TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp( +bool calcNearestPoseInterp( const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, double * nearest_time, const double max_dist, const double max_yaw, const rclcpp::Logger & logger, @@ -177,7 +174,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp( // /** // * @brief calculate distance to stopped point // */ -TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance( +double calcStopDistance( const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin); /** @@ -195,8 +192,8 @@ void extendTrajectoryInYawDirection( const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); } // namespace MPCUtils -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ +#endif // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp similarity index 83% rename from control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index dc48253d7c0d0..27123abfa8beb 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -12,13 +12,12 @@ // 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_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Dense" #include "eigen3/Eigen/LU" -#include "trajectory_follower/visibility_control.hpp" namespace autoware { @@ -26,11 +25,11 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /// Interface for solvers of Quadratic Programming (QP) problems -class TRAJECTORY_FOLLOWER_PUBLIC QPSolverInterface +class QPSolverInterface { public: /** @@ -55,8 +54,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverInterface 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 mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp similarity index 83% rename from control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 546b16769bfc9..744ec33db2e57 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -12,14 +12,13 @@ // 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_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ #include "eigen3/Eigen/Dense" +#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/qp_solver/qp_solver_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" namespace autoware { @@ -27,11 +26,11 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /// Solver for QP problems using the OSQP library -class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface +class QPSolverOSQP : public QPSolverInterface { public: /** @@ -65,8 +64,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface autoware::common::osqp::OSQPInterface osqpsolver_; rclcpp::Logger logger_; }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp similarity index 81% rename from control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp index f96ff6621521c..82d88b15465ab 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp @@ -12,14 +12,13 @@ // 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_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Dense" #include "eigen3/Eigen/LU" -#include "trajectory_follower/qp_solver/qp_solver_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include @@ -29,14 +28,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Solver for QP problems using least square decomposition * implement with Eigen's standard Cholesky decomposition (LLT) */ -class TRAJECTORY_FOLLOWER_PUBLIC QPSolverEigenLeastSquareLLT : public QPSolverInterface +class QPSolverEigenLeastSquareLLT : public QPSolverInterface { public: /** @@ -66,8 +65,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverEigenLeastSquareLLT : public QPSolverIn 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 mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp similarity index 88% rename from control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 91c873d54bad1..94a6326d3980a 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -44,13 +44,12 @@ * 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_ +#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware { @@ -58,14 +57,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Vehicle model class of bicycle dynamics * @brief calculate model-related values */ -class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInterface +class DynamicsBicycleModel : public VehicleModelInterface { public: /** @@ -113,8 +112,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInter double m_cf; //!< @brief front cornering power [N/rad] double m_cr; //!< @brief rear cornering power [N/rad] }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#endif // MPC_LATERAL_CONTROLLER__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/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 85% rename from control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 6ab35f7a23413..dfd62a540a077 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -38,13 +38,12 @@ * */ -#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware { @@ -52,14 +51,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Vehicle model class of bicycle kinematics * @brief calculate model-related values */ -class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInterface +class KinematicsBicycleModel : public VehicleModelInterface { public: /** @@ -97,8 +96,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInt double m_steer_lim; //!< @brief steering angle limit [rad] double m_steer_tau; //!< @brief steering time constant for 1d-model [s] }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // MPC_LATERAL_CONTROLLER__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/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp similarity index 82% rename from control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 88b3909bcf7f0..0306826344bbc 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -35,13 +35,12 @@ * */ -#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware { @@ -49,14 +48,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Vehicle model class of bicycle kinematics without steering delay * @brief calculate model-related values */ -class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleModelInterface +class KinematicsBicycleModelNoDelay : public VehicleModelInterface { public: /** @@ -92,8 +91,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleM private: double m_steer_lim; //!< @brief steering angle limit [rad] }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#endif // MPC_LATERAL_CONTROLLER__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/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp similarity index 88% rename from control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index b2ededcfb7808..86dc86941274e 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -12,11 +12,10 @@ // 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_ +#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #include "eigen3/Eigen/Core" -#include "trajectory_follower/visibility_control.hpp" namespace autoware { @@ -24,14 +23,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Vehicle model class * @brief calculate model-related values */ -class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface +class VehicleModelInterface { protected: const int m_dim_x; //!< @brief dimension of state x @@ -110,8 +109,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface */ virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0; }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml new file mode 100644 index 0000000000000..c805abe341bef --- /dev/null +++ b/control/mpc_lateral_controller/package.xml @@ -0,0 +1,50 @@ + + + + mpc_lateral_controller + 1.0.0 + MPC-based lateral controller + + Takamasa Horibe + Takayuki Murooka + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_geometry + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + diagnostic_msgs + diagnostic_updater + eigen + geometry_msgs + interpolation + motion_utils + osqp_interface + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + trajectory_follower_base + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml similarity index 100% rename from control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml rename to control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml diff --git a/control/trajectory_follower/src/interpolate.cpp b/control/mpc_lateral_controller/src/interpolate.cpp similarity index 97% rename from control/trajectory_follower/src/interpolate.cpp rename to control/mpc_lateral_controller/src/interpolate.cpp index 3e833dcd8b183..e3309fed61652 100644 --- a/control/trajectory_follower/src/interpolate.cpp +++ b/control/mpc_lateral_controller/src/interpolate.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/interpolate.hpp" +#include "mpc_lateral_controller/interpolate.hpp" #include #include @@ -28,7 +28,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace { @@ -137,7 +137,7 @@ bool linearInterpolate( return_value = return_value_v.at(0); return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp similarity index 97% rename from control/trajectory_follower/src/longitudinal_controller_utils.cpp rename to control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp index 21a79b574c93f..6f65171e2c80a 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/longitudinal_controller_utils.hpp" +#include "mpc_lateral_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" @@ -34,7 +34,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace longitudinal_utils { @@ -182,7 +182,7 @@ double applyDiffLimitFilter( return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); } } // namespace longitudinal_utils -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp similarity index 96% rename from control/trajectory_follower/src/lowpass_filter.cpp rename to control/mpc_lateral_controller/src/lowpass_filter.cpp index e45221cf9c6a5..40097e46cd1c4 100644 --- a/control/trajectory_follower/src/lowpass_filter.cpp +++ b/control/mpc_lateral_controller/src/lowpass_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/lowpass_filter.hpp" +#include "mpc_lateral_controller/lowpass_filter.hpp" #include @@ -22,7 +22,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { Butterworth2dFilter::Butterworth2dFilter(double dt, double f_cutoff_hz) { @@ -142,7 +142,7 @@ bool filt_vector(const int num, std::vector & u) return true; } } // namespace MoveAverageFilter -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp similarity index 91% rename from control/trajectory_follower/src/mpc.cpp rename to control/mpc_lateral_controller/src/mpc.cpp index ed6472700e14a..5a3d56319a7be 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/mpc.hpp" +#include "mpc_lateral_controller/mpc.hpp" #include "motion_utils/motion_utils.hpp" @@ -33,7 +33,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { using namespace std::literals::chrono_literals; @@ -45,7 +45,7 @@ bool MPC::calculateMPC( tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic) { /* recalculate velocity from ego-velocity with dynamics */ - trajectory_follower::MPCTrajectory reference_trajectory = + mpc_lateral_controller::MPCTrajectory reference_trajectory = applyVelocityDynamicsFilter(m_ref_traj, current_pose, current_velocity); MPCData mpc_data; @@ -66,7 +66,7 @@ bool MPC::calculateMPC( } /* resample ref_traj with mpc sampling time */ - trajectory_follower::MPCTrajectory mpc_resampled_ref_traj; + mpc_lateral_controller::MPCTrajectory mpc_resampled_ref_traj; const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay; const double prediction_dt = getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_pose); @@ -107,7 +107,7 @@ bool MPC::calculateMPC( /* calculate predicted trajectory */ Eigen::VectorXd Xex = mpc_matrix.Aex * x0 + mpc_matrix.Bex * Uex + mpc_matrix.Wex; - trajectory_follower::MPCTrajectory mpc_predicted_traj; + mpc_lateral_controller::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 int DIM_X = m_vehicle_model_ptr->getDimX(); @@ -123,7 +123,7 @@ bool MPC::calculateMPC( const double 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); + mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj); /* prepare diagnostic message */ const double nearest_k = reference_trajectory.k[static_cast(mpc_data.nearest_idx)]; @@ -182,13 +182,13 @@ void MPC::setReferenceTrajectory( const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control) { - 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 + mpc_lateral_controller::MPCTrajectory mpc_traj_raw; // received raw trajectory + mpc_lateral_controller::MPCTrajectory mpc_traj_resampled; // resampled trajectory + mpc_lateral_controller::MPCTrajectory mpc_traj_smoothed; // smooth filtered trajectory /* resampling */ - trajectory_follower::MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw); - if (!trajectory_follower::MPCUtils::resampleMPCTrajectoryByDistance( + mpc_lateral_controller::MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw); + if (!mpc_lateral_controller::MPCUtils::resampleMPCTrajectoryByDistance( mpc_traj_raw, traj_resample_dist, &mpc_traj_resampled)) { RCLCPP_WARN(m_logger, "[setReferenceTrajectory] spline error when resampling by distance"); return; @@ -204,13 +204,13 @@ void MPC::setReferenceTrajectory( const int 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( + !mpc_lateral_controller::MoveAverageFilter::filt_vector( path_filter_moving_ave_num, mpc_traj_smoothed.x) || - !trajectory_follower::MoveAverageFilter::filt_vector( + !mpc_lateral_controller::MoveAverageFilter::filt_vector( path_filter_moving_ave_num, mpc_traj_smoothed.y) || - !trajectory_follower::MoveAverageFilter::filt_vector( + !mpc_lateral_controller::MoveAverageFilter::filt_vector( path_filter_moving_ave_num, mpc_traj_smoothed.yaw) || - !trajectory_follower::MoveAverageFilter::filt_vector( + !mpc_lateral_controller::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; @@ -224,16 +224,16 @@ void MPC::setReferenceTrajectory( * well-defined considering the end point attitude angle, this feature is not necessary. */ if (extend_trajectory_for_end_yaw_control) { - trajectory_follower::MPCUtils::extendTrajectoryInYawDirection( + mpc_lateral_controller::MPCUtils::extendTrajectoryInYawDirection( mpc_traj_raw.yaw.back(), traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed); } /* calculate yaw angle */ - trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); - trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); + mpc_lateral_controller::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); + mpc_lateral_controller::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); /* calculate curvature */ - trajectory_follower::MPCUtils::calcTrajectoryCurvature( + mpc_lateral_controller::MPCUtils::calcTrajectoryCurvature( static_cast(curvature_smoothing_num_traj), static_cast(curvature_smoothing_num_ref_steer), &mpc_traj_smoothed); @@ -264,13 +264,13 @@ void MPC::resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport } bool MPC::getData( - const trajectory_follower::MPCTrajectory & traj, + const mpc_lateral_controller::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( + if (!mpc_lateral_controller::MPCUtils::calcNearestPoseInterp( traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), ego_nearest_dist_threshold, ego_nearest_yaw_threshold, m_logger, *m_clock)) { // reset previous MPC result @@ -288,7 +288,7 @@ bool MPC::getData( 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); + mpc_lateral_controller::MPCUtils::calcLateralError(current_pose, data->nearest_pose); data->yaw_err = autoware::common::helper_functions::wrap_angle( tf2::getYaw(current_pose.orientation) - tf2::getYaw(data->nearest_pose.orientation)); @@ -406,14 +406,14 @@ void MPC::storeSteerCmd(const double steer) } bool MPC::resampleMPCTrajectoryByTime( - const double ts, const double prediction_dt, const trajectory_follower::MPCTrajectory & input, - trajectory_follower::MPCTrajectory * output) const + const double ts, const double prediction_dt, const mpc_lateral_controller::MPCTrajectory & input, + mpc_lateral_controller::MPCTrajectory * output) const { std::vector mpc_time_v; for (double i = 0; i < static_cast(m_param.prediction_horizon); ++i) { mpc_time_v.push_back(ts + i * prediction_dt); } - if (!trajectory_follower::MPCUtils::linearInterpMPCTrajectory( + if (!mpc_lateral_controller::MPCUtils::linearInterpMPCTrajectory( input.relative_time, input, mpc_time_v, output)) { RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, 1000 /*ms*/, @@ -453,7 +453,8 @@ Eigen::VectorXd MPC::getInitialState(const MPCData & data) } bool MPC::updateStateForDelayCompensation( - const trajectory_follower::MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x) + const mpc_lateral_controller::MPCTrajectory & traj, const double & start_time, + Eigen::VectorXd * x) { const int DIM_X = m_vehicle_model_ptr->getDimX(); const int DIM_U = m_vehicle_model_ptr->getDimU(); @@ -470,8 +471,8 @@ bool MPC::updateStateForDelayCompensation( double k = 0.0; double 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)) { + !mpc_lateral_controller::linearInterpolate(traj.relative_time, traj.k, mpc_curr_time, k) || + !mpc_lateral_controller::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; @@ -490,12 +491,12 @@ bool MPC::updateStateForDelayCompensation( return true; } -trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( - const trajectory_follower::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose, - const double v0) const +mpc_lateral_controller::MPCTrajectory MPC::applyVelocityDynamicsFilter( + const mpc_lateral_controller::MPCTrajectory & input, + const geometry_msgs::msg::Pose & current_pose, const double v0) const { autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( + autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( input, autoware_traj); if (autoware_traj.points.empty()) { return input; @@ -507,8 +508,8 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( const double acc_lim = m_param.acceleration_limit; const double tau = m_param.velocity_time_constant; - trajectory_follower::MPCTrajectory output = input; - trajectory_follower::MPCUtils::dynamicSmoothingVelocity( + mpc_lateral_controller::MPCTrajectory output = input; + mpc_lateral_controller::MPCUtils::dynamicSmoothingVelocity( static_cast(nearest_idx), v0, acc_lim, tau, output); const double t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time const double t_end = output.relative_time.back() + t_ext; @@ -526,7 +527,7 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) */ MPCMatrix MPC::generateMPCMatrix( - const trajectory_follower::MPCTrajectory & reference_trajectory, const double prediction_dt) + const mpc_lateral_controller::MPCTrajectory & reference_trajectory, const double prediction_dt) { using Eigen::MatrixXd; @@ -788,12 +789,12 @@ void MPC::addSteerWeightF(const double prediction_dt, Eigen::MatrixXd * f_ptr) c } double MPC::getPredictionDeltaTime( - const double start_time, const trajectory_follower::MPCTrajectory & input, + const double start_time, const mpc_lateral_controller::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose) const { // Calculate the time min_prediction_length ahead from current_pose autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( + autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( input, autoware_traj); const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); @@ -865,7 +866,7 @@ bool MPC::isValid(const MPCMatrix & m) const return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp similarity index 95% rename from control/trajectory_follower/src/mpc_lateral_controller.cpp rename to control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index b9c5e9c77af41..9ccb35bac795d 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/mpc_lateral_controller.hpp" +#include "mpc_lateral_controller/mpc_lateral_controller.hpp" #include "motion_utils/motion_utils.hpp" #include "tf2_ros/create_timer_ros.h" @@ -31,7 +31,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace { @@ -89,12 +89,12 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* vehicle model setup */ const std::string vehicle_model_type = node_->declare_parameter("vehicle_model_type"); - std::shared_ptr vehicle_model_ptr; + std::shared_ptr vehicle_model_ptr; if (vehicle_model_type == "kinematics") { - vehicle_model_ptr = std::make_shared( + vehicle_model_ptr = std::make_shared( wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); } else if (vehicle_model_type == "kinematics_no_delay") { - vehicle_model_ptr = std::make_shared( + vehicle_model_ptr = std::make_shared( wheelbase, m_mpc.m_steer_lim); } else if (vehicle_model_type == "dynamics") { const double mass_fl = node_->declare_parameter("vehicle.mass_fl"); @@ -106,7 +106,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time // // NOLINT - vehicle_model_ptr = std::make_shared( + vehicle_model_ptr = std::make_shared( wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); } else { RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); @@ -114,11 +114,11 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* QP solver setup */ const std::string qp_solver_type = node_->declare_parameter("qp_solver_type"); - std::shared_ptr qpsolver_ptr; + std::shared_ptr qpsolver_ptr; if (qp_solver_type == "unconstraint_fast") { - qpsolver_ptr = std::make_shared(); + qpsolver_ptr = std::make_shared(); } else if (qp_solver_type == "osqp") { - qpsolver_ptr = std::make_shared(node_->get_logger()); + qpsolver_ptr = std::make_shared(node_->get_logger()); } else { RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); } @@ -173,7 +173,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} MpcLateralController::~MpcLateralController() {} -LateralOutput MpcLateralController::run(InputData const & input_data) +trajectory_follower::LateralOutput MpcLateralController::run( + trajectory_follower::InputData const & input_data) { // set input data setTrajectory(input_data.current_trajectory); @@ -197,7 +198,7 @@ LateralOutput MpcLateralController::run(InputData const & input_data) publishDebugValues(debug_values); const auto createLateralOutput = [this](const auto & cmd) { - LateralOutput output; + trajectory_follower::LateralOutput output; output.control_cmd = createCtrlCmdMsg(cmd); output.sync_data.is_steer_converged = isSteerConverged(cmd); return output; @@ -241,7 +242,7 @@ bool MpcLateralController::isSteerConverged( return is_converged; } -bool MpcLateralController::isReady(const InputData & input_data) +bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data) { setTrajectory(input_data.current_trajectory); m_current_kinematic_state = input_data.current_odometry; @@ -429,7 +430,7 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( result.reason = "success"; // strong exception safety wrt MPCParam - trajectory_follower::MPCParam param = m_mpc.m_param; + mpc_lateral_controller::MPCParam param = m_mpc.m_param; try { update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon); update_param(parameters, "mpc_prediction_dt", param.prediction_dt); @@ -524,7 +525,7 @@ bool MpcLateralController::isValidTrajectory( return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/mpc_trajectory.cpp b/control/mpc_lateral_controller/src/mpc_trajectory.cpp similarity index 93% rename from control/trajectory_follower/src/mpc_trajectory.cpp rename to control/mpc_lateral_controller/src/mpc_trajectory.cpp index cf0a45763a7ff..810d7eaf7997f 100644 --- a/control/trajectory_follower/src/mpc_trajectory.cpp +++ b/control/mpc_lateral_controller/src/mpc_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/mpc_trajectory.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" namespace autoware { @@ -20,7 +20,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { void MPCTrajectory::push_back( const double & xp, const double & yp, const double & zp, const double & yawp, const double & vxp, @@ -60,7 +60,7 @@ size_t MPCTrajectory::size() const return 0; } } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp similarity index 98% rename from control/trajectory_follower/src/mpc_utils.cpp rename to control/mpc_lateral_controller/src/mpc_utils.cpp index 6e5b31d340e6b..142dfc50f0751 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/mpc_utils.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" #include "motion_utils/motion_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -28,7 +28,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace MPCUtils { @@ -411,7 +411,7 @@ void extendTrajectoryInYawDirection( // get terminal pose autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( + autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( traj, autoware_traj); auto extended_pose = autoware_traj.points.back().pose; @@ -429,7 +429,7 @@ void extendTrajectoryInYawDirection( } } // namespace MPCUtils -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp similarity index 95% rename from control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp rename to control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index cbfd57e998503..a90c4457e36a5 100644 --- a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include #include @@ -23,7 +23,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger} {} bool QPSolverOSQP::solve( @@ -80,7 +80,7 @@ bool QPSolverOSQP::solve( } return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp similarity index 89% rename from control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp rename to control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp index 18692d175922b..7208473ccef6d 100644 --- a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" namespace autoware { @@ -20,7 +20,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() {} bool QPSolverEigenLeastSquareLLT::solve( @@ -36,7 +36,7 @@ bool QPSolverEigenLeastSquareLLT::solve( return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/smooth_stop.cpp b/control/mpc_lateral_controller/src/smooth_stop.cpp similarity index 97% rename from control/trajectory_follower/src/smooth_stop.cpp rename to control/mpc_lateral_controller/src/smooth_stop.cpp index 9b5dc14f75248..26cfcabfebead 100644 --- a/control/trajectory_follower/src/smooth_stop.cpp +++ b/control/mpc_lateral_controller/src/smooth_stop.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/smooth_stop.hpp" +#include "mpc_lateral_controller/smooth_stop.hpp" #include // NOLINT @@ -28,7 +28,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { void SmoothStop::init(const double pred_vel_in_target, const double pred_stop_dist) { @@ -164,7 +164,7 @@ double SmoothStop::calculate( // when the car is not running return m_params.strong_stop_acc; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp similarity index 95% rename from control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp rename to control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index fa7e354b6d318..a634fa02232b4 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -12,7 +12,7 @@ // 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 "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include @@ -22,7 +22,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { DynamicsBicycleModel::DynamicsBicycleModel( const double wheelbase, const double mass_fl, const double mass_fr, const double mass_rl, @@ -92,7 +92,7 @@ void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) 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 mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp similarity index 94% rename from control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp rename to control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 73c3aabea3fb0..9955ce43fcb1b 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -12,7 +12,7 @@ // 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 "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include @@ -22,7 +22,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { KinematicsBicycleModel::KinematicsBicycleModel( const double wheelbase, const double steer_lim, const double steer_tau) @@ -74,7 +74,7 @@ void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp similarity index 92% rename from control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp rename to control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index d9a399c5740c2..7cd94e1406e10 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -12,7 +12,7 @@ // 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 "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include @@ -22,7 +22,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( const double wheelbase, const double steer_lim) @@ -65,7 +65,7 @@ void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp similarity index 90% rename from control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp rename to control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index 52a0d5c7f9c8b..432e98a672e88 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware { @@ -20,7 +20,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase) : m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) @@ -32,7 +32,7 @@ int VehicleModelInterface::getDimY() { return m_dim_y; } double VehicleModelInterface::getWheelbase() { return m_wheelbase; } void VehicleModelInterface::setVelocity(const double velocity) { m_velocity = velocity; } void VehicleModelInterface::setCurvature(const double curvature) { m_curvature = curvature; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/test/test_interpolate.cpp b/control/mpc_lateral_controller/test/test_interpolate.cpp similarity index 93% rename from control/trajectory_follower/test/test_interpolate.cpp rename to control/mpc_lateral_controller/test/test_interpolate.cpp index 69715b024d0dd..97b2d28205ee4 100644 --- a/control/trajectory_follower/test/test_interpolate.cpp +++ b/control/mpc_lateral_controller/test/test_interpolate.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/interpolate.hpp" +#include "mpc_lateral_controller/interpolate.hpp" #include TEST(TestInterpolate, Nominal) { - using autoware::motion::control::trajectory_follower::linearInterpolate; + using autoware::motion::control::mpc_lateral_controller::linearInterpolate; // Simple case { @@ -62,7 +62,7 @@ TEST(TestInterpolate, Nominal) } TEST(TestInterpolate, Failure) { - using autoware::motion::control::trajectory_follower::linearInterpolate; + using autoware::motion::control::mpc_lateral_controller::linearInterpolate; std::vector target_values; diff --git a/control/trajectory_follower/test/test_lowpass_filter.cpp b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp similarity index 74% rename from control/trajectory_follower/test/test_lowpass_filter.cpp rename to control/mpc_lateral_controller/test/test_lowpass_filter.cpp index 691cdebb9b94d..89afe436dd2d1 100644 --- a/control/trajectory_follower/test/test_lowpass_filter.cpp +++ b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -13,41 +13,20 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/lowpass_filter.hpp" +#include "mpc_lateral_controller/lowpass_filter.hpp" #include -TEST(TestLowpassFilter, LowpassFilter1d) -{ - using autoware::motion::control::trajectory_follower::LowpassFilter1d; - - const double 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; + namespace MoveAverageFilter = + autoware::motion::control::mpc_lateral_controller::MoveAverageFilter; { // Fail case: window size higher than the vector size const int window_size = 5; std::vector vec = {1.0, 2.0, 3.0, 4.0}; EXPECT_FALSE(MoveAverageFilter::filt_vector(window_size, vec)); - } // namespace autoware::motion::control::trajectory_follower::MoveAverageFilter; + } // namespace autoware::motion::control::mpc_lateral_controller::MoveAverageFilter; { const int window_size = 0; const std::vector original_vec = {1.0, 3.0, 4.0, 6.0}; @@ -85,7 +64,7 @@ TEST(TestLowpassFilter, MoveAverageFilter) } TEST(TestLowpassFilter, Butterworth2dFilter) { - using autoware::motion::control::trajectory_follower::Butterworth2dFilter; + using autoware::motion::control::mpc_lateral_controller::Butterworth2dFilter; const double dt = 1.0; const double cutoff_hz = 1.0; Butterworth2dFilter filter(dt, cutoff_hz); diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp similarity index 81% rename from control/trajectory_follower/test/test_mpc.cpp rename to control/mpc_lateral_controller/test/test_mpc.cpp index 6aa767289c1fd..056dcd77c2154 100644 --- a/control/trajectory_follower/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -13,10 +13,10 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/mpc.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_kinematics.hpp" +#include "mpc_lateral_controller/mpc.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -38,7 +38,7 @@ namespace { -namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; +namespace mpc_lateral_controller = ::autoware::motion::control::mpc_lateral_controller; typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; typedef autoware_auto_vehicle_msgs::msg::SteeringReport SteeringReport; @@ -50,7 +50,7 @@ typedef tier4_debug_msgs::msg::Float32MultiArrayStamped Float32MultiArrayStamped class MPCTest : public ::testing::Test { protected: - trajectory_follower::MPCParam param; + mpc_lateral_controller::MPCParam param; // Test inputs Trajectory dummy_straight_trajectory; Trajectory dummy_right_turn_trajectory; @@ -162,7 +162,7 @@ class MPCTest : public ::testing::Test pose_zero_ptr->pose = pose_zero; } - void initializeMPC(trajectory_follower::MPC & mpc) + void initializeMPC(mpc_lateral_controller::MPC & mpc) { mpc.m_param = param; mpc.m_admissible_position_error = admissible_position_error; @@ -184,19 +184,19 @@ class MPCTest : public ::testing::Test /* cppcheck-suppress syntaxError */ TEST_F(MPCTest, InitializeAndCalculate) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::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( + 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(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -215,19 +215,19 @@ TEST_F(MPCTest, InitializeAndCalculate) TEST_F(MPCTest, InitializeAndCalculateRightTurn) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::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( + 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(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -250,7 +250,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) TEST_F(MPCTest, OsqpCalculate) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, @@ -258,14 +258,14 @@ TEST_F(MPCTest, OsqpCalculate) extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + 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); + std::shared_ptr qpsolver_ptr = + std::make_shared(logger); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -282,7 +282,7 @@ TEST_F(MPCTest, OsqpCalculate) TEST_F(MPCTest, OsqpCalculateRightTurn) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, @@ -290,14 +290,14 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + 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); + std::shared_ptr qpsolver_ptr = + std::make_shared(logger); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -313,17 +313,17 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) TEST_F(MPCTest, KinematicsNoDelayCalculate) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::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); + 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(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -346,7 +346,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, @@ -354,13 +354,13 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics_no_delay"; - std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit); + 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(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -379,18 +379,18 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) TEST_F(MPCTest, DynamicCalculate) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); const std::string vehicle_model_type = "dynamics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + 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(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -406,14 +406,14 @@ TEST_F(MPCTest, DynamicCalculate) TEST_F(MPCTest, MultiSolveWithBuffer) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + 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(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory @@ -448,14 +448,14 @@ TEST_F(MPCTest, MultiSolveWithBuffer) TEST_F(MPCTest, FailureCases) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + 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(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory @@ -477,7 +477,7 @@ TEST_F(MPCTest, FailureCases) // 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( + vehicle_model_ptr = std::make_shared( wheelbase, steer_limit, steer_tau); mpc.setVehicleModel(vehicle_model_ptr, wrong_vehicle_model_type); EXPECT_TRUE( diff --git a/control/trajectory_follower/test/test_mpc_utils.cpp b/control/mpc_lateral_controller/test/test_mpc_utils.cpp similarity index 94% rename from control/trajectory_follower/test/test_mpc_utils.cpp rename to control/mpc_lateral_controller/test/test_mpc_utils.cpp index 7883537885bec..01c8e55699c62 100644 --- a/control/trajectory_follower/test/test_mpc_utils.cpp +++ b/control/mpc_lateral_controller/test/test_mpc_utils.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/mpc_trajectory.hpp" -#include "trajectory_follower/mpc_utils.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -25,7 +25,7 @@ namespace { -namespace MPCUtils = ::autoware::motion::control::trajectory_follower::MPCUtils; +namespace MPCUtils = ::autoware::motion::control::mpc_lateral_controller::MPCUtils; typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; typedef geometry_msgs::msg::Pose Pose; diff --git a/control/pid_longitudinal_controller/CMakeLists.txt b/control/pid_longitudinal_controller/CMakeLists.txt new file mode 100644 index 0000000000000..3e7a992b15e00 --- /dev/null +++ b/control/pid_longitudinal_controller/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(pid_longitudinal_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(PID_LON_CON_LIB ${PROJECT_NAME}_lib) +ament_auto_add_library(${PID_LON_CON_LIB} SHARED + src/pid_longitudinal_controller.cpp + src/pid.cpp + src/smooth_stop.cpp + src/longitudinal_controller_utils.cpp +) + +if(BUILD_TESTING) + set(TEST_LON_SOURCES + 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_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) + target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${PID_LON_CON_LIB}) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param +) diff --git a/control/trajectory_follower/design/pid_longitudinal_controller-design.md b/control/pid_longitudinal_controller/README.md similarity index 98% rename from control/trajectory_follower/design/pid_longitudinal_controller-design.md rename to control/pid_longitudinal_controller/README.md index 0cca6803490cd..7147a76b8eeff 100644 --- a/control/trajectory_follower/design/pid_longitudinal_controller-design.md +++ b/control/pid_longitudinal_controller/README.md @@ -79,7 +79,7 @@ This PID logic has a maximum value for the output of each term. This is to preve - 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. +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. @@ -125,14 +125,14 @@ There are two sources of the slope information, which can be switched by a param ### Input -Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) +Set the following from the [controller_node](../trajectory_follower_node/README.md) - `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry ### Output -Return LongitudinalOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) +Return LongitudinalOutput which contains the following to the controller node - `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. - LongitudinalSyncData diff --git a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp similarity index 89% rename from control/trajectory_follower/include/trajectory_follower/debug_values.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp index b8c2ac445568f..2f20f28dd6da9 100644 --- a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp @@ -12,10 +12,8 @@ // 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 "trajectory_follower/visibility_control.hpp" +#ifndef PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ #include @@ -25,11 +23,11 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { /// Debug Values used for debugging or controller tuning -class TRAJECTORY_FOLLOWER_PUBLIC DebugValues +class DebugValues { public: /// Types of debug values @@ -96,9 +94,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues private: std::array(TYPE::SIZE)> m_values; }; -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp similarity index 83% rename from control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 084da2e6f7198..32928d64a12b8 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" @@ -21,7 +21,6 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" -#include "trajectory_follower/visibility_control.hpp" #include // NOLINT @@ -37,7 +36,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { namespace longitudinal_utils { @@ -51,18 +50,18 @@ using geometry_msgs::msg::Quaternion; /** * @brief check if trajectory is invalid or not */ -TRAJECTORY_FOLLOWER_PUBLIC bool isValidTrajectory(const Trajectory & traj); +bool isValidTrajectory(const Trajectory & traj); /** * @brief calculate distance to stopline from current vehicle position where velocity is 0 */ -TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance( +double calcStopDistance( const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw); /** * @brief calculate pitch angle from estimated current pose */ -TRAJECTORY_FOLLOWER_PUBLIC double getPitchByPose(const Quaternion & quaternion); +double getPitchByPose(const Quaternion & quaternion); /** * @brief calculate pitch angle from trajectory on map @@ -71,20 +70,19 @@ TRAJECTORY_FOLLOWER_PUBLIC double getPitchByPose(const Quaternion & quaternion); * @param [in] closest_idx nearest index to current vehicle position * @param [in] wheel_base length of wheel base */ -TRAJECTORY_FOLLOWER_PUBLIC double getPitchByTraj( +double getPitchByTraj( const Trajectory & trajectory, const size_t closest_idx, const double wheel_base); /** * @brief calculate elevation angle */ -TRAJECTORY_FOLLOWER_PUBLIC double calcElevationAngle( - const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); +double 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( +Pose calcPoseAfterTimeDelay( const Pose & current_pose, const double delay_time, const double current_vel); /** @@ -93,8 +91,7 @@ TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay( * @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 double ratio); +Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); /** * @brief apply linear interpolation to trajectory point that is nearest to a certain point @@ -102,7 +99,7 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double * @param [in] point Interpolated point is nearest to this point. */ template -TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( +TrajectoryPoint lerpTrajectoryPoint( const T & points, const Pose & pose, const double max_dist, const double max_yaw) { TrajectoryPoint interpolated_point; @@ -144,7 +141,7 @@ TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( * @param [in] dt time between current and previous one * @param [in] lim_val limitation value for differential */ -TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter( +double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double lim_val); /** @@ -155,13 +152,13 @@ TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter( * @param [in] max_val maximum value for differential * @param [in] min_val minimum value for differential */ -TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter( +double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val); } // namespace longitudinal_utils -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp new file mode 100644 index 0000000000000..6215ee7a15826 --- /dev/null +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.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 PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ + +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace pid_longitudinal_controller +{ + +/** + * @brief Simple filter with gain on the first derivative of the value + */ +class LowpassFilter1d +{ +private: + double m_x; //!< @brief current filtered value + double 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 double x, const double gain) : m_x(x), m_gain(gain) {} + + /** + * @brief set the current value of the filter + * @param [in] x new value + */ + void reset(const double x) { m_x = x; } + + /** + * @brief get the current value of the filter + */ + double getValue() const { return m_x; } + + /** + * @brief filter a new value + * @param [in] u new value + */ + double filter(const double u) + { + const double ret = m_gain * m_x + (1.0 - m_gain) * u; + m_x = ret; + return ret; + } +}; +} // namespace pid_longitudinal_controller +} // namespace control +} // namespace motion +} // namespace autoware +#endif // PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp similarity index 91% rename from control/trajectory_follower/include/trajectory_follower/pid.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp index d9392ba3ced91..b261191075e2c 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp @@ -12,10 +12,8 @@ // 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 "trajectory_follower/visibility_control.hpp" +#ifndef PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__PID_HPP_ #include @@ -25,11 +23,11 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { /// @brief implementation of a PID controller -class TRAJECTORY_FOLLOWER_PUBLIC PIDController +class PIDController { public: PIDController(); @@ -97,9 +95,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC PIDController bool m_is_gains_set; bool m_is_limits_set; }; -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__PID_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp similarity index 91% rename from control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index d37aed07c377b..ce4aa88bc7a10 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ -#define TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#ifndef PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ #include "diagnostic_updater/diagnostic_updater.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" +#include "pid_longitudinal_controller/debug_values.hpp" +#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "pid_longitudinal_controller/lowpass_filter.hpp" +#include "pid_longitudinal_controller/pid.hpp" +#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "trajectory_follower/debug_values.hpp" -#include "trajectory_follower/longitudinal_controller_base.hpp" -#include "trajectory_follower/longitudinal_controller_utils.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/pid.hpp" -#include "trajectory_follower/smooth_stop.hpp" +#include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" @@ -50,14 +50,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; /// \class PidLongitudinalController /// \brief The node class used for generating longitudinal control commands (velocity/acceleration) -class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public LongitudinalControllerBase +class PidLongitudinalController : public trajectory_follower::LongitudinalControllerBase { public: explicit PidLongitudinalController(rclcpp::Node & node); @@ -143,14 +143,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal StateTransitionParams m_state_transition_params; // drive - trajectory_follower::PIDController m_pid_vel; - std::shared_ptr m_lpf_vel_error{nullptr}; + PIDController m_pid_vel; + std::shared_ptr m_lpf_vel_error{nullptr}; double m_current_vel_threshold_pid_integrate; bool m_enable_brake_keeping_before_stop; double m_brake_keeping_acc; // smooth stop - trajectory_follower::SmoothStop m_smooth_stop; + SmoothStop m_smooth_stop; // stop struct StoppedStateParams @@ -180,7 +180,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal // slope compensation bool m_use_traj_for_pitch; - std::shared_ptr m_lpf_pitch{nullptr}; + std::shared_ptr m_lpf_pitch{nullptr}; double m_max_pitch_rad; double m_min_pitch_rad; @@ -203,7 +203,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal std::vector> m_vel_hist; // debug values - trajectory_follower::DebugValues m_debug_values; + DebugValues m_debug_values; std::shared_ptr m_last_running_time{std::make_shared(node_->now())}; @@ -237,12 +237,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal */ void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg); - bool isReady(const InputData & input_data) override; + bool isReady(const trajectory_follower::InputData & input_data) override; /** * @brief compute control command, and publish periodically */ - LongitudinalOutput run(InputData const & input_data) override; + trajectory_follower::LongitudinalOutput run( + trajectory_follower::InputData const & input_data) override; /** * @brief calculate data for controllers whose type is ControlData @@ -378,9 +379,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); }; -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp similarity index 93% rename from control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp index c86f319f1288c..45c042dc026be 100644 --- a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp @@ -12,11 +12,10 @@ // 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_ +#ifndef PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/visibility_control.hpp" #include // NOLINT @@ -32,13 +31,13 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { /** * @brief Smooth stop class to implement vehicle specific deceleration profiles */ -class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop +class SmoothStop { public: /** @@ -117,9 +116,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop rclcpp::Time m_weak_acc_time; bool m_is_set_params = false; }; -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ diff --git a/control/trajectory_follower/design/media/BrakeKeeping.drawio.svg b/control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg similarity index 100% rename from control/trajectory_follower/design/media/BrakeKeeping.drawio.svg rename to control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg diff --git a/control/trajectory_follower/design/media/LongitudinalControllerDiagram.drawio.svg b/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg similarity index 100% rename from control/trajectory_follower/design/media/LongitudinalControllerDiagram.drawio.svg rename to control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg diff --git a/control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg b/control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg similarity index 100% rename from control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg rename to control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml new file mode 100644 index 0000000000000..e3ff39b187d14 --- /dev/null +++ b/control/pid_longitudinal_controller/package.xml @@ -0,0 +1,49 @@ + + + + pid_longitudinal_controller + 1.0.0 + PID-based longitudinal controller + + Takamasa Horibe + Takayuki Murooka + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_geometry + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + diagnostic_msgs + diagnostic_updater + eigen + geometry_msgs + interpolation + motion_utils + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + trajectory_follower_base + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml similarity index 100% rename from control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml rename to control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp new file mode 100644 index 0000000000000..a056acc901d8d --- /dev/null +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -0,0 +1,188 @@ +// 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 "pid_longitudinal_controller/longitudinal_controller_utils.hpp" + +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" + +#include // NOLINT + +#ifdef ROS_DISTRO_GALACTIC +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#else +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#endif + +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace pid_longitudinal_controller +{ +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; +} + +double calcStopDistance( + const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw) +{ + const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points); + + const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; + const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, current_pose, max_dist, max_yaw); + const double signed_length_on_traj = motion_utils::calcSignedArcLength( + traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position, + std::min(end_idx, traj.points.size() - 2)); + + if (std::isnan(signed_length_on_traj)) { + return 0.0; + } + return signed_length_on_traj; +} + +double getPitchByPose(const Quaternion & quaternion_msg) +{ + double roll, pitch, yaw; + tf2::Quaternion quaternion; + tf2::fromMsg(quaternion_msg, quaternion); + tf2::Matrix3x3{quaternion}.getRPY(roll, pitch, yaw); + + return pitch; +} + +double getPitchByTraj( + const Trajectory & trajectory, const size_t nearest_idx, const double 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 double 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 double 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()); +} + +double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) +{ + const double dx = p_from.pose.position.x - p_to.pose.position.x; + const double dy = p_from.pose.position.y - p_to.pose.position.y; + const double dz = p_from.pose.position.z - p_to.pose.position.z; + + const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits::epsilon()); + const double pitch = std::atan2(dz, dxy); + + return pitch; +} + +Pose calcPoseAfterTimeDelay( + const Pose & current_pose, const double delay_time, const double current_vel) +{ + // simple linear prediction + const double yaw = tf2::getYaw(current_pose.orientation); + const double running_distance = delay_time * current_vel; + const double dx = running_distance * std::cos(yaw); + const double dy = running_distance * std::sin(yaw); + + auto pred_pose = current_pose; + pred_pose.position.x += dx; + pred_pose.position.y += dy; + return pred_pose; +} + +double lerp(const double v_from, const double v_to, const double ratio) +{ + return v_from + (v_to - v_from) * ratio; +} + +Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double 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); +} + +double applyDiffLimitFilter( + const double input_val, const double prev_val, const double dt, const double max_val, + const double min_val) +{ + const double diff_raw = (input_val - prev_val) / dt; + const double diff = std::min(std::max(diff_raw, min_val), max_val); + const double filtered_val = prev_val + diff * dt; + return filtered_val; +} + +double applyDiffLimitFilter( + const double input_val, const double prev_val, const double dt, const double lim_val) +{ + const double max_val = std::fabs(lim_val); + const double min_val = -max_val; + return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); +} +} // namespace longitudinal_utils +} // namespace pid_longitudinal_controller +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp similarity index 95% rename from control/trajectory_follower/src/pid.cpp rename to control/pid_longitudinal_controller/src/pid.cpp index a44a3d4694729..b08bb78455a10 100644 --- a/control/trajectory_follower/src/pid.cpp +++ b/control/pid_longitudinal_controller/src/pid.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/pid.hpp" +#include "pid_longitudinal_controller/pid.hpp" #include #include @@ -26,7 +26,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { PIDController::PIDController() : m_error_integral(0.0), @@ -108,7 +108,7 @@ void PIDController::reset() m_prev_error = 0.0; m_is_first_time = true; } -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp similarity index 94% rename from control/trajectory_follower/src/pid_longitudinal_controller.cpp rename to control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 77af30e33bfbb..5f8031c425cfd 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/pid_longitudinal_controller.hpp" +#include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "motion_utils/motion_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -30,7 +30,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node_{&node}, diagnostic_updater_(&node) @@ -103,7 +103,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // set lowpass filter for vel error and pitch const double lpf_vel_error_gain{node_->declare_parameter("lpf_vel_error_gain")}; m_lpf_vel_error = - std::make_shared(0.0, lpf_vel_error_gain); + std::make_shared(0.0, lpf_vel_error_gain); m_current_vel_threshold_pid_integrate = node_->declare_parameter("current_vel_threshold_pid_integration"); // [m/s] @@ -171,7 +171,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters for slope compensation m_use_traj_for_pitch = node_->declare_parameter("use_trajectory_for_pitch_calculation"); const double lpf_pitch_gain{node_->declare_parameter("lpf_pitch_gain")}; - m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); + m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] @@ -213,7 +213,7 @@ void PidLongitudinalController::setCurrentAcceleration( void PidLongitudinalController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & msg) { - if (!trajectory_follower::longitudinal_utils::isValidTrajectory(msg)) { + if (!pid_longitudinal_controller::longitudinal_utils::isValidTrajectory(msg)) { RCLCPP_ERROR_THROTTLE( node_->get_logger(), *node_->get_clock(), 3000, "received invalid trajectory. ignore."); return; @@ -351,12 +351,14 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac return result; } -bool PidLongitudinalController::isReady([[maybe_unused]] const InputData & input_data) +bool PidLongitudinalController::isReady( + [[maybe_unused]] const trajectory_follower::InputData & input_data) { return true; } -LongitudinalOutput PidLongitudinalController::run(InputData const & input_data) +trajectory_follower::LongitudinalOutput PidLongitudinalController::run( + trajectory_follower::InputData const & input_data) { // set input data setTrajectory(input_data.current_trajectory); @@ -378,7 +380,7 @@ LongitudinalOutput PidLongitudinalController::run(InputData const & input_data) const auto cmd_msg = createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command publishDebugData(raw_ctrl_cmd, control_data); // publish debug data - LongitudinalOutput output; + trajectory_follower::LongitudinalOutput output; output.control_cmd = cmd_msg; return output; } @@ -391,7 +393,7 @@ LongitudinalOutput PidLongitudinalController::run(InputData const & input_data) // publish control command const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel); - LongitudinalOutput output; + trajectory_follower::LongitudinalOutput output; output.control_cmd = cmd_msg; // publish debug data @@ -445,13 +447,13 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData m_prev_shift = control_data.shift; // distance to stopline - control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( + control_data.stop_dist = pid_longitudinal_controller::longitudinal_utils::calcStopDistance( current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch const double raw_pitch = - trajectory_follower::longitudinal_utils::getPitchByPose(current_pose.orientation); - const double traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj( + pid_longitudinal_controller::longitudinal_utils::getPitchByPose(current_pose.orientation); + const double traj_pitch = pid_longitudinal_controller::longitudinal_utils::getPitchByTraj( m_trajectory, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); @@ -464,9 +466,9 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm { // These accelerations are without slope compensation const auto & p = m_emergency_state_params; - const double vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + const double vel = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); - const double acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + const double acc = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); RCLCPP_ERROR_THROTTLE( @@ -620,8 +622,9 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( Motion raw_ctrl_cmd{}; Motion target_motion{}; if (m_control_state == ControlState::DRIVE) { - const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( - current_pose, m_delay_compensation_time, current_vel); + const auto target_pose = + pid_longitudinal_controller::longitudinal_utils::calcPoseAfterTimeDelay( + current_pose, m_delay_compensation_time, current_vel); const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, @@ -632,7 +635,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const double pred_vel_in_target = predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); m_debug_values.setValues( - trajectory_follower::DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); + pid_longitudinal_controller::DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); raw_ctrl_cmd.vel = target_motion.vel; raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); @@ -654,7 +657,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( // This acceleration is without slope compensation const auto & p = m_stopped_state_params; raw_ctrl_cmd.vel = p.vel; - raw_ctrl_cmd.acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + raw_ctrl_cmd.acc = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); RCLCPP_DEBUG( @@ -700,7 +703,7 @@ autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController:: void PidLongitudinalController::publishDebugData( const Motion & ctrl_cmd, const ControlData & control_data) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; // set debug values m_debug_values.setValues(DebugValues::TYPE::DT, control_data.dt); m_debug_values.setValues(DebugValues::TYPE::CALCULATED_ACC, control_data.current_motion.acc); @@ -759,7 +762,7 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift double PidLongitudinalController::calcFilteredAcc( const double raw_acc, const ControlData & control_data) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; const double acc_max_filtered = std::clamp(raw_acc, m_min_acc, m_max_acc); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered); @@ -771,8 +774,9 @@ double PidLongitudinalController::calcFilteredAcc( m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); // This jerk filter must be applied after slope compensation - const double acc_jerk_filtered = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( - acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); + const double acc_jerk_filtered = + pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( + acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); return acc_jerk_filtered; @@ -860,7 +864,7 @@ PidLongitudinalController::calcInterpolatedTargetValue( } // apply linear interpolation - return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint( + return pid_longitudinal_controller::longitudinal_utils::lerpTrajectoryPoint( traj.points, pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); } @@ -915,7 +919,7 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( double PidLongitudinalController::applyVelocityFeedback( const Motion target_motion, const double dt, const double current_vel) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; const double current_vel_abs = std::fabs(current_vel); const double target_vel_abs = std::fabs(target_motion.vel); const bool enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate); @@ -941,7 +945,7 @@ double PidLongitudinalController::applyVelocityFeedback( void PidLongitudinalController::updatePitchDebugValues( const double pitch, const double traj_pitch, const double raw_pitch) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; const double to_degrees = (180.0 / static_cast(M_PI)); m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); @@ -955,7 +959,7 @@ void PidLongitudinalController::updateDebugVelAcc( const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; const double current_vel = control_data.current_motion.vel; const auto interpolated_point = calcInterpolatedTargetValue(m_trajectory, current_pose); @@ -1008,7 +1012,7 @@ void PidLongitudinalController::checkControlState( stat.summary(level, msg); } -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/pid_longitudinal_controller/src/smooth_stop.cpp b/control/pid_longitudinal_controller/src/smooth_stop.cpp new file mode 100644 index 0000000000000..db8d81b837012 --- /dev/null +++ b/control/pid_longitudinal_controller/src/smooth_stop.cpp @@ -0,0 +1,170 @@ +// 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 "pid_longitudinal_controller/smooth_stop.hpp" + +#include // NOLINT + +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace pid_longitudinal_controller +{ +void SmoothStop::init(const double pred_vel_in_target, const double 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( + double max_strong_acc, double min_strong_acc, double weak_acc, double weak_stop_acc, + double strong_stop_acc, double min_fast_vel, double min_running_vel, double min_running_acc, + double weak_stop_time, double weak_stop_dist, double 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 double 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(); + double mean_t = 0.0; + double mean_v = 0.0; + double sum_tv = 0.0; + double sum_tt = 0.0; + for (const auto & vel : vel_hist) { + const double t = (vel.first - current_ros_time).seconds(); + const double 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 double a = + (vel_hist_size * mean_t * mean_v - sum_tv) / (vel_hist_size * mean_t * mean_t - sum_tt); + const double 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 double time_to_stop = -b / a; + if (time_to_stop > 0) { + return time_to_stop; + } + + return {}; +} + +double SmoothStop::calculate( + const double stop_dist, const double current_vel, const double current_acc, + const std::vector> & vel_hist, const double 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 bool is_fast_vel = std::abs(current_vel) > m_params.min_fast_vel; + const bool 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 pid_longitudinal_controller +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp similarity index 99% rename from control/trajectory_follower/test/test_longitudinal_controller_utils.cpp rename to control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 6bd8403b2b514..ef38533376446 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" +#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.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" @@ -30,7 +30,8 @@ #include -namespace longitudinal_utils = ::autoware::motion::control::trajectory_follower::longitudinal_utils; +namespace longitudinal_utils = + ::autoware::motion::control::pid_longitudinal_controller::longitudinal_utils; TEST(TestLongitudinalControllerUtils, isValidTrajectory) { diff --git a/control/trajectory_follower/test/test_pid.cpp b/control/pid_longitudinal_controller/test/test_pid.cpp similarity index 95% rename from control/trajectory_follower/test/test_pid.cpp rename to control/pid_longitudinal_controller/test/test_pid.cpp index 8c7db8f6af255..82d01e0028a9c 100644 --- a/control/trajectory_follower/test/test_pid.cpp +++ b/control/pid_longitudinal_controller/test/test_pid.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/pid.hpp" +#include "pid_longitudinal_controller/pid.hpp" #include TEST(TestPID, calculate_pid_output) { - using ::autoware::motion::control::trajectory_follower::PIDController; + using ::autoware::motion::control::pid_longitudinal_controller::PIDController; std::vector contributions; const double dt = 1.0; double target = 10.0; diff --git a/control/trajectory_follower/test/test_smooth_stop.cpp b/control/pid_longitudinal_controller/test/test_smooth_stop.cpp similarity index 96% rename from control/trajectory_follower/test/test_smooth_stop.cpp rename to control/pid_longitudinal_controller/test/test_smooth_stop.cpp index d03ee02c7d109..d658a5271d249 100644 --- a/control/trajectory_follower/test/test_smooth_stop.cpp +++ b/control/pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -13,15 +13,15 @@ // limitations under the License. #include "gtest/gtest.h" +#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/smooth_stop.hpp" #include #include TEST(TestSmoothStop, calculate_stopping_acceleration) { - using ::autoware::motion::control::trajectory_follower::SmoothStop; + using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop; using rclcpp::Duration; using rclcpp::Time; diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index 6a5ea9d17b4c9..c0ca24bfcfb28 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -36,7 +36,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/ros/self_pose_listener.hpp" -#include "trajectory_follower/lateral_controller_base.hpp" +#include "trajectory_follower_base/lateral_controller_base.hpp" #include #include diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 222d75aa833bf..d2b3ed292c7c6 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -31,7 +31,7 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - trajectory_follower + trajectory_follower_base vehicle_info_util visualization_msgs diff --git a/control/trajectory_follower/CMakeLists.txt b/control/trajectory_follower/CMakeLists.txt deleted file mode 100644 index 52e46781f0ab7..0000000000000 --- a/control/trajectory_follower/CMakeLists.txt +++ /dev/null @@ -1,98 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(trajectory_follower) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# lateral_controller -set(LATERAL_CONTROLLER_LIB lateral_controller_lib) -set(LATERAL_CONTROLLER_LIB_SRC - src/mpc_lateral_controller.cpp - 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/lateral_controller_base.hpp - include/trajectory_follower/mpc_lateral_controller.hpp - include/trajectory_follower/sync_data.hpp - include/trajectory_follower/input_data.hpp - 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 -) - -ament_auto_add_library(${LATERAL_CONTROLLER_LIB} SHARED - ${LATERAL_CONTROLLER_LIB_SRC} - ${LATERAL_CONTROLLER_LIB_HEADERS} -) -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_longitudinal_controller.cpp - src/pid.cpp - src/smooth_stop.cpp - src/longitudinal_controller_utils.cpp -) - -set(LONGITUDINAL_CONTROLLER_LIB_HEADERS - include/trajectory_follower/longitudinal_controller_base.hpp - include/trajectory_follower/pid_longitudinal_controller.hpp - include/trajectory_follower/sync_data.hpp - include/trajectory_follower/input_data.hpp - include/trajectory_follower/debug_values.hpp - include/trajectory_follower/pid.hpp - include/trajectory_follower/smooth_stop.hpp - include/trajectory_follower/longitudinal_controller_utils.hpp -) - -ament_auto_add_library(${LONGITUDINAL_CONTROLLER_LIB} SHARED - ${LONGITUDINAL_CONTROLLER_LIB_SRC} - ${LONGITUDINAL_CONTROLLER_LIB_HEADERS} -) -target_compile_options(${LONGITUDINAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-cast) - -if(BUILD_TESTING) - set(TEST_LAT_SOURCES - test/test_mpc.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_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) - target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${LATERAL_CONTROLLER_LIB}) - - set(TEST_LON_SOURCES - 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_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) - target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${LONGITUDINAL_CONTROLLER_LIB}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/control/trajectory_follower_base/CMakeLists.txt b/control/trajectory_follower_base/CMakeLists.txt new file mode 100644 index 0000000000000..98f86e468e819 --- /dev/null +++ b/control/trajectory_follower_base/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(trajectory_follower_base) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/longitudinal_controller_base.cpp + src/lateral_controller_base.cpp +) + +ament_auto_package() diff --git a/control/trajectory_follower/design/trajectory_follower-design.md b/control/trajectory_follower_base/README.md similarity index 69% rename from control/trajectory_follower/design/trajectory_follower-design.md rename to control/trajectory_follower_base/README.md index 46358abe91090..6bd3d74e75271 100644 --- a/control/trajectory_follower/design/trajectory_follower-design.md +++ b/control/trajectory_follower_base/README.md @@ -8,29 +8,22 @@ This is the design document for the `trajectory_follower` package. -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. - - [trajectory_follower-mpc-design](mpc_lateral_controller-design.md) -- PID control for the computation of velocity and acceleration commands. - - [trajectory_follower-pid-design](pid_longitudinal_controller-design.md) +This package provides the interface of longitudinal and lateral controllers used by the node of the `trajectory_follower_node` package. +We can implement a detailed controller by deriving the longitudinal and lateral base interfaces. ## Design -![Controller](../../trajectory_follower_nodes/design/media/Controller.drawio.svg) - There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement. The interface class has the following base functions. -- `setInputData()`: Input the data subscribed in [Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md). This must be implemented with the inherited algorithm and the used data must be selected. -- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md). This must be implemented by inherited algorithms. -- `syncData()`: Input the result of running the other controller. +- `isReady()`: Check if the control is ready to compute. +- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../trajectory_follower_node/README.md). This must be implemented by inherited algorithms. +- `sync()`: Input the result of running the other controller. - steer angle convergence - allow keeping stopped until steer is converged. - velocity convergence(currently not used) -See [the Design of Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md#Design) for how these functions work in the node. +See [the Design of Trajectory Follower Nodes](../trajectory_follower_node/README.md#Design) for how these functions work in the node. ## Separated lateral (steering) and longitudinal (velocity) controls diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp similarity index 89% rename from control/trajectory_follower/include/trajectory_follower/input_data.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp index 6eab1823a5915..b135f5034d103 100644 --- a/control/trajectory_follower/include/trajectory_follower/input_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ -#define TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" @@ -40,4 +40,4 @@ struct InputData } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp similarity index 80% rename from control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp index 540fab950cebe..28eddd175e38e 100644 --- a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/input_data.hpp" -#include "trajectory_follower/sync_data.hpp" +#include "trajectory_follower_base/input_data.hpp" +#include "trajectory_follower_base/sync_data.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" @@ -42,10 +42,7 @@ class LateralControllerBase public: virtual bool isReady(const InputData & input_data) = 0; virtual LateralOutput run(InputData const & input_data) = 0; - void sync(LongitudinalSyncData const & longitudinal_sync_data) - { - longitudinal_sync_data_ = longitudinal_sync_data; - }; + void sync(LongitudinalSyncData const & longitudinal_sync_data); protected: LongitudinalSyncData longitudinal_sync_data_; @@ -56,4 +53,4 @@ class LateralControllerBase } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp similarity index 75% rename from control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp index c6f578ebdaef7..5da298fe32b12 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/input_data.hpp" -#include "trajectory_follower/sync_data.hpp" +#include "trajectory_follower_base/input_data.hpp" +#include "trajectory_follower_base/sync_data.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" @@ -41,10 +41,10 @@ class LongitudinalControllerBase public: virtual bool isReady(const InputData & input_data) = 0; virtual LongitudinalOutput run(InputData const & input_data) = 0; - void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; } + void sync(LateralSyncData const & lateral_sync_data); // NOTE: This reset function should be called when the trajectory is replaned by changing ego pose // or goal pose. - void reset() { lateral_sync_data_.is_steer_converged = false; } + void reset(); protected: LateralSyncData lateral_sync_data_; @@ -55,4 +55,4 @@ class LongitudinalControllerBase } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp similarity index 86% rename from control/trajectory_follower/include/trajectory_follower/sync_data.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp index 7acb37aa69808..4735fc4a3e842 100644 --- a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ -#define TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ namespace autoware { @@ -39,4 +39,4 @@ struct LongitudinalSyncData } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp similarity index 89% rename from control/trajectory_follower/include/trajectory_follower/visibility_control.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp index b501ec9a55264..c2419427ac961 100644 --- a/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // TRAJECTORY_FOLLOWER__VISIBILITY_CONTROL_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower_base/package.xml similarity index 97% rename from control/trajectory_follower/package.xml rename to control/trajectory_follower_base/package.xml index b6de5384180ec..7482cbf190be3 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -1,7 +1,7 @@ - trajectory_follower + trajectory_follower_base 1.0.0 Library for generating lateral and longitudinal controls following a trajectory diff --git a/control/trajectory_follower_base/src/lateral_controller_base.cpp b/control/trajectory_follower_base/src/lateral_controller_base.cpp new file mode 100644 index 0000000000000..1fe2f77d8b074 --- /dev/null +++ b/control/trajectory_follower_base/src/lateral_controller_base.cpp @@ -0,0 +1,33 @@ +// Copyright 2022 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_base/lateral_controller_base.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +void LateralControllerBase::sync(LongitudinalSyncData const & longitudinal_sync_data) +{ + longitudinal_sync_data_ = longitudinal_sync_data; +} + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp new file mode 100644 index 0000000000000..70fe47ee9d54a --- /dev/null +++ b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp @@ -0,0 +1,33 @@ +// Copyright 2022 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_base/longitudinal_controller_base.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +void LongitudinalControllerBase::sync(LateralSyncData const & lateral_sync_data) +{ + lateral_sync_data_ = lateral_sync_data; +} +void LongitudinalControllerBase::reset() { lateral_sync_data_.is_steer_converged = false; } +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_node/CMakeLists.txt similarity index 87% rename from control/trajectory_follower_nodes/CMakeLists.txt rename to control/trajectory_follower_node/CMakeLists.txt index b40bd8f16a626..07bc45c2933c8 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_node/CMakeLists.txt @@ -1,25 +1,25 @@ cmake_minimum_required(VERSION 3.14) -project(trajectory_follower_nodes) +project(trajectory_follower_node) find_package(autoware_cmake REQUIRED) autoware_package() set(CONTROLLER_NODE controller_node) ament_auto_add_library(${CONTROLLER_NODE} SHARED - include/trajectory_follower_nodes/controller_node.hpp + include/trajectory_follower_node/controller_node.hpp src/controller_node.cpp ) target_compile_options(${CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) rclcpp_components_register_node(${CONTROLLER_NODE} - PLUGIN "autoware::motion::control::trajectory_follower_nodes::Controller" + PLUGIN "autoware::motion::control::trajectory_follower_node::Controller" EXECUTABLE ${CONTROLLER_NODE}_exe ) # simple trajectory follower set(SIMPLE_TRAJECTORY_FOLLOWER_NODE simple_trajectory_follower) ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED - include/trajectory_follower_nodes/simple_trajectory_follower.hpp + include/trajectory_follower_node/simple_trajectory_follower.hpp src/simple_trajectory_follower.cpp ) @@ -32,7 +32,7 @@ rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} ) if(BUILD_TESTING) - set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_nodes) + set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_node) ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} test/trajectory_follower_test_utils.hpp test/test_controller_node.cpp diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md new file mode 100644 index 0000000000000..a98804f12d796 --- /dev/null +++ b/control/trajectory_follower_node/README.md @@ -0,0 +1,156 @@ +# Trajectory Follower Nodes + +## Purpose + +Generate control commands to follow a given Trajectory. + +## Design + +This is a node of the functionalities implemented in the controller class derived from [trajectory_follower_base](../trajectory_follower_base/README.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. + +By default, the controller instance with the `Controller` class as follows is used. + +```plantuml +@startuml +package trajectory_follower_base { +abstract class LateralControllerBase { +longitudinal_sync_data_ + + virtual isReady(InputData) + virtual run(InputData) + sync(LongitudinalSyncData) + reset() + +} +abstract class LongitudinalControllerBase { +lateral_sync_data_ + + virtual isReady(InputData) + virtual run(InputData) + sync(LateralSyncData) + reset() + +} + +struct InputData { +trajectory +odometry +steering +accel +} +struct LongitudinalSyncData { +is_steer_converged +} +struct LateralSyncData { +} +} + +package mpc_lateral_controller { +class MPCLateralController { +isReady(InputData) override +run(InputData) override +} +} +package pure_pursuit { +class PurePursuitLateralController { +isReady(InputData) override +run(InputData) override +} +} +package pid_longitudinal_controller { +class PIDLongitudinalController { +isReady(InputData) override +run(InputData) override +} +} + +package trajectory_follower_node { +class Controller { +longitudinal_controller_ +lateral_controller_ +onTimer() +createInputData(): InputData +} +} + +MPCLateralController --|> LateralControllerBase +PurePursuitLateralController --|> LateralControllerBase +PIDLongitudinalController --|> LongitudinalControllerBase + +LateralSyncData --> LongitudinalControllerBase +LateralSyncData --> LateralControllerBase +LongitudinalSyncData --> LongitudinalControllerBase +LongitudinalSyncData --> LateralControllerBase +InputData ..> LateralControllerBase +InputData ..> LongitudinalControllerBase + +LateralControllerBase --o Controller +LongitudinalControllerBase --o Controller +InputData ..> Controller +@enduml +``` + +The process flow of `Controller` class is as follows. + +```cpp +// 1. create input data +const auto input_data = createInputData(*get_clock()); +if (!input_data) { + return; +} + +// 2. check if controllers are ready +const bool is_lat_ready = lateral_controller_->isReady(*input_data); +const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); +if (!is_lat_ready || !is_lon_ready) { + return; +} + +// 3. run controllers +const auto lat_out = lateral_controller_->run(*input_data); +const auto lon_out = longitudinal_controller_->run(*input_data); + +// 4. sync with each other controllers +longitudinal_controller_->sync(lat_out.sync_data); +lateral_controller_->sync(lon_out.sync_data); + +// 5. publish control command +control_cmd_pub_->publish(out); +``` + +Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` + +- lateral controller + - `keep_steer_control_until_converged` +- longitudinal controller + - `enable_keep_stopped_until_steer_convergence` + +### Inputs / Outputs / API + +#### Inputs + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_auto_vehicle_msgs/SteeringReport` current steering + +#### Outputs + +- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. + +#### Parameter + +- `ctrl_period`: control commands publishing period +- `timeout_thr_sec`: duration in second after which input messages are discarded. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. + 1. Both commands have been received. + 2. The last received commands are not older than defined by `timeout_thr_sec`. +- `lateral_controller_mode`: `mpc_follower` or `pure_pursuit` + - (currently there is only `PID` for longitudinal controller) + +## Debugging + +Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. + +A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. + +In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. diff --git a/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml b/control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml similarity index 100% rename from control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml rename to control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml diff --git a/control/trajectory_follower_nodes/design/media/Controller.drawio.svg b/control/trajectory_follower_node/design/media/Controller.drawio.svg similarity index 99% rename from control/trajectory_follower_nodes/design/media/Controller.drawio.svg rename to control/trajectory_follower_node/design/media/Controller.drawio.svg index 7da747243d061..1152ef2b17f59 100644 --- a/control/trajectory_follower_nodes/design/media/Controller.drawio.svg +++ b/control/trajectory_follower_node/design/media/Controller.drawio.svg @@ -83,12 +83,12 @@
- trajectory_follower_nodes/controller_node + trajectory_follower_node/controller_node
- trajectory_follower_nodes/controller_node + trajectory_follower_node/controller_node diff --git a/control/trajectory_follower_nodes/design/simple_trajectory_follower-design.md b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md similarity index 100% rename from control/trajectory_follower_nodes/design/simple_trajectory_follower-design.md rename to control/trajectory_follower_node/design/simple_trajectory_follower-design.md diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp similarity index 87% rename from control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp rename to control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 62bf01b75eb44..8d46732679af8 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ +#ifndef TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ +#define TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" @@ -21,13 +21,9 @@ #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "trajectory_follower/debug_values.hpp" -#include "trajectory_follower/lateral_controller_base.hpp" -#include "trajectory_follower/longitudinal_controller_base.hpp" -#include "trajectory_follower/longitudinal_controller_utils.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/pid.hpp" -#include "trajectory_follower/smooth_stop.hpp" +#include "trajectory_follower_base/lateral_controller_base.hpp" +#include "trajectory_follower_base/longitudinal_controller_base.hpp" +#include "trajectory_follower_node/visibility_control.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" @@ -53,7 +49,7 @@ namespace control { using trajectory_follower::LateralOutput; using trajectory_follower::LongitudinalOutput; -namespace trajectory_follower_nodes +namespace trajectory_follower_node { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -114,9 +110,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node const trajectory_follower::InputData & input_data, const trajectory_follower::LateralOutput & lat_out) const; }; -} // namespace trajectory_follower_nodes +} // namespace trajectory_follower_node } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ +#endif // TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp similarity index 91% rename from control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp rename to control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp index a862477f309ab..e748bdf25d419 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#ifndef TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#define TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ #include @@ -63,4 +63,4 @@ class SimpleTrajectoryFollower : public rclcpp::Node } // namespace simple_trajectory_follower -#endif // TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#endif // TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp similarity index 89% rename from control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp rename to control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp index e4f75f3e456b6..36446c4144a0a 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ +#ifndef TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ +#define TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ +#endif // TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml b/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml similarity index 84% rename from control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml rename to control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml index f74fa994086b2..8ce799e17e632 100644 --- a/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml +++ b/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_node/package.xml similarity index 90% rename from control/trajectory_follower_nodes/package.xml rename to control/trajectory_follower_node/package.xml index de184228e5f87..7a32ac96d1555 100644 --- a/control/trajectory_follower_nodes/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -1,7 +1,7 @@ - trajectory_follower_nodes + trajectory_follower_node 1.0.0 Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands @@ -25,10 +25,12 @@ autoware_auto_system_msgs autoware_auto_vehicle_msgs motion_utils + mpc_lateral_controller + pid_longitudinal_controller pure_pursuit rclcpp rclcpp_components - trajectory_follower + trajectory_follower_base vehicle_info_util ros2launch diff --git a/control/trajectory_follower_node/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_node/param/lateral_controller_defaults.param.yaml new file mode 100644 index 0000000000000..92d15da9e49e6 --- /dev/null +++ b/control/trajectory_follower_node/param/lateral_controller_defaults.param.yaml @@ -0,0 +1,73 @@ +/**: + ros__parameters: + # -- system -- + traj_resample_dist: 0.1 # path resampling interval [m] + use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) + admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value + admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + + # -- path smoothing -- + enable_path_smoothing: false # flag for path smoothing + path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing + curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + + # -- trajectory extending -- + extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control + + # -- mpc optimization -- + qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) + mpc_prediction_horizon: 50 # prediction horizon step + mpc_prediction_dt: 0.1 # prediction horizon period [s] + mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q + mpc_weight_heading_error: 0.0 # heading error weight in matrix Q + mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q + mpc_weight_steering_input: 1.0 # steering error weight in matrix R + mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R + mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R + mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R + mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R + mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point + mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point + mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability + mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability + mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero + mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration + mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length + + # -- vehicle model -- + vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics + input_delay: 0.24 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] + acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] + + # -- lowpass filter for noise reduction -- + steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] + error_deriv_lpf_cutoff_hz: 5.0 + + # stop state: steering command is kept in the previous value in the stop state. + stop_state_entry_ego_speed: 0.001 + stop_state_entry_target_speed: 0.001 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: true + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 + + # vehicle parameters + mass_kg: 2400.0 + mass_fl: 600.0 + mass_fr: 600.0 + mass_rl: 600.0 + mass_rr: 600.0 + cf: 155494.663 + cr: 155494.663 diff --git a/control/trajectory_follower_node/param/longitudinal_controller_defaults.param.yaml b/control/trajectory_follower_node/param/longitudinal_controller_defaults.param.yaml new file mode 100644 index 0000000000000..eb2ef443c4576 --- /dev/null +++ b/control/trajectory_follower_node/param/longitudinal_controller_defaults.param.yaml @@ -0,0 +1,74 @@ +/**: + ros__parameters: + delay_compensation_time: 0.17 + + enable_smooth_stop: true + enable_overshoot_emergency: true + enable_large_tracking_error_emergency: true + enable_slope_compensation: false + enable_keep_stopped_until_steer_convergence: true + + # state transition + drive_state_stop_dist: 0.5 + drive_state_offset_stop_dist: 1.0 + stopping_state_stop_dist: 0.49 + stopped_state_entry_duration_time: 0.1 + stopped_state_entry_vel: 0.1 + stopped_state_entry_acc: 0.1 + emergency_state_overshoot_stop_dist: 1.5 + emergency_state_traj_trans_dev: 3.0 + emergency_state_traj_rot_dev: 0.7 + + # drive state + kp: 1.0 + ki: 0.1 + kd: 0.0 + max_out: 1.0 + min_out: -1.0 + max_p_effort: 1.0 + min_p_effort: -1.0 + max_i_effort: 0.3 + min_i_effort: -0.3 + max_d_effort: 0.0 + min_d_effort: 0.0 + lpf_vel_error_gain: 0.9 + current_vel_threshold_pid_integration: 0.5 + enable_brake_keeping_before_stop: false + brake_keeping_acc: -0.2 + + # smooth stop state + smooth_stop_max_strong_acc: -0.5 + smooth_stop_min_strong_acc: -1.0 + smooth_stop_weak_acc: -0.3 + smooth_stop_weak_stop_acc: -0.8 + smooth_stop_strong_stop_acc: -3.4 + smooth_stop_max_fast_vel: 0.5 + smooth_stop_min_running_vel: 0.01 + smooth_stop_min_running_acc: 0.01 + smooth_stop_weak_stop_time: 0.8 + smooth_stop_weak_stop_dist: -0.3 + smooth_stop_strong_stop_dist: -0.5 + + # stopped state + stopped_vel: 0.0 + stopped_acc: -3.4 + stopped_jerk: -5.0 + + # emergency state + emergency_vel: 0.0 + emergency_acc: -5.0 + emergency_jerk: -3.0 + + # acceleration limit + max_acc: 3.0 + min_acc: -5.0 + + # jerk limit + max_jerk: 2.0 + min_jerk: -5.0 + + # pitch + use_trajectory_for_pitch_calculation: false + lpf_pitch_gain: 0.95 + max_pitch_rad: 0.1 + min_pitch_rad: -0.1 diff --git a/control/trajectory_follower_nodes/param/test_nearest_search.param.yaml b/control/trajectory_follower_node/param/test_nearest_search.param.yaml similarity index 100% rename from control/trajectory_follower_nodes/param/test_nearest_search.param.yaml rename to control/trajectory_follower_node/param/test_nearest_search.param.yaml diff --git a/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml b/control/trajectory_follower_node/param/test_vehicle_info.param.yaml similarity index 100% rename from control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml rename to control/trajectory_follower_node/param/test_vehicle_info.param.yaml diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp similarity index 93% rename from control/trajectory_follower_nodes/src/controller_node.cpp rename to control/trajectory_follower_node/src/controller_node.cpp index 0436d0feca0d7..b8a2aa03864bd 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_nodes/controller_node.hpp" +#include "trajectory_follower_node/controller_node.hpp" +#include "mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "pure_pursuit/pure_pursuit_lateral_controller.hpp" -#include "trajectory_follower/mpc_lateral_controller.hpp" -#include "trajectory_follower/pid_longitudinal_controller.hpp" #include #include @@ -31,7 +31,7 @@ namespace motion { namespace control { -namespace trajectory_follower_nodes +namespace trajectory_follower_node { Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) { @@ -41,10 +41,10 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control timeout_thr_sec_ = declare_parameter("timeout_thr_sec", 0.5); const auto lateral_controller_mode = - getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc_follower")); + getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc")); switch (lateral_controller_mode) { case LateralControllerMode::MPC: { - lateral_controller_ = std::make_shared(*this); + lateral_controller_ = std::make_shared(*this); break; } case LateralControllerMode::PURE_PURSUIT: { @@ -60,7 +60,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control switch (longitudinal_controller_mode) { case LongitudinalControllerMode::PID: { longitudinal_controller_ = - std::make_shared(*this); + std::make_shared(*this); break; } default: @@ -92,7 +92,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control Controller::LateralControllerMode Controller::getLateralControllerMode( const std::string & controller_mode) const { - if (controller_mode == "mpc_follower") return LateralControllerMode::MPC; + if (controller_mode == "mpc") return LateralControllerMode::MPC; if (controller_mode == "pure_pursuit") return LateralControllerMode::PURE_PURSUIT; return LateralControllerMode::INVALID; @@ -248,10 +248,10 @@ void Controller::publishDebugMarker( debug_marker_pub_->publish(debug_marker_array); } -} // namespace trajectory_follower_nodes +} // namespace trajectory_follower_node } // namespace control } // namespace motion } // namespace autoware #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_nodes::Controller) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_node::Controller) diff --git a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp similarity index 98% rename from control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp rename to control/trajectory_follower_node/src/simple_trajectory_follower.cpp index ecaa12398a68e..0af901be16125 100644 --- a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_nodes/simple_trajectory_follower.hpp" +#include "trajectory_follower_node/simple_trajectory_follower.hpp" #include #include diff --git a/control/trajectory_follower_nodes/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp similarity index 96% rename from control/trajectory_follower_nodes/test/test_controller_node.cpp rename to control/trajectory_follower_node/test/test_controller_node.cpp index 205c4ea9e8c2d..306b971adb22e 100644 --- a/control/trajectory_follower_nodes/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -17,7 +17,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" -#include "trajectory_follower_nodes/controller_node.hpp" +#include "trajectory_follower_node/controller_node.hpp" #include "trajectory_follower_test_utils.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" @@ -33,7 +33,7 @@ #include #include -using Controller = autoware::motion::control::trajectory_follower_nodes::Controller; +using Controller = autoware::motion::control::trajectory_follower_node::Controller; using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -48,7 +48,11 @@ const rclcpp::Duration one_second(1, 0); rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_convergence = false) { // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); + const auto longitudinal_share_dir = + ament_index_cpp::get_package_share_directory("pid_longitudinal_controller"); + const auto lateral_share_dir = + ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); rclcpp::NodeOptions node_options; node_options.append_parameter_override("ctrl_period", 0.03); node_options.append_parameter_override("timeout_thr_sec", 0.5); @@ -56,9 +60,10 @@ rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_c "enable_keep_stopped_until_steer_convergence", enable_keep_stopped_until_steer_convergence); // longitudinal node_options.arguments( - {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", - "--params-file", share_dir + "/param/longitudinal_controller_defaults.param.yaml", - "--params-file", share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", + {"--ros-args", "--params-file", + lateral_share_dir + "/param/lateral_controller_defaults.param.yaml", "--params-file", + longitudinal_share_dir + "/param/longitudinal_controller_defaults.param.yaml", "--params-file", + share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", share_dir + "/param/test_nearest_search.param.yaml"}); return node_options; diff --git a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp b/control/trajectory_follower_node/test/test_lateral_controller_node.cpp similarity index 99% rename from control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp rename to control/trajectory_follower_node/test/test_lateral_controller_node.cpp index 2109de2ece069..0c0a1f84ab1c2 100644 --- a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_lateral_controller_node.cpp @@ -19,7 +19,7 @@ #include "rclcpp/time.hpp" #include "trajectory_follower_test_utils.hpp" -#include +#include #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -33,7 +33,7 @@ #include #include -using LateralController = autoware::motion::control::trajectory_follower_nodes::LateralController; +using LateralController = autoware::motion::control::trajectory_follower_node::LateralController; using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -47,7 +47,7 @@ const rclcpp::Duration one_second(1, 0); std::shared_ptr makeLateralNode() { // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); rclcpp::NodeOptions node_options; node_options.arguments( {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", diff --git a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp b/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp similarity index 99% rename from control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp rename to control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp index 2704d0596b4e0..badafd5f2fd15 100644 --- a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp @@ -19,7 +19,7 @@ #include "rclcpp/time.hpp" #include "trajectory_follower_test_utils.hpp" -#include +#include #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -32,7 +32,7 @@ #include using LongitudinalController = - autoware::motion::control::trajectory_follower_nodes::LongitudinalController; + autoware::motion::control::trajectory_follower_node::LongitudinalController; using LongitudinalCommand = autoware_auto_control_msgs::msg::LongitudinalCommand; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -43,7 +43,7 @@ using FakeNodeFixture = autoware::tools::testing::FakeTestNode; std::shared_ptr makeLongitudinalNode() { // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); rclcpp::NodeOptions node_options; node_options.arguments( {"--ros-args", "--params-file", diff --git a/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp b/control/trajectory_follower_node/test/trajectory_follower_test_utils.hpp similarity index 100% rename from control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp rename to control/trajectory_follower_node/test/trajectory_follower_test_utils.hpp diff --git a/control/trajectory_follower_nodes/design/trajectory_follower-design.md b/control/trajectory_follower_nodes/design/trajectory_follower-design.md deleted file mode 100644 index 574ac2934c955..0000000000000 --- a/control/trajectory_follower_nodes/design/trajectory_follower-design.md +++ /dev/null @@ -1,59 +0,0 @@ -# Trajectory Follower Nodes - -## Purpose - -Generate control commands to follow a given Trajectory. - -## Design - -This is a node of the functionalities implemented in [trajectory_follower](../../trajectory_follower/design/trajectory_follower-design.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. - -![Controller](media/Controller.drawio.svg) - -The process flow is as follows. - -1. Set input data to the lateral controller -2. Compute lateral commands. -3. Sync the result of the lateral control to the longitudinal controller. - - steer angle convergence -4. Set input data to the longitudinal controller -5. Compute longitudinal commands. -6. Sync the result of the longitudinal control to the longitudinal controller. - - velocity convergence(currently not used) - -Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` - -- lateral controller - - `keep_steer_control_until_converged` -- longitudinal controller - - `enable_keep_stopped_until_steer_convergence` - -### Inputs / Outputs / API - -#### Inputs - -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering - -#### Outputs - -- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. - -#### Parameter - -- `ctrl_period`: control commands publishing period -- `timeout_thr_sec`: duration in second after which input messages are discarded. - - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. - 1. Both commands have been received. - 2. The last received commands are not older than defined by `timeout_thr_sec`. -- `lateral_controller_mode`: `mpc_follower` or `pure_pursuit` - - (currently there is only `PID` for longitudinal controller) - -## Debugging - -Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. - -A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. - -In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 4cb4c54dd5ddb..27963a4a5a0a6 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -101,8 +101,8 @@ def launch_setup(context, *args, **kwargs): obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( - package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::Controller", + package="trajectory_follower_node", + plugin="autoware::motion::control::trajectory_follower_node::Controller", name="controller_node_exe", namespace="trajectory_follower", remappings=[ diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 900e1c55b48dc..bcb705d6b885e 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -15,8 +15,7 @@ external_cmd_selector lane_departure_checker shift_decider - trajectory_follower - trajectory_follower_nodes + trajectory_follower_node vehicle_cmd_gate ament_lint_auto