diff --git a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp b/control/trajectory_follower/include/trajectory_follower/debug_values.hpp index ab6e7e04998e7..b8c2ac445568f 100644 --- a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp +++ b/control/trajectory_follower/include/trajectory_follower/debug_values.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ #define TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ -#include "common/types.hpp" #include "trajectory_follower/visibility_control.hpp" #include @@ -28,7 +27,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::float64_t; + /// Debug Values used for debugging or controller tuning class TRAJECTORY_FOLLOWER_PUBLIC DebugValues { @@ -77,13 +76,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues * @brief get all the debug values as an std::array * @return array of all debug values */ - std::array(TYPE::SIZE)> getValues() const { return m_values; } + std::array(TYPE::SIZE)> getValues() const { return m_values; } /** * @brief set the given type to the given value * @param [in] type TYPE of the value * @param [in] value value to set */ - void setValues(const TYPE type, const float64_t value) + void setValues(const TYPE type, const double value) { m_values.at(static_cast(type)) = value; } @@ -92,10 +91,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues * @param [in] type index of the type * @param [in] value value to set */ - void setValues(const size_t type, const float64_t value) { m_values.at(type) = value; } + void setValues(const size_t type, const double value) { m_values.at(type) = value; } private: - std::array(TYPE::SIZE)> m_values; + std::array(TYPE::SIZE)> m_values; }; } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp b/control/trajectory_follower/include/trajectory_follower/interpolate.hpp index 5a3ffa0942797..52c26c9330e1a 100644 --- a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp +++ b/control/trajectory_follower/include/trajectory_follower/interpolate.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_ #define TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_ -#include "common/types.hpp" #include "trajectory_follower/visibility_control.hpp" #include @@ -30,8 +29,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + /** * @brief linearly interpolate the given values assuming a base indexing and a new desired indexing * @param [in] base_index indexes for each base value @@ -39,9 +37,9 @@ using autoware::common::types::float64_t; * @param [in] return_index desired interpolated indexes * @param [out] return_value resulting interpolated values */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index, std::vector & return_value); +TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index, std::vector & return_value); /** * @brief linearly interpolate the given values assuming a base indexing and a new desired index * @param [in] base_index indexes for each base value @@ -49,9 +47,9 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate( * @param [in] return_index desired interpolated index * @param [out] return_value resulting interpolated value */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate( - const std::vector & base_index, const std::vector & base_value, - const float64_t & return_index, float64_t & return_value); +TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate( + const std::vector & base_index, const std::vector & base_value, + const double & return_index, double & return_value); } // namespace trajectory_follower } // namespace control } // namespace motion diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 94e360b2ce622..084da2e6f7198 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" #include "geometry/common_2d.hpp" @@ -42,8 +41,7 @@ namespace trajectory_follower { namespace longitudinal_utils { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; @@ -53,19 +51,18 @@ using geometry_msgs::msg::Quaternion; /** * @brief check if trajectory is invalid or not */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj); +TRAJECTORY_FOLLOWER_PUBLIC bool isValidTrajectory(const Trajectory & traj); /** * @brief calculate distance to stopline from current vehicle position where velocity is 0 */ -TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( - const Pose & current_pose, const Trajectory & traj, const float64_t max_dist, - const float64_t max_yaw); +TRAJECTORY_FOLLOWER_PUBLIC 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 float64_t getPitchByPose(const Quaternion & quaternion); +TRAJECTORY_FOLLOWER_PUBLIC double getPitchByPose(const Quaternion & quaternion); /** * @brief calculate pitch angle from trajectory on map @@ -74,21 +71,21 @@ TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByPose(const Quaternion & quaternio * @param [in] closest_idx nearest index to current vehicle position * @param [in] wheel_base length of wheel base */ -TRAJECTORY_FOLLOWER_PUBLIC float64_t -getPitchByTraj(const Trajectory & trajectory, const size_t closest_idx, const float64_t wheel_base); +TRAJECTORY_FOLLOWER_PUBLIC double getPitchByTraj( + const Trajectory & trajectory, const size_t closest_idx, const double wheel_base); /** * @brief calculate elevation angle */ -TRAJECTORY_FOLLOWER_PUBLIC float64_t -calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); +TRAJECTORY_FOLLOWER_PUBLIC 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( - const Pose & current_pose, const float64_t delay_time, const float64_t current_vel); + const Pose & current_pose, const double delay_time, const double current_vel); /** * @brief apply linear interpolation to orientation @@ -97,7 +94,7 @@ TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay( * @param [in] ratio ratio between o_from and o_to for interpolation */ TRAJECTORY_FOLLOWER_PUBLIC Quaternion -lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float64_t ratio); +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 @@ -106,16 +103,16 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float6 */ template TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( - const T & points, const Pose & pose, const float64_t max_dist, const float64_t max_yaw) + const T & points, const Pose & pose, const double max_dist, const double max_yaw) { TrajectoryPoint interpolated_point; const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(points, pose, max_dist, max_yaw); - const float64_t len_to_interpolated = + const double len_to_interpolated = motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); - const float64_t len_segment = motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1); - const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); + const double len_segment = motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1); + const double interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); { const size_t i = seg_idx; @@ -147,8 +144,8 @@ 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 float64_t applyDiffLimitFilter( - const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t lim_val); +TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter( + const double input_val, const double prev_val, const double dt, const double lim_val); /** * @brief limit variable whose differential is within a certain value @@ -158,9 +155,9 @@ TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter( * @param [in] max_val maximum value for differential * @param [in] min_val minimum value for differential */ -TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter( - const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t max_val, - const float64_t min_val); +TRAJECTORY_FOLLOWER_PUBLIC 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 control diff --git a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp b/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp index 6b750dd890f3f..c006c50a6ea2c 100644 --- a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp +++ b/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ #define TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ -#include "common/types.hpp" #include "trajectory_follower/visibility_control.hpp" #include @@ -31,8 +30,6 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; /** * @brief Simple filter with gain on the first derivative of the value @@ -40,8 +37,8 @@ using autoware::common::types::float64_t; class LowpassFilter1d { private: - float64_t m_x; //!< @brief current filtered value - float64_t m_gain; //!< @brief gain value of first-order low-pass filter + double m_x; //!< @brief current filtered value + double m_gain; //!< @brief gain value of first-order low-pass filter public: /** @@ -49,26 +46,26 @@ class LowpassFilter1d * @param [in] x initial value * @param [in] gain first-order gain */ - LowpassFilter1d(const float64_t x, const float64_t gain) : m_x(x), m_gain(gain) {} + 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 float64_t x) { m_x = x; } + void reset(const double x) { m_x = x; } /** * @brief get the current value of the filter */ - float64_t getValue() const { return m_x; } + double getValue() const { return m_x; } /** * @brief filter a new value * @param [in] u new value */ - float64_t filter(const float64_t u) + double filter(const double u) { - const float64_t ret = m_gain * m_x + (1.0 - m_gain) * u; + const double ret = m_gain * m_x + (1.0 - m_gain) * u; m_x = ret; return ret; } @@ -81,16 +78,16 @@ class LowpassFilter1d class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter { private: - float64_t m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_a0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - float64_t m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_a0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time public: /** @@ -98,7 +95,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter * @param [in] dt sampling time * @param [in] f_cutoff_hz cutoff frequency [Hz] */ - explicit Butterworth2dFilter(float64_t dt = 0.01, float64_t f_cutoff_hz = 5.0); + explicit Butterworth2dFilter(double dt = 0.01, double f_cutoff_hz = 5.0); /** * @brief destructor @@ -110,21 +107,21 @@ class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter * @param [in] dt sampling time * @param [in] f_cutoff_hz cutoff frequency [Hz] */ - void initialize(const float64_t & dt, const float64_t & f_cutoff_hz); + void initialize(const double & dt, const double & f_cutoff_hz); /** * @brief filtering (call this function at each sampling time with input) * @param [in] u scalar input for filter * @return filtered scalar value */ - float64_t filter(const float64_t & u); + double filter(const double & u); /** * @brief filtering for time-series data * @param [in] t time-series data for input vector * @param [out] u object vector */ - void filt_vector(const std::vector & t, std::vector & u) const; + void filt_vector(const std::vector & t, std::vector & u) const; /** * @brief filtering for time-series data from both forward-backward direction for zero phase delay @@ -132,14 +129,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter * @param [out] u object vector */ void filtfilt_vector( - const std::vector & t, - std::vector & u) const; // filtering forward and backward direction + const std::vector & t, + std::vector & u) const; // filtering forward and backward direction /** * @brief get filter coefficients * @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2]. */ - void getCoefficients(std::vector & coeffs) const; + void getCoefficients(std::vector & coeffs) const; }; /** @@ -152,7 +149,7 @@ namespace MoveAverageFilter * @param [in] num index distance for moving average filter * @param [out] u object vector */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t filt_vector(const int64_t num, std::vector & u); +TRAJECTORY_FOLLOWER_PUBLIC bool filt_vector(const int num, std::vector & u); } // namespace MoveAverageFilter } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index c72f0b9c25ed0..8ae6dc5449fc3 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__MPC_HPP_ #define TRAJECTORY_FOLLOWER__MPC_HPP_ -#include "common/types.hpp" #include "geometry/common_2d.hpp" #include "helper_functions/angle_utils.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -53,79 +52,78 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + struct MPCParam { //!< @brief prediction horizon step - int64_t prediction_horizon; + int prediction_horizon; //!< @brief prediction horizon sampling time - float64_t prediction_dt; + double prediction_dt; //!< @brief threshold that feed-forward angle becomes zero - float64_t zero_ff_steer_deg; + double zero_ff_steer_deg; //!< @brief delay time for steering input to be compensated - float64_t input_delay; + double input_delay; //!< @brief for trajectory velocity calculation - float64_t acceleration_limit; + double acceleration_limit; //!< @brief for trajectory velocity calculation - float64_t velocity_time_constant; + double velocity_time_constant; //!< @brief minimum prediction dist for low velocity - float64_t min_prediction_length; + double min_prediction_length; //!< @brief time constant for steer model - float64_t steer_tau; + double steer_tau; // for weight matrix Q //!< @brief lateral error weight - float64_t weight_lat_error; + double weight_lat_error; //!< @brief heading error weight - float64_t weight_heading_error; + double weight_heading_error; //!< @brief heading error * velocity weight - float64_t weight_heading_error_squared_vel; + double weight_heading_error_squared_vel; //!< @brief terminal lateral error weight - float64_t weight_terminal_lat_error; + double weight_terminal_lat_error; //!< @brief terminal heading error weight - float64_t weight_terminal_heading_error; + double weight_terminal_heading_error; //!< @brief lateral error weight in matrix Q in low curvature point - float64_t low_curvature_weight_lat_error; + double low_curvature_weight_lat_error; //!< @brief heading error weight in matrix Q in low curvature point - float64_t low_curvature_weight_heading_error; + double low_curvature_weight_heading_error; //!< @brief heading error * velocity weight in matrix Q in low curvature point - float64_t low_curvature_weight_heading_error_squared_vel; + double low_curvature_weight_heading_error_squared_vel; // for weight matrix R //!< @brief steering error weight - float64_t weight_steering_input; + double weight_steering_input; //!< @brief steering error * velocity weight - float64_t weight_steering_input_squared_vel; + double weight_steering_input_squared_vel; //!< @brief lateral jerk weight - float64_t weight_lat_jerk; + double weight_lat_jerk; //!< @brief steering rate weight - float64_t weight_steer_rate; + double weight_steer_rate; //!< @brief steering angle acceleration weight - float64_t weight_steer_acc; + double weight_steer_acc; //!< @brief steering error weight in matrix R in low curvature point - float64_t low_curvature_weight_steering_input; + double low_curvature_weight_steering_input; //!< @brief steering error * velocity weight in matrix R in low curvature point - float64_t low_curvature_weight_steering_input_squared_vel; + double low_curvature_weight_steering_input_squared_vel; //!< @brief lateral jerk weight in matrix R in low curvature point - float64_t low_curvature_weight_lat_jerk; + double low_curvature_weight_lat_jerk; //!< @brief steering rate weight in matrix R in low curvature point - float64_t low_curvature_weight_steer_rate; + double low_curvature_weight_steer_rate; //!< @brief steering angle acceleration weight in matrix R in low curvature - float64_t low_curvature_weight_steer_acc; + double low_curvature_weight_steer_acc; //!< @brief threshold of curvature to use "low curvature" parameter - float64_t low_curvature_thresh_curvature; + double low_curvature_thresh_curvature; }; /** * MPC problem data */ struct MPCData { - int64_t nearest_idx; - float64_t nearest_time; + int nearest_idx; + double nearest_time; geometry_msgs::msg::Pose nearest_pose; - float64_t steer; - float64_t predicted_steer; - float64_t lateral_err; - float64_t yaw_err; + double steer; + double predicted_steer; + double lateral_err; + double yaw_err; }; /** * Matrices used for MPC optimization @@ -167,42 +165,42 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC trajectory_follower::Butterworth2dFilter m_lpf_yaw_error; //!< @brief raw output computed two iterations ago - float64_t m_raw_steer_cmd_pprev = 0.0; + double m_raw_steer_cmd_pprev = 0.0; //!< @brief previous lateral error for derivative - float64_t m_lateral_error_prev = 0.0; + double m_lateral_error_prev = 0.0; //!< @brief previous lateral error for derivative - float64_t m_yaw_error_prev = 0.0; + double m_yaw_error_prev = 0.0; //!< @brief previous predicted steering - std::shared_ptr m_steer_prediction_prev; + std::shared_ptr m_steer_prediction_prev; //!< @brief previous computation time rclcpp::Time m_time_prev = rclcpp::Time(0, 0, RCL_ROS_TIME); //!< @brief shift is forward or not. - bool8_t m_is_forward_shift = true; + bool m_is_forward_shift = true; //!< @brief buffer of sent command std::vector m_ctrl_cmd_vec; //!< @brief minimum prediction distance - float64_t m_min_prediction_length = 5.0; + double m_min_prediction_length = 5.0; /** * @brief get variables for mpc calculation */ - bool8_t getData( + bool getData( const trajectory_follower::MPCTrajectory & traj, const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, const geometry_msgs::msg::Pose & current_pose, MPCData * data); /** * @brief calculate predicted steering */ - float64_t calcSteerPrediction(); + double calcSteerPrediction(); /** * @brief get the sum of all steering commands over the given time range */ - float64_t getSteerCmdSum( - const rclcpp::Time & t_start, const rclcpp::Time & t_end, const float64_t time_constant) const; + double getSteerCmdSum( + const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const; /** * @brief set the reference trajectory to follow */ - void storeSteerCmd(const float64_t steer); + void storeSteerCmd(const double steer); /** * @brief reset previous result of MPC */ @@ -218,15 +216,15 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @param [in] start_time start time for update * @param [out] x updated state at delayed_time */ - bool8_t updateStateForDelayCompensation( - const trajectory_follower::MPCTrajectory & traj, const float64_t & start_time, + bool updateStateForDelayCompensation( + const trajectory_follower::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 float64_t prediction_dt); + const trajectory_follower::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 @@ -234,14 +232,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @param [in] prediction_dt prediction delta time * @param [out] Uex optimized input vector */ - bool8_t executeOptimization( - const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, const float64_t prediction_dt, + bool executeOptimization( + const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, const double prediction_dt, Eigen::VectorXd * Uex); /** * @brief resample trajectory with mpc resampling time */ - bool8_t resampleMPCTrajectoryByTime( - const float64_t start_time, const float64_t prediction_dt, + bool resampleMPCTrajectoryByTime( + const double start_time, const double prediction_dt, const trajectory_follower::MPCTrajectory & input, trajectory_follower::MPCTrajectory * output) const; /** @@ -249,45 +247,45 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC */ trajectory_follower::MPCTrajectory applyVelocityDynamicsFilter( const trajectory_follower::MPCTrajectory & trajectory, - const geometry_msgs::msg::Pose & current_pose, const float64_t v0) const; + 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. */ - float64_t getPredictionDeltaTime( - const float64_t start_time, const trajectory_follower::MPCTrajectory & input, + double getPredictionDeltaTime( + const double start_time, const trajectory_follower::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose) const; /** * @brief add weights related to lateral_jerk, steering_rate, steering_acc into R */ - void addSteerWeightR(const float64_t prediction_dt, Eigen::MatrixXd * R) const; + void addSteerWeightR(const double prediction_dt, Eigen::MatrixXd * R) const; /** * @brief add weights related to lateral_jerk, steering_rate, steering_acc into f */ - void addSteerWeightF(const float64_t prediction_dt, Eigen::MatrixXd * f) const; + void addSteerWeightF(const double prediction_dt, Eigen::MatrixXd * f) const; /** * @brief calculate desired steering rate. */ - float64_t calcDesiredSteeringRate( + double calcDesiredSteeringRate( const MPCMatrix & m, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const float64_t u_filtered, const float current_steer, const float64_t predict_dt) const; + const double u_filtered, const float current_steer, const double predict_dt) const; /** * @brief check if the matrix has invalid value */ - bool8_t isValid(const MPCMatrix & m) const; + bool isValid(const MPCMatrix & m) const; /** * @brief return true if the given curvature is considered low */ - inline bool8_t isLowCurvature(const float64_t curvature) + inline bool isLowCurvature(const double curvature) { return std::fabs(curvature) < m_param.low_curvature_thresh_curvature; } /** * @brief return the weight of the lateral error for the given curvature */ - inline float64_t getWeightLatError(const float64_t curvature) + inline double getWeightLatError(const double curvature) { return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_error : m_param.weight_lat_error; @@ -295,7 +293,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief return the weight of the heading error for the given curvature */ - inline float64_t getWeightHeadingError(const float64_t curvature) + inline double getWeightHeadingError(const double curvature) { return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error : m_param.weight_heading_error; @@ -303,7 +301,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief return the squared velocity weight of the heading error for the given curvature */ - inline float64_t getWeightHeadingErrorSqVel(const float64_t curvature) + inline double getWeightHeadingErrorSqVel(const double curvature) { return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error_squared_vel : m_param.weight_heading_error_squared_vel; @@ -311,7 +309,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief return the weight of the steer input for the given curvature */ - inline float64_t getWeightSteerInput(const float64_t curvature) + inline double getWeightSteerInput(const double curvature) { return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input : m_param.weight_steering_input; @@ -319,7 +317,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief return the squared velocity weight of the steer input for the given curvature */ - inline float64_t getWeightSteerInputSqVel(const float64_t curvature) + inline double getWeightSteerInputSqVel(const double curvature) { return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input_squared_vel : m_param.weight_steering_input_squared_vel; @@ -327,7 +325,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief return the weight of the lateral jerk for the given curvature */ - inline float64_t getWeightLatJerk(const float64_t curvature) + inline double getWeightLatJerk(const double curvature) { return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_jerk : m_param.weight_lat_jerk; @@ -335,7 +333,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief return the weight of the steering rate for the given curvature */ - inline float64_t getWeightSteerRate(const float64_t curvature) + inline double getWeightSteerRate(const double curvature) { return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_rate : m_param.weight_steer_rate; @@ -343,7 +341,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief return the weight of the steering acceleration for the given curvature */ - inline float64_t getWeightSteerAcc(const float64_t curvature) + inline double getWeightSteerAcc(const double curvature) { return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_acc : m_param.weight_steer_acc; @@ -355,23 +353,23 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC //!< @brief MPC design parameter MPCParam m_param; //!< @brief mpc_output buffer for delay time compensation - std::deque m_input_buffer; + std::deque m_input_buffer; //!< @brief mpc raw output in previous period - float64_t m_raw_steer_cmd_prev = 0.0; + double m_raw_steer_cmd_prev = 0.0; /* parameters for control*/ //!< @brief use stop cmd when lateral error exceeds this [m] - float64_t m_admissible_position_error; + double m_admissible_position_error; //!< @brief use stop cmd when yaw error exceeds this [rad] - float64_t m_admissible_yaw_error_rad; + double m_admissible_yaw_error_rad; //!< @brief steering command limit [rad] - float64_t m_steer_lim; + double m_steer_lim; //!< @brief steering rate limit [rad/s] - float64_t m_steer_rate_lim; + double m_steer_rate_lim; //!< @brief control frequency [s] - float64_t m_ctrl_period; + double m_ctrl_period; /* parameters for path smoothing */ //!< @brief flag to use predicted steer, not measured steer. - bool8_t m_use_steer_prediction; + bool m_use_steer_prediction; //!< @brief parameters for nearest index search double ego_nearest_dist_threshold; double ego_nearest_yaw_threshold; @@ -389,9 +387,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @param [out] predicted_traj predicted MPC trajectory * @param [out] diagnostic diagnostic msg to be filled-out */ - bool8_t calculateMPC( + bool calculateMPC( const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, - const float64_t current_velocity, const geometry_msgs::msg::Pose & current_pose, + const double current_velocity, const geometry_msgs::msg::Pose & current_pose, autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd, autoware_auto_planning_msgs::msg::Trajectory & predicted_traj, tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic); @@ -400,9 +398,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC */ void setReferenceTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, - const float64_t traj_resample_dist, const bool8_t enable_path_smoothing, - const int64_t path_filter_moving_ave_num, const int64_t curvature_smoothing_num_traj, - const int64_t curvature_smoothing_num_ref_steer); + const double traj_resample_dist, const bool enable_path_smoothing, + const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, + const int curvature_smoothing_num_ref_steer); /** * @brief set the vehicle model of this MPC */ @@ -424,7 +422,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @brief initialize low pass filters */ inline void initializeLowPassFilters( - const float64_t steering_lpf_cutoff_hz, const float64_t error_deriv_lpf_cutoff_hz) + const double steering_lpf_cutoff_hz, const double error_deriv_lpf_cutoff_hz) { m_lpf_steering_cmd.initialize(m_ctrl_period, steering_lpf_cutoff_hz); m_lpf_lateral_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz); @@ -433,11 +431,11 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief return true if the vehicle model of this MPC is set */ - inline bool8_t hasVehicleModel() const { return m_vehicle_model_ptr != nullptr; } + inline bool hasVehicleModel() const { return m_vehicle_model_ptr != nullptr; } /** * @brief return true if the QP solver of this MPC is set */ - inline bool8_t hasQPSolver() const { return m_qpsolver_ptr != nullptr; } + inline bool hasQPSolver() const { return m_qpsolver_ptr != nullptr; } /** * @brief set the RCLCPP logger to use for logging */ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index a15869578cd48..d38312505fed7 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ #define TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ -#include "common/types.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -58,8 +57,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralControllerBase @@ -88,23 +86,23 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController /* parameters for path smoothing */ //!< @brief flag for path smoothing - bool8_t m_enable_path_smoothing; + bool m_enable_path_smoothing; //!< @brief param of moving average filter for path smoothing - int64_t m_path_filter_moving_ave_num; + int m_path_filter_moving_ave_num; //!< @brief point-to-point index distance for curvature calculation for trajectory //NOLINT - int64_t m_curvature_smoothing_num_traj; + int m_curvature_smoothing_num_traj; //!< @brief point-to-point index distance for curvature calculation for reference steer command //!< //NOLINT - int64_t m_curvature_smoothing_num_ref_steer; + int m_curvature_smoothing_num_ref_steer; //!< @brief path resampling interval [m] - float64_t m_traj_resample_dist; + double m_traj_resample_dist; /* parameters for stop state */ - float64_t m_stop_state_entry_ego_speed; - float64_t m_stop_state_entry_target_speed; - float64_t m_converged_steer_rad; - float64_t m_new_traj_duration_time; // check trajectory shape change - float64_t m_new_traj_end_dist; // check trajectory shape change + double m_stop_state_entry_ego_speed; + double m_stop_state_entry_target_speed; + double m_converged_steer_rad; + double m_new_traj_duration_time; // check trajectory shape change + double m_new_traj_end_dist; // check trajectory shape change bool m_keep_steer_control_until_converged; // trajectory buffer for detecting new trajectory @@ -121,10 +119,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController autoware_auto_planning_msgs::msg::Trajectory::SharedPtr m_current_trajectory_ptr; //!< @brief mpc filtered output in previous period - float64_t m_steer_cmd_prev = 0.0; + double m_steer_cmd_prev = 0.0; //!< @brief flag of m_ctrl_cmd_prev initialization - bool8_t m_is_ctrl_cmd_prev_initialized = false; + bool m_is_ctrl_cmd_prev_initialized = false; //!< @brief previous control command autoware_auto_control_msgs::msg::AckermannLateralCommand m_ctrl_cmd_prev; @@ -136,7 +134,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController double m_ego_nearest_yaw_threshold; //!< initialize timer to work in real, simulation, and replay - void initTimer(float64_t period_s); + void initTimer(double period_s); /** * @brief compute control command for path follow with a constant control period */ @@ -155,7 +153,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController /** * @brief check if the received data is valid. */ - bool8_t checkData() const; + bool checkData() const; /** * @brief create control command @@ -189,14 +187,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController /** * @brief check ego car is in stopped state */ - bool8_t isStoppedState() const; + bool isStoppedState() const; /** * @brief check if the trajectory has valid value */ - bool8_t isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const; + bool isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const; - bool8_t isTrajectoryShapeChanged() const; + bool isTrajectoryShapeChanged() const; bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const; diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp index 7c252dd61d995..9016c0adbb689 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ #define TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ -#include "common/types.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "trajectory_follower/visibility_control.hpp" @@ -32,7 +31,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::float64_t; + /** * Trajectory class for mpc follower * @brief calculate control command to follow reference waypoints @@ -40,21 +39,21 @@ using autoware::common::types::float64_t; class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory { public: - std::vector x; //!< @brief x position x vector - std::vector y; //!< @brief y position y vector - std::vector z; //!< @brief z position z vector - std::vector yaw; //!< @brief yaw pose yaw vector - std::vector vx; //!< @brief vx velocity vx vector - std::vector k; //!< @brief k curvature k vector - std::vector smooth_k; //!< @brief k smoothed-curvature k vector - std::vector relative_time; //!< @brief relative_time duration time from start point + std::vector x; //!< @brief x position x vector + std::vector y; //!< @brief y position y vector + std::vector z; //!< @brief z position z vector + std::vector yaw; //!< @brief yaw pose yaw vector + std::vector vx; //!< @brief vx velocity vx vector + std::vector k; //!< @brief k curvature k vector + std::vector smooth_k; //!< @brief k smoothed-curvature k vector + std::vector relative_time; //!< @brief relative_time duration time from start point /** * @brief push_back for all values */ void push_back( - const float64_t & xp, const float64_t & yp, const float64_t & zp, const float64_t & yawp, - const float64_t & vxp, const float64_t & kp, const float64_t & smooth_kp, const float64_t & tp); + const double & xp, const double & yp, const double & zp, const double & yawp, + const double & vxp, const double & kp, const double & smooth_kp, const double & tp); /** * @brief clear for all values */ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp index bd681e4997b62..9f394ff9baa6f 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ #define TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "geometry/common_2d.hpp" #include "helper_functions/angle_utils.hpp" @@ -53,27 +52,25 @@ namespace trajectory_follower { namespace MPCUtils { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + /** * @brief convert from yaw to ros-Quaternion * @param [in] yaw input yaw angle * @return quaternion */ -TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion getQuaternionFromYaw( - const float64_t & yaw); +TRAJECTORY_FOLLOWER_PUBLIC 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); +TRAJECTORY_FOLLOWER_PUBLIC void convertEulerAngleToMonotonic(std::vector * a); /** * @brief calculate the lateral error of the given pose relative to the given reference pose * @param [in] ego_pose pose to check for error * @param [in] ref_pose reference pose * @return lateral distance between the two poses */ -TRAJECTORY_FOLLOWER_PUBLIC float64_t calcLateralError( +TRAJECTORY_FOLLOWER_PUBLIC 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 @@ -81,7 +78,7 @@ TRAJECTORY_FOLLOWER_PUBLIC float64_t calcLateralError( * @param [out] output resulting MPCTrajectory * @return true if the conversion was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToMPCTrajectory( +TRAJECTORY_FOLLOWER_PUBLIC bool convertToMPCTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output); /** * @brief convert the given MPCTrajectory to a Trajectory msg @@ -89,7 +86,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToMPCTrajectory( * @param [out] output resulting Trajectory msg * @return true if the conversion was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToAutowareTrajectory( +TRAJECTORY_FOLLOWER_PUBLIC bool convertToAutowareTrajectory( const MPCTrajectory & input, autoware_auto_planning_msgs::msg::Trajectory & output); /** * @brief calculate the arc length at each point of the given trajectory @@ -97,15 +94,15 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToAutowareTrajectory( * @param [out] arclength the cummulative arc length at each point of the trajectory */ TRAJECTORY_FOLLOWER_PUBLIC void calcMPCTrajectoryArclength( - const MPCTrajectory & trajectory, std::vector * arclength); + const MPCTrajectory & trajectory, std::vector * arclength); /** * @brief resample the given trajectory with the given fixed interval * @param [in] input trajectory to resample * @param [in] resample_interval_dist the desired distance between two successive trajectory points * @param [out] output the resampled trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const float64_t resample_interval_dist, MPCTrajectory * output); +TRAJECTORY_FOLLOWER_PUBLIC 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 * indexing @@ -114,15 +111,15 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t resampleMPCTrajectoryByDistance( * @param [in] out_index desired interpolated indexes * @param [out] out_traj resulting interpolated MPCTrajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpMPCTrajectory( - const std::vector & in_index, const MPCTrajectory & in_traj, - const std::vector & out_index, MPCTrajectory * out_traj); +TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpMPCTrajectory( + const std::vector & in_index, const MPCTrajectory & in_traj, + const std::vector & out_index, MPCTrajectory * out_traj); /** * @brief fill the relative_time field of the given MPCTrajectory * @param [in] traj MPCTrajectory for which to fill in the relative_time * @return true if the calculation was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcMPCTrajectoryTime(MPCTrajectory & traj); +TRAJECTORY_FOLLOWER_PUBLIC 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 @@ -132,7 +129,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcMPCTrajectoryTime(MPCTrajectory & traj); * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity */ TRAJECTORY_FOLLOWER_PUBLIC void dynamicSmoothingVelocity( - const size_t start_idx, const float64_t start_vel, const float64_t acc_lim, const float64_t tau, + const size_t start_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj); /** * @brief calculate yaw angle in MPCTrajectory from xy vector @@ -149,7 +146,7 @@ TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY( * calculation * @param [inout] traj object trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcTrajectoryCurvature( +TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature( const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer, MPCTrajectory * traj); /** @@ -159,7 +156,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcTrajectoryCurvature( * @param [in] traj input trajectory * @return vector of curvatures at each point of the given trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( +TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( const size_t curvature_smoothing_num, const MPCTrajectory & traj); /** * @brief calculate nearest pose on MPCTrajectory with linear interpolation @@ -172,16 +169,16 @@ 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 bool8_t calcNearestPoseInterp( +TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp( const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, - geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time, + 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, rclcpp::Clock & clock); // /** // * @brief calculate distance to stopped point // */ -TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( - const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int64_t origin); +TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance( + const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin); } // namespace MPCUtils } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/pid.hpp b/control/trajectory_follower/include/trajectory_follower/pid.hpp index ca64614c9411f..d9392ba3ced91 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__PID_HPP_ #define TRAJECTORY_FOLLOWER__PID_HPP_ -#include "common/types.hpp" #include "trajectory_follower/visibility_control.hpp" #include @@ -28,8 +27,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + /// @brief implementation of a PID controller class TRAJECTORY_FOLLOWER_PUBLIC PIDController { @@ -45,16 +43,16 @@ class TRAJECTORY_FOLLOWER_PUBLIC PIDController * @return PID output * @throw std::runtime_error if gains or limits have not been set */ - float64_t calculate( - const float64_t error, const float64_t dt, const bool8_t is_integrated, - std::vector & pid_contributions); + double calculate( + const double error, const double dt, const bool is_integrated, + std::vector & pid_contributions); /** * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms * @param [in] kp proportional coefficient * @param [in] ki integral coefficient * @param [in] kd derivative coefficient */ - void setGains(const float64_t kp, const float64_t ki, const float64_t kd); + void setGains(const double kp, const double ki, const double kd); /** * @brief set limits on the total, proportional, integral, and derivative components * @param [in] max_ret maximum return value of this PID @@ -67,9 +65,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC PIDController * @param [in] min_ret_d minimum value of the derivative component */ void setLimits( - const float64_t max_ret, const float64_t min_ret, const float64_t max_ret_p, - const float64_t min_ret_p, const float64_t max_ret_i, const float64_t min_ret_i, - const float64_t max_ret_d, const float64_t min_ret_d); + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d); /** * @brief reset this PID to its initial state */ @@ -79,26 +76,26 @@ class TRAJECTORY_FOLLOWER_PUBLIC PIDController // PID parameters struct Params { - float64_t kp; - float64_t ki; - float64_t kd; - float64_t max_ret_p; - float64_t min_ret_p; - float64_t max_ret_i; - float64_t min_ret_i; - float64_t max_ret_d; - float64_t min_ret_d; - float64_t max_ret; - float64_t min_ret; + double kp; + double ki; + double kd; + double max_ret_p; + double min_ret_p; + double max_ret_i; + double min_ret_i; + double max_ret_d; + double min_ret_d; + double max_ret; + double min_ret; }; Params m_params; // state variables - float64_t m_error_integral; - float64_t m_prev_error; - bool8_t m_is_first_time; - bool8_t m_is_gains_set; - bool8_t m_is_limits_set; + double m_error_integral; + double m_prev_error; + bool m_is_first_time; + bool m_is_gains_set; + bool m_is_limits_set; }; } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index d9fe113e448e9..f91c09ecf3527 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -52,8 +52,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; /// \class PidLongitudinalController @@ -66,21 +65,21 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal private: struct Motion { - float64_t vel{0.0}; - float64_t acc{0.0}; + double vel{0.0}; + double acc{0.0}; }; enum class Shift { Forward = 0, Reverse }; struct ControlData { - bool8_t is_far_from_trajectory{false}; + bool is_far_from_trajectory{false}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx Motion current_motion{}; Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation - float64_t stop_dist{0.0}; // signed distance that is positive when car is before the stopline - float64_t slope_angle{0.0}; - float64_t dt{0.0}; + double stop_dist{0.0}; // signed distance that is positive when car is before the stopline + double slope_angle{0.0}; + double dt{0.0}; }; rclcpp::Node * node_; // ros variables @@ -97,7 +96,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr}; // vehicle info - float64_t m_wheel_base; + double m_wheel_base; // control state enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; @@ -112,43 +111,43 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal }; // control period - float64_t m_longitudinal_ctrl_period; + double m_longitudinal_ctrl_period; // delay compensation - float64_t m_delay_compensation_time; + double m_delay_compensation_time; // enable flags - bool8_t m_enable_smooth_stop; - bool8_t m_enable_overshoot_emergency; - bool8_t m_enable_slope_compensation; - bool8_t m_enable_large_tracking_error_emergency; - bool8_t m_enable_keep_stopped_until_steer_convergence; + bool m_enable_smooth_stop; + bool m_enable_overshoot_emergency; + bool m_enable_slope_compensation; + bool m_enable_large_tracking_error_emergency; + bool m_enable_keep_stopped_until_steer_convergence; // smooth stop transition struct StateTransitionParams { // drive - float64_t drive_state_stop_dist; - float64_t drive_state_offset_stop_dist; + double drive_state_stop_dist; + double drive_state_offset_stop_dist; // stopping - float64_t stopping_state_stop_dist; + double stopping_state_stop_dist; // stop - float64_t stopped_state_entry_duration_time; - float64_t stopped_state_entry_vel; - float64_t stopped_state_entry_acc; + double stopped_state_entry_duration_time; + double stopped_state_entry_vel; + double stopped_state_entry_acc; // emergency - float64_t emergency_state_overshoot_stop_dist; - float64_t emergency_state_traj_trans_dev; - float64_t emergency_state_traj_rot_dev; + double emergency_state_overshoot_stop_dist; + double emergency_state_traj_trans_dev; + double emergency_state_traj_rot_dev; }; StateTransitionParams m_state_transition_params; // drive trajectory_follower::PIDController m_pid_vel; std::shared_ptr m_lpf_vel_error{nullptr}; - float64_t m_current_vel_threshold_pid_integrate; - bool8_t m_enable_brake_keeping_before_stop; - float64_t m_brake_keeping_acc; + 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; @@ -156,34 +155,34 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal // stop struct StoppedStateParams { - float64_t vel; - float64_t acc; - float64_t jerk; + double vel; + double acc; + double jerk; }; StoppedStateParams m_stopped_state_params; // emergency struct EmergencyStateParams { - float64_t vel; - float64_t acc; - float64_t jerk; + double vel; + double acc; + double jerk; }; EmergencyStateParams m_emergency_state_params; // acceleration limit - float64_t m_max_acc; - float64_t m_min_acc; + double m_max_acc; + double m_min_acc; // jerk limit - float64_t m_max_jerk; - float64_t m_min_jerk; + double m_max_jerk; + double m_min_jerk; // slope compensation - bool8_t m_use_traj_for_pitch; + bool m_use_traj_for_pitch; std::shared_ptr m_lpf_pitch{nullptr}; - float64_t m_max_pitch_rad; - float64_t m_min_pitch_rad; + double m_max_pitch_rad; + double m_min_pitch_rad; // ego nearest index search double m_ego_nearest_dist_threshold; @@ -201,7 +200,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal // diff limit Motion m_prev_ctrl_cmd{}; // with slope compensation Motion m_prev_raw_ctrl_cmd{}; // without slope compensation - std::vector> m_vel_hist; + std::vector> m_vel_hist; // debug values trajectory_follower::DebugValues m_debug_values; @@ -213,8 +212,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal diagnostic_updater::Updater diagnostic_updater_; struct DiagnosticData { - float64_t trans_deviation{0.0}; // translation deviation between nearest point and current_pose - float64_t rot_deviation{0.0}; // rotation deviation between nearest point and current_pose + double trans_deviation{0.0}; // translation deviation between nearest point and current_pose + double rot_deviation{0.0}; // rotation deviation between nearest point and current_pose }; DiagnosticData m_diagnostic_data; void setupDiagnosticUpdater(); @@ -259,7 +258,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal * @brief calculate control command in emergency state * @param [in] dt time between previous and current one */ - Motion calcEmergencyCtrlCmd(const float64_t dt) const; + Motion calcEmergencyCtrlCmd(const double dt) const; /** * @brief update control state according to the current situation @@ -281,7 +280,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal * @param [in] current_vel current velocity of the vehicle */ autoware_auto_control_msgs::msg::LongitudinalCommand createCtrlCmdMsg( - const Motion & ctrl_cmd, const float64_t & current_vel); + const Motion & ctrl_cmd, const double & current_vel); /** * @brief publish debug data @@ -293,7 +292,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal /** * @brief calculate time between current and previous one */ - float64_t getDt(); + double getDt(); /** * @brief calculate current velocity and acceleration @@ -312,13 +311,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal * @param [in] raw_acc acceleration before filtered * @param [in] control_data data for control calculation */ - float64_t calcFilteredAcc(const float64_t raw_acc, const ControlData & control_data); + double calcFilteredAcc(const double raw_acc, const ControlData & control_data); /** * @brief store acceleration command before slope compensation * @param [in] accel command before slope compensation */ - void storeAccelCmd(const float64_t accel); + void storeAccelCmd(const double accel); /** * @brief add acceleration to compensate for slope @@ -326,8 +325,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal * @param [in] pitch pitch angle (upward is negative) * @param [in] shift direction that vehicle move (forward or backward) */ - float64_t applySlopeCompensation( - const float64_t acc, const float64_t pitch, const Shift shift) const; + double applySlopeCompensation(const double acc, const double pitch, const Shift shift) const; /** * @brief keep target motion acceleration negative before stop @@ -353,8 +351,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal * @param [in] current_motion current velocity and acceleration of the vehicle * @param [in] delay_compensation_time predicted time delay */ - float64_t predictedVelocityInTargetPoint( - const Motion current_motion, const float64_t delay_compensation_time) const; + double predictedVelocityInTargetPoint( + const Motion current_motion, const double delay_compensation_time) const; /** * @brief calculate velocity feedback with feed forward and pid controller @@ -363,8 +361,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal * @param [in] dt time step to use * @param [in] current_vel current velocity of the vehicle */ - float64_t applyVelocityFeedback( - const Motion target_motion, const float64_t dt, const float64_t current_vel); + double applyVelocityFeedback( + const Motion target_motion, const double dt, const double current_vel); /** * @brief update variables for debugging about pitch @@ -372,8 +370,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal * @param [in] traj_pitch current trajectory pitch * @param [in] raw_pitch current raw pitch of the vehicle (unfiltered) */ - void updatePitchDebugValues( - const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch); + void updatePitchDebugValues(const double pitch, const double traj_pitch, const double raw_pitch); /** * @brief update variables for velocity and acceleration diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp index 1a1c0b4c782a4..dc48253d7c0d0 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Dense" #include "eigen3/Eigen/LU" @@ -29,7 +28,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; + /// Interface for solvers of Quadratic Programming (QP) problems class TRAJECTORY_FOLLOWER_PUBLIC QPSolverInterface { @@ -51,7 +50,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverInterface * @param [out] u optimal variable vector * @return ture if the problem was solved */ - virtual bool8_t solve( + virtual bool solve( const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0; diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp index c73199a654b37..546b16769bfc9 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ #define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Dense" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" @@ -30,8 +29,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + /// Solver for QP problems using the OSQP library class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface { @@ -58,7 +56,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface * @param [out] u optimal variable vector * @return true if the problem was solved */ - bool8_t solve( + bool solve( const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp index 9add78edd918d..f96ff6621521c 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ #define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Dense" #include "eigen3/Eigen/LU" @@ -32,7 +31,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; + /** * Solver for QP problems using least square decomposition * implement with Eigen's standard Cholesky decomposition (LLT) @@ -62,7 +61,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverEigenLeastSquareLLT : public QPSolverIn * @param [out] u optimal variable vector * @return true if the problem was solved */ - bool8_t solve( + bool solve( const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; diff --git a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp b/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp index ad40de50a35aa..c86f319f1288c 100644 --- a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp +++ b/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ #define TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ -#include "common/types.hpp" #include "rclcpp/rclcpp.hpp" #include "trajectory_follower/visibility_control.hpp" @@ -35,8 +34,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + /** * @brief Smooth stop class to implement vehicle specific deceleration profiles */ @@ -48,7 +46,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop * @param [in] pred_vel_in_target predicted ego velocity when the stop command will be executed * @param [in] pred_stop_dist predicted stop distance when the stop command will be executed */ - void init(const float64_t pred_vel_in_target, const float64_t pred_stop_dist); + void init(const double pred_vel_in_target, const double pred_stop_dist); /** * @brief set the parameters of this smooth stop @@ -66,19 +64,18 @@ class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop * [m] */ void setParams( - float64_t max_strong_acc, float64_t min_strong_acc, float64_t weak_acc, float64_t weak_stop_acc, - float64_t strong_stop_acc, float64_t min_fast_vel, float64_t min_running_vel, - float64_t min_running_acc, float64_t weak_stop_time, float64_t weak_stop_dist, - float64_t strong_stop_dist); + 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); /** * @brief predict time when car stops by fitting some latest observed velocity history * with linear function (v = at + b) - * @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, float64_t[m/s]) pairs + * @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, double[m/s]) pairs * @throw std::runtime_error if parameters have not been set */ - std::experimental::optional calcTimeToStop( - const std::vector> & vel_hist) const; + std::experimental::optional calcTimeToStop( + const std::vector> & vel_hist) const; /** * @brief calculate accel command while stopping @@ -89,36 +86,36 @@ class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop * @param [in] stop_dist distance left to travel before stopping [m] * @param [in] current_vel current velocity of ego [m/s] * @param [in] current_acc current acceleration of ego [m/s²] - * @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, float64_t[m/s]) pairs + * @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, double[m/s]) pairs * @param [in] delay_time assumed time delay when the stop command will actually be executed * @throw std::runtime_error if parameters have not been set */ - float64_t calculate( - const float64_t stop_dist, const float64_t current_vel, const float64_t current_acc, - const std::vector> & vel_hist, const float64_t delay_time); + double calculate( + const double stop_dist, const double current_vel, const double current_acc, + const std::vector> & vel_hist, const double delay_time); private: struct Params { - float64_t max_strong_acc; - float64_t min_strong_acc; - float64_t weak_acc; - float64_t weak_stop_acc; - float64_t strong_stop_acc; + double max_strong_acc; + double min_strong_acc; + double weak_acc; + double weak_stop_acc; + double strong_stop_acc; - float64_t min_fast_vel; - float64_t min_running_vel; - float64_t min_running_acc; - float64_t weak_stop_time; + double min_fast_vel; + double min_running_vel; + double min_running_acc; + double weak_stop_time; - float64_t weak_stop_dist; - float64_t strong_stop_dist; + double weak_stop_dist; + double strong_stop_dist; }; Params m_params; - float64_t m_strong_acc; + double m_strong_acc; rclcpp::Time m_weak_acc_time; - bool8_t m_is_set_params = false; + bool m_is_set_params = false; }; } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 23f3ca8e288fa..91c873d54bad1 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -47,7 +47,6 @@ #ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ #define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" #include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" @@ -61,7 +60,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::float64_t; + /** * Vehicle model class of bicycle dynamics * @brief calculate model-related values @@ -80,8 +79,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInter * @param [in] cr rear cornering power [N/rad] */ DynamicsBicycleModel( - const float64_t wheelbase, const float64_t mass_fl, const float64_t mass_fr, - const float64_t mass_rl, const float64_t mass_rr, const float64_t cf, const float64_t cr); + const double wheelbase, const double mass_fl, const double mass_fr, const double mass_rl, + const double mass_rr, const double cf, const double cr); /** * @brief destructor @@ -98,7 +97,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInter */ void calculateDiscreteMatrix( Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & w_d, Eigen::MatrixXd & c_d, - const float64_t dt) override; + const double dt) override; /** * @brief calculate reference input @@ -107,12 +106,12 @@ class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInter void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; private: - float64_t m_lf; //!< @brief length from center of mass to front wheel [m] - float64_t m_lr; //!< @brief length from center of mass to rear wheel [m] - float64_t m_mass; //!< @brief total mass of vehicle [kg] - float64_t m_iz; //!< @brief moment of inertia [kg * m2] - float64_t m_cf; //!< @brief front cornering power [N/rad] - float64_t m_cr; //!< @brief rear cornering power [N/rad] + double m_lf; //!< @brief length from center of mass to front wheel [m] + double m_lr; //!< @brief length from center of mass to rear wheel [m] + double m_mass; //!< @brief total mass of vehicle [kg] + double m_iz; //!< @brief moment of inertia [kg * m2] + double m_cf; //!< @brief front cornering power [N/rad] + double m_cr; //!< @brief rear cornering power [N/rad] }; } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 525461f3a9e5d..6ab35f7a23413 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -41,7 +41,6 @@ #ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" #include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" @@ -55,7 +54,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::float64_t; + /** * Vehicle model class of bicycle kinematics * @brief calculate model-related values @@ -69,8 +68,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInt * @param [in] steer_lim steering angle limit [rad] * @param [in] steer_tau steering time constant for 1d-model [s] */ - KinematicsBicycleModel( - const float64_t wheelbase, const float64_t steer_lim, const float64_t steer_tau); + KinematicsBicycleModel(const double wheelbase, const double steer_lim, const double steer_tau); /** * @brief destructor @@ -87,7 +85,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInt */ void calculateDiscreteMatrix( Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const float64_t dt) override; + const double dt) override; /** * @brief calculate reference input @@ -96,8 +94,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInt void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; private: - float64_t m_steer_lim; //!< @brief steering angle limit [rad] - float64_t m_steer_tau; //!< @brief steering time constant for 1d-model [s] + 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 control diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 8656f1ab7afb8..88b3909bcf7f0 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -38,7 +38,6 @@ #ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ #define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" #include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" @@ -52,7 +51,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::float64_t; + /** * Vehicle model class of bicycle kinematics without steering delay * @brief calculate model-related values @@ -65,7 +64,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleM * @param [in] wheelbase wheelbase length [m] * @param [in] steer_lim steering angle limit [rad] */ - KinematicsBicycleModelNoDelay(const float64_t wheelbase, const float64_t steer_lim); + KinematicsBicycleModelNoDelay(const double wheelbase, const double steer_lim); /** * @brief destructor @@ -82,7 +81,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleM */ void calculateDiscreteMatrix( Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const float64_t dt) override; + const double dt) override; /** * @brief calculate reference input @@ -91,7 +90,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleM void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; private: - float64_t m_steer_lim; //!< @brief steering angle limit [rad] + double m_steer_lim; //!< @brief steering angle limit [rad] }; } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp index 1c3856246f719..b2ededcfb7808 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp @@ -15,7 +15,6 @@ #ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "trajectory_follower/visibility_control.hpp" @@ -27,7 +26,7 @@ namespace control { namespace trajectory_follower { -using autoware::common::types::float64_t; + /** * Vehicle model class * @brief calculate model-related values @@ -35,12 +34,12 @@ using autoware::common::types::float64_t; class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface { protected: - const int64_t m_dim_x; //!< @brief dimension of state x - const int64_t m_dim_u; //!< @brief dimension of input u - const int64_t m_dim_y; //!< @brief dimension of output y - float64_t m_velocity; //!< @brief vehicle velocity [m/s] - float64_t m_curvature; //!< @brief curvature on the linearized point on path - float64_t m_wheelbase; //!< @brief wheelbase of the vehicle [m] + const int m_dim_x; //!< @brief dimension of state x + const int m_dim_u; //!< @brief dimension of input u + const int m_dim_y; //!< @brief dimension of output y + double m_velocity; //!< @brief vehicle velocity [m/s] + double m_curvature; //!< @brief curvature on the linearized point on path + double m_wheelbase; //!< @brief wheelbase of the vehicle [m] public: /** @@ -50,7 +49,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface * @param [in] dim_y dimension of output y * @param [in] wheelbase wheelbase of the vehicle [m] */ - VehicleModelInterface(int64_t dim_x, int64_t dim_u, int64_t dim_y, float64_t wheelbase); + VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase); /** * @brief destructor @@ -61,37 +60,37 @@ class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface * @brief get state x dimension * @return state dimension */ - int64_t getDimX(); + int getDimX(); /** * @brief get input u dimension * @return input dimension */ - int64_t getDimU(); + int getDimU(); /** * @brief get output y dimension * @return output dimension */ - int64_t getDimY(); + int getDimY(); /** * @brief get wheelbase of the vehicle * @return wheelbase value [m] */ - float64_t getWheelbase(); + double getWheelbase(); /** * @brief set velocity * @param [in] velocity vehicle velocity */ - void setVelocity(const float64_t velocity); + void setVelocity(const double velocity); /** * @brief set curvature * @param [in] curvature curvature on the linearized point on path */ - void setCurvature(const float64_t curvature); + void setCurvature(const double curvature); /** * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk @@ -103,7 +102,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface */ virtual void calculateDiscreteMatrix( Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const float64_t dt) = 0; + const double dt) = 0; /** * @brief calculate reference input diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml index 86c19a914591c..b6de5384180ec 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower/package.xml @@ -20,7 +20,6 @@ autoware_cmake - autoware_auto_common autoware_auto_control_msgs autoware_auto_geometry autoware_auto_planning_msgs diff --git a/control/trajectory_follower/src/interpolate.cpp b/control/trajectory_follower/src/interpolate.cpp index 8eaa7fab36c4d..3e833dcd8b183 100644 --- a/control/trajectory_follower/src/interpolate.cpp +++ b/control/trajectory_follower/src/interpolate.cpp @@ -32,7 +32,7 @@ namespace trajectory_follower { namespace { -bool8_t isIncrease(const std::vector & x) +bool isIncrease(const std::vector & x) { for (size_t i = 0; i < x.size() - 1; ++i) { if (x.at(i) > x.at(i + 1)) { @@ -42,9 +42,9 @@ bool8_t isIncrease(const std::vector & x) return true; } -bool8_t isValidInput( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index) +bool isValidInput( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index) { if (base_index.empty() || base_value.empty() || return_index.empty()) { std::cerr << "mpc bad index : some vector is empty. base_index: " << base_index.size() @@ -79,9 +79,9 @@ bool8_t isValidInput( } } // namespace -bool8_t linearInterpolate( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index, std::vector & return_value) +bool linearInterpolate( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index, std::vector & return_value) { // check if inputs are valid if (!isValidInput(base_index, base_value, return_index)) { @@ -111,11 +111,11 @@ bool8_t linearInterpolate( continue; } - const float64_t dist_base_idx = base_index.at(i) - base_index.at(i - 1); - const float64_t dist_to_forward = base_index.at(i) - idx; - const float64_t dist_to_backward = idx - base_index.at(i - 1); + const double dist_base_idx = base_index.at(i) - base_index.at(i - 1); + const double dist_to_forward = base_index.at(i) - idx; + const double dist_to_backward = idx - base_index.at(i - 1); - const float64_t value = + const double value = (dist_to_backward * base_value.at(i) + dist_to_forward * base_value.at(i - 1)) / dist_base_idx; return_value.push_back(value); @@ -123,14 +123,14 @@ bool8_t linearInterpolate( return true; } -bool8_t linearInterpolate( - const std::vector & base_index, const std::vector & base_value, - const float64_t & return_index, float64_t & return_value) +bool linearInterpolate( + const std::vector & base_index, const std::vector & base_value, + const double & return_index, double & return_value) { - std::vector return_index_v; + std::vector return_index_v; return_index_v.push_back(return_index); - std::vector return_value_v; + std::vector return_value_v; if (!linearInterpolate(base_index, base_value, return_index_v, return_value_v)) { return false; } diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 6472ad3f43b07..21a79b574c93f 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -61,16 +61,15 @@ bool isValidTrajectory(const Trajectory & traj) return true; } -float64_t calcStopDistance( - const Pose & current_pose, const Trajectory & traj, const float64_t max_dist, - const float64_t max_yaw) +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 float64_t signed_length_on_traj = motion_utils::calcSignedArcLength( + 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)); @@ -80,9 +79,9 @@ float64_t calcStopDistance( return signed_length_on_traj; } -float64_t getPitchByPose(const Quaternion & quaternion_msg) +double getPitchByPose(const Quaternion & quaternion_msg) { - float64_t roll, pitch, yaw; + double roll, pitch, yaw; tf2::Quaternion quaternion; tf2::fromMsg(quaternion_msg, quaternion); tf2::Matrix3x3{quaternion}.getRPY(roll, pitch, yaw); @@ -90,8 +89,8 @@ float64_t getPitchByPose(const Quaternion & quaternion_msg) return pitch; } -float64_t getPitchByTraj( - const Trajectory & trajectory, const size_t nearest_idx, const float64_t wheel_base) +double getPitchByTraj( + const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base) { using autoware::common::geometry::distance_2d; // cannot calculate pitch @@ -100,8 +99,8 @@ float64_t getPitchByTraj( } for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const float64_t dist = - distance_2d(trajectory.points.at(nearest_idx), trajectory.points.at(i)); + 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)); @@ -110,8 +109,7 @@ float64_t getPitchByTraj( // close to goal for (size_t i = trajectory.points.size() - 1; i > 0; --i) { - const float64_t dist = - distance_2d(trajectory.points.back(), trajectory.points.at(i)); + const double dist = distance_2d(trajectory.points.back(), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory @@ -124,26 +122,26 @@ float64_t getPitchByTraj( return calcElevationAngle(trajectory.points.at(0), trajectory.points.back()); } -float64_t calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) +double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) { - const float64_t dx = p_from.pose.position.x - p_to.pose.position.x; - const float64_t dy = p_from.pose.position.y - p_to.pose.position.y; - const float64_t dz = p_from.pose.position.z - p_to.pose.position.z; + const 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 float64_t dxy = std::max(std::hypot(dx, dy), std::numeric_limits::epsilon()); - const float64_t pitch = std::atan2(dz, dxy); + 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 float64_t delay_time, const float64_t current_vel) + const Pose & current_pose, const double delay_time, const double current_vel) { // simple linear prediction - const float64_t yaw = tf2::getYaw(current_pose.orientation); - const float64_t running_distance = delay_time * current_vel; - const float64_t dx = running_distance * std::cos(yaw); - const float64_t dy = running_distance * std::sin(yaw); + 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; @@ -151,13 +149,12 @@ Pose calcPoseAfterTimeDelay( return pred_pose; } -float64_t lerp(const float64_t v_from, const float64_t v_to, const float64_t ratio) +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 float64_t 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); @@ -167,21 +164,21 @@ Quaternion lerpOrientation( return tf2::toMsg(q_interpolated); } -float64_t applyDiffLimitFilter( - const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t max_val, - const float64_t min_val) +double applyDiffLimitFilter( + const double input_val, const double prev_val, const double dt, const double max_val, + const double min_val) { - const float64_t diff_raw = (input_val - prev_val) / dt; - const float64_t diff = std::min(std::max(diff_raw, min_val), max_val); - const float64_t filtered_val = prev_val + diff * dt; + 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; } -float64_t applyDiffLimitFilter( - const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t lim_val) +double applyDiffLimitFilter( + const double input_val, const double prev_val, const double dt, const double lim_val) { - const float64_t max_val = std::fabs(lim_val); - const float64_t min_val = -max_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 diff --git a/control/trajectory_follower/src/lowpass_filter.cpp b/control/trajectory_follower/src/lowpass_filter.cpp index 1f357b258e40f..e45221cf9c6a5 100644 --- a/control/trajectory_follower/src/lowpass_filter.cpp +++ b/control/trajectory_follower/src/lowpass_filter.cpp @@ -24,14 +24,14 @@ namespace control { namespace trajectory_follower { -Butterworth2dFilter::Butterworth2dFilter(float64_t dt, float64_t f_cutoff_hz) +Butterworth2dFilter::Butterworth2dFilter(double dt, double f_cutoff_hz) { initialize(dt, f_cutoff_hz); } Butterworth2dFilter::~Butterworth2dFilter() {} -void Butterworth2dFilter::initialize(const float64_t & dt, const float64_t & f_cutoff_hz) +void Butterworth2dFilter::initialize(const double & dt, const double & f_cutoff_hz) { m_y1 = 0.0; m_y2 = 0.0; @@ -39,8 +39,8 @@ void Butterworth2dFilter::initialize(const float64_t & dt, const float64_t & f_c m_u1 = 0.0; /* 2d butterworth lowpass filter with bi-linear transformation */ - float64_t wc = 2.0 * M_PI * f_cutoff_hz; - float64_t n = 2 / dt; + double wc = 2.0 * M_PI * f_cutoff_hz; + double n = 2 / dt; m_a0 = n * n + sqrt(2) * wc * n + wc * wc; m_a1 = 2 * wc * wc - 2 * n * n; m_a2 = n * n - sqrt(2) * wc * n + wc * wc; @@ -49,9 +49,9 @@ void Butterworth2dFilter::initialize(const float64_t & dt, const float64_t & f_c m_b2 = m_b0; } -float64_t Butterworth2dFilter::filter(const float64_t & u0) +double Butterworth2dFilter::filter(const double & u0) { - float64_t y0 = (m_b2 * m_u2 + m_b1 * m_u1 + m_b0 * u0 - m_a2 * m_y2 - m_a1 * m_y1) / m_a0; + double y0 = (m_b2 * m_u2 + m_b1 * m_u1 + m_b0 * u0 - m_a2 * m_y2 - m_a1 * m_y1) / m_a0; m_y2 = m_y1; m_y1 = y0; m_u2 = m_u1; @@ -59,16 +59,15 @@ float64_t Butterworth2dFilter::filter(const float64_t & u0) return y0; } -void Butterworth2dFilter::filt_vector( - const std::vector & t, std::vector & u) const +void Butterworth2dFilter::filt_vector(const std::vector & t, std::vector & u) const { u.resize(t.size()); - float64_t y1 = t.at(0); - float64_t y2 = t.at(0); - float64_t u2 = t.at(0); - float64_t u1 = t.at(0); - float64_t y0 = 0.0; - float64_t u0 = 0.0; + double y1 = t.at(0); + double y2 = t.at(0); + double u2 = t.at(0); + double u1 = t.at(0); + double y0 = 0.0; + double u0 = 0.0; for (size_t i = 0; i < t.size(); ++i) { u0 = t.at(i); y0 = (m_b2 * u2 + m_b1 * u1 + m_b0 * u0 - m_a2 * y2 - m_a1 * y1) / m_a0; @@ -82,10 +81,10 @@ void Butterworth2dFilter::filt_vector( // filtering forward and backward direction void Butterworth2dFilter::filtfilt_vector( - const std::vector & t, std::vector & u) const + const std::vector & t, std::vector & u) const { - std::vector t_fwd(t); - std::vector t_rev(t); + std::vector t_fwd(t); + std::vector t_rev(t); // forward filtering filt_vector(t, t_fwd); @@ -102,7 +101,7 @@ void Butterworth2dFilter::filtfilt_vector( } } -void Butterworth2dFilter::getCoefficients(std::vector & coeffs) const +void Butterworth2dFilter::getCoefficients(std::vector & coeffs) const { coeffs.clear(); coeffs.push_back(m_a0); @@ -115,25 +114,25 @@ void Butterworth2dFilter::getCoefficients(std::vector & coeffs) const namespace MoveAverageFilter { -bool8_t filt_vector(const int64_t num, std::vector & u) +bool filt_vector(const int num, std::vector & u) { - if (static_cast(u.size()) < num) { + if (static_cast(u.size()) < num) { return false; } - std::vector filtered_u(u); - for (int64_t i = 0; i < static_cast(u.size()); ++i) { - float64_t tmp = 0.0; - int64_t num_tmp = 0; - float64_t count = 0; + std::vector filtered_u(u); + for (int i = 0; i < static_cast(u.size()); ++i) { + double tmp = 0.0; + int num_tmp = 0; + double count = 0; if (i - num < 0) { num_tmp = i; - } else if (i + num > static_cast(u.size()) - 1) { - num_tmp = static_cast(u.size()) - i - 1; + } else if (i + num > static_cast(u.size()) - 1) { + num_tmp = static_cast(u.size()) - i - 1; } else { num_tmp = num; } - for (int64_t j = -num_tmp; j <= num_tmp; ++j) { + for (int j = -num_tmp; j <= num_tmp; ++j) { tmp += u[static_cast(i + j)]; ++count; } diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index 3dbc891c7782f..050727c40a50d 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -37,9 +37,9 @@ namespace trajectory_follower { using namespace std::literals::chrono_literals; -bool8_t MPC::calculateMPC( +bool MPC::calculateMPC( const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, - const float64_t current_velocity, const geometry_msgs::msg::Pose & current_pose, + const double current_velocity, const geometry_msgs::msg::Pose & current_pose, autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd, autoware_auto_planning_msgs::msg::Trajectory & predicted_traj, tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic) @@ -67,8 +67,8 @@ bool8_t MPC::calculateMPC( /* resample ref_traj with mpc sampling time */ trajectory_follower::MPCTrajectory mpc_resampled_ref_traj; - const float64_t mpc_start_time = mpc_data.nearest_time + m_param.input_delay; - const float64_t prediction_dt = + 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); if (!resampleMPCTrajectoryByTime( mpc_start_time, prediction_dt, reference_trajectory, &mpc_resampled_ref_traj)) { @@ -87,8 +87,8 @@ bool8_t MPC::calculateMPC( } /* apply saturation and filter */ - const float64_t u_saturated = std::max(std::min(Uex(0), m_steer_lim), -m_steer_lim); - const float64_t u_filtered = m_lpf_steering_cmd.filter(u_saturated); + const double u_saturated = std::max(std::min(Uex(0), m_steer_lim), -m_steer_lim); + const double u_filtered = m_lpf_steering_cmd.filter(u_saturated); /* set control command */ { @@ -110,27 +110,27 @@ bool8_t MPC::calculateMPC( trajectory_follower::MPCTrajectory mpc_predicted_traj; const auto & traj = mpc_resampled_ref_traj; for (size_t i = 0; i < static_cast(m_param.prediction_horizon); ++i) { - const int64_t DIM_X = m_vehicle_model_ptr->getDimX(); - const float64_t lat_error = Xex(static_cast(i) * DIM_X); - const float64_t yaw_error = Xex(static_cast(i) * DIM_X + 1); - const float64_t x = traj.x[i] - std::sin(traj.yaw[i]) * lat_error; - const float64_t y = traj.y[i] + std::cos(traj.yaw[i]) * lat_error; - const float64_t z = traj.z[i]; - const float64_t yaw = traj.yaw[i] + yaw_error; - const float64_t vx = traj.vx[i]; - const float64_t k = traj.k[i]; - const float64_t smooth_k = traj.smooth_k[i]; - const float64_t relative_time = traj.relative_time[i]; + const int DIM_X = m_vehicle_model_ptr->getDimX(); + const double lat_error = Xex(static_cast(i) * DIM_X); + const double yaw_error = Xex(static_cast(i) * DIM_X + 1); + const double x = traj.x[i] - std::sin(traj.yaw[i]) * lat_error; + const double y = traj.y[i] + std::cos(traj.yaw[i]) * lat_error; + const double z = traj.z[i]; + const double yaw = traj.yaw[i] + yaw_error; + const double vx = traj.vx[i]; + const double k = traj.k[i]; + const double smooth_k = traj.smooth_k[i]; + 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); /* prepare diagnostic message */ - const float64_t nearest_k = reference_trajectory.k[static_cast(mpc_data.nearest_idx)]; - const float64_t nearest_smooth_k = + const double nearest_k = reference_trajectory.k[static_cast(mpc_data.nearest_idx)]; + const double nearest_smooth_k = reference_trajectory.smooth_k[static_cast(mpc_data.nearest_idx)]; - const float64_t steer_cmd = ctrl_cmd.steering_tire_angle; - const float64_t wb = m_vehicle_model_ptr->getWheelbase(); + const double steer_cmd = ctrl_cmd.steering_tire_angle; + const double wb = m_vehicle_model_ptr->getWheelbase(); typedef decltype(diagnostic.data)::value_type DiagnosticValueType; auto append_diag_data = [&](const auto & val) -> void { @@ -178,9 +178,9 @@ bool8_t MPC::calculateMPC( void MPC::setReferenceTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, - const float64_t traj_resample_dist, const bool8_t enable_path_smoothing, - const int64_t path_filter_moving_ave_num, const int64_t curvature_smoothing_num_traj, - const int64_t curvature_smoothing_num_ref_steer) + const double traj_resample_dist, const bool enable_path_smoothing, + const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, + const int curvature_smoothing_num_ref_steer) { trajectory_follower::MPCTrajectory mpc_traj_raw; // received raw trajectory trajectory_follower::MPCTrajectory mpc_traj_resampled; // resampled trajectory @@ -201,7 +201,7 @@ void MPC::setReferenceTrajectory( /* path smoothing */ mpc_traj_smoothed = mpc_traj_resampled; - const int64_t mpc_traj_resampled_size = static_cast(mpc_traj_resampled.size()); + 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( @@ -229,9 +229,9 @@ void MPC::setReferenceTrajectory( /* add end point with vel=0 on traj for mpc prediction */ { auto & t = mpc_traj_smoothed; - const float64_t t_ext = 100.0; // extra time to prevent mpc calc failure due to short time - const float64_t t_end = t.relative_time.back() + t_ext; - const float64_t v_end = 0.0; + const double t_ext = 100.0; // extra time to prevent mpc calc failure due to short time + const double t_end = t.relative_time.back() + t_ext; + const double v_end = 0.0; t.vx.back() = v_end; // set for end point t.push_back( t.x.back(), t.y.back(), t.z.back(), t.yaw.back(), v_end, t.k.back(), t.smooth_k.back(), @@ -252,7 +252,7 @@ void MPC::resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport m_raw_steer_cmd_pprev = current_steer.steering_tire_angle; } -bool8_t MPC::getData( +bool MPC::getData( const trajectory_follower::MPCTrajectory & traj, const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, const geometry_msgs::msg::Pose & current_pose, MPCData * data) @@ -274,8 +274,8 @@ bool8_t MPC::getData( } /* get data */ - data->nearest_idx = static_cast(nearest_idx); - data->steer = static_cast(current_steer.steering_tire_angle); + data->nearest_idx = static_cast(nearest_idx); + data->steer = static_cast(current_steer.steering_tire_angle); data->lateral_err = trajectory_follower::MPCUtils::calcLateralError(current_pose, data->nearest_pose); data->yaw_err = autoware::common::helper_functions::wrap_angle( @@ -283,13 +283,13 @@ bool8_t MPC::getData( /* get predicted steer */ if (!m_steer_prediction_prev) { - m_steer_prediction_prev = std::make_shared(current_steer.steering_tire_angle); + m_steer_prediction_prev = std::make_shared(current_steer.steering_tire_angle); } data->predicted_steer = calcSteerPrediction(); *m_steer_prediction_prev = data->predicted_steer; /* check error limit */ - const float64_t dist_err = autoware::common::geometry::distance_2d( + const double dist_err = autoware::common::geometry::distance_2d( current_pose.position, data->nearest_pose.position); if (dist_err > m_admissible_position_error) { RCLCPP_WARN_SKIPFIRST_THROTTLE( @@ -307,8 +307,8 @@ bool8_t MPC::getData( } /* check trajectory time length */ - const float64_t max_prediction_time = - m_param.min_prediction_length / static_cast(m_param.prediction_horizon - 1); + const double max_prediction_time = + m_param.min_prediction_length / static_cast(m_param.prediction_horizon - 1); auto end_time = data->nearest_time + m_param.input_delay + m_ctrl_period + max_prediction_time; if (end_time > traj.relative_time.back()) { RCLCPP_WARN_SKIPFIRST_THROTTLE( @@ -318,17 +318,16 @@ bool8_t MPC::getData( return true; } -float64_t MPC::calcSteerPrediction() +double MPC::calcSteerPrediction() { auto t_start = m_time_prev; auto t_end = m_clock->now(); m_time_prev = t_end; - const float64_t duration = (t_end - t_start).seconds(); - const float64_t time_constant = m_param.steer_tau; + const double duration = (t_end - t_start).seconds(); + const double time_constant = m_param.steer_tau; - const float64_t initial_response = - std::exp(-duration / time_constant) * (*m_steer_prediction_prev); + const double initial_response = std::exp(-duration / time_constant) * (*m_steer_prediction_prev); if (m_ctrl_cmd_vec.size() <= 2) { return initial_response; @@ -337,8 +336,8 @@ float64_t MPC::calcSteerPrediction() return initial_response + getSteerCmdSum(t_start, t_end, time_constant); } -float64_t MPC::getSteerCmdSum( - const rclcpp::Time & t_start, const rclcpp::Time & t_end, const float64_t time_constant) const +double MPC::getSteerCmdSum( + const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const { if (m_ctrl_cmd_vec.size() <= 2) { return 0.0; @@ -354,27 +353,27 @@ float64_t MPC::getSteerCmdSum( } // Compute steer command input response - float64_t steer_sum = 0.0; + double steer_sum = 0.0; auto t = t_start; while (t_end > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) { - const float64_t duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds(); + const double duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds(); t = rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp); steer_sum += (1 - std::exp(-duration / time_constant)) * - static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); + static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); ++idx; if (idx >= m_ctrl_cmd_vec.size()) { break; } } - const float64_t duration = (t_end - t).seconds(); + const double duration = (t_end - t).seconds(); steer_sum += (1 - std::exp(-duration / time_constant)) * - static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); + static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); return steer_sum; } -void MPC::storeSteerCmd(const float64_t steer) +void MPC::storeSteerCmd(const double steer) { const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_param.input_delay); autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; @@ -389,19 +388,18 @@ void MPC::storeSteerCmd(const float64_t steer) } // remove unused ctrl cmd - constexpr float64_t store_time = 0.3; + constexpr double store_time = 0.3; if ((time_delayed - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_param.input_delay + store_time) { m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); } } -bool8_t MPC::resampleMPCTrajectoryByTime( - const float64_t ts, const float64_t prediction_dt, - const trajectory_follower::MPCTrajectory & input, +bool MPC::resampleMPCTrajectoryByTime( + const double ts, const double prediction_dt, const trajectory_follower::MPCTrajectory & input, trajectory_follower::MPCTrajectory * output) const { - std::vector mpc_time_v; - for (float64_t i = 0; i < static_cast(m_param.prediction_horizon); ++i) { + 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( @@ -416,7 +414,7 @@ bool8_t MPC::resampleMPCTrajectoryByTime( Eigen::VectorXd MPC::getInitialState(const MPCData & data) { - const int64_t DIM_X = m_vehicle_model_ptr->getDimX(); + const int DIM_X = m_vehicle_model_ptr->getDimX(); Eigen::VectorXd x0 = Eigen::VectorXd::Zero(DIM_X); const auto & lat_err = data.lateral_err; @@ -428,8 +426,8 @@ Eigen::VectorXd MPC::getInitialState(const MPCData & data) } else if (m_vehicle_model_type == "kinematics_no_delay") { x0 << lat_err, yaw_err; } else if (m_vehicle_model_type == "dynamics") { - float64_t dlat = (lat_err - m_lateral_error_prev) / m_ctrl_period; - float64_t dyaw = (yaw_err - m_yaw_error_prev) / m_ctrl_period; + double dlat = (lat_err - m_lateral_error_prev) / m_ctrl_period; + double dyaw = (yaw_err - m_yaw_error_prev) / m_ctrl_period; m_lateral_error_prev = lat_err; m_yaw_error_prev = yaw_err; dlat = m_lpf_lateral_error.filter(dlat); @@ -443,13 +441,12 @@ Eigen::VectorXd MPC::getInitialState(const MPCData & data) return x0; } -bool8_t MPC::updateStateForDelayCompensation( - const trajectory_follower::MPCTrajectory & traj, const float64_t & start_time, - Eigen::VectorXd * x) +bool MPC::updateStateForDelayCompensation( + const trajectory_follower::MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x) { - const int64_t DIM_X = m_vehicle_model_ptr->getDimX(); - const int64_t DIM_U = m_vehicle_model_ptr->getDimU(); - const int64_t DIM_Y = m_vehicle_model_ptr->getDimY(); + const int DIM_X = m_vehicle_model_ptr->getDimX(); + const int DIM_U = m_vehicle_model_ptr->getDimU(); + const int DIM_Y = m_vehicle_model_ptr->getDimY(); Eigen::MatrixXd Ad(DIM_X, DIM_X); Eigen::MatrixXd Bd(DIM_X, DIM_U); @@ -457,10 +454,10 @@ bool8_t MPC::updateStateForDelayCompensation( Eigen::MatrixXd Cd(DIM_Y, DIM_X); Eigen::MatrixXd x_curr = *x; - float64_t mpc_curr_time = start_time; - for (uint64_t i = 0; i < m_input_buffer.size(); ++i) { - float64_t k = 0.0; - float64_t v = 0.0; + double mpc_curr_time = start_time; + for (uint i = 0; i < m_input_buffer.size(); ++i) { + 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)) { @@ -484,7 +481,7 @@ bool8_t MPC::updateStateForDelayCompensation( trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( const trajectory_follower::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose, - const float64_t v0) const + const double v0) const { autoware_auto_planning_msgs::msg::Trajectory autoware_traj; autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( @@ -496,15 +493,15 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - const float64_t acc_lim = m_param.acceleration_limit; - const float64_t tau = m_param.velocity_time_constant; + 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( static_cast(nearest_idx), v0, acc_lim, tau, output); - const float64_t t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time - const float64_t t_end = output.relative_time.back() + t_ext; - const float64_t v_end = 0.0; + 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; + const double v_end = 0.0; output.vx.back() = v_end; // set for end point output.push_back( output.x.back(), output.y.back(), output.z.back(), output.yaw.back(), v_end, output.k.back(), @@ -518,15 +515,15 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) */ MPCMatrix MPC::generateMPCMatrix( - const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t prediction_dt) + const trajectory_follower::MPCTrajectory & reference_trajectory, const double prediction_dt) { using Eigen::MatrixXd; - const int64_t N = m_param.prediction_horizon; - const float64_t DT = prediction_dt; - const int64_t DIM_X = m_vehicle_model_ptr->getDimX(); - const int64_t DIM_U = m_vehicle_model_ptr->getDimU(); - const int64_t DIM_Y = m_vehicle_model_ptr->getDimY(); + const int N = m_param.prediction_horizon; + const double DT = prediction_dt; + const int DIM_X = m_vehicle_model_ptr->getDimX(); + const int DIM_U = m_vehicle_model_ptr->getDimU(); + const int DIM_Y = m_vehicle_model_ptr->getDimY(); MPCMatrix m; m.Aex = MatrixXd::Zero(DIM_X * N, DIM_X); @@ -550,15 +547,15 @@ MPCMatrix MPC::generateMPCMatrix( MatrixXd Cd(DIM_Y, DIM_X); MatrixXd Uref(DIM_U, 1); - const float64_t sign_vx = m_is_forward_shift ? 1 : -1; + const double sign_vx = m_is_forward_shift ? 1 : -1; /* predict dynamics for N times */ - for (int64_t i = 0; i < N; ++i) { - const float64_t ref_vx = reference_trajectory.vx[static_cast(i)]; - const float64_t ref_vx_squared = ref_vx * ref_vx; + for (int i = 0; i < N; ++i) { + const double ref_vx = reference_trajectory.vx[static_cast(i)]; + const double ref_vx_squared = ref_vx * ref_vx; - const float64_t ref_k = reference_trajectory.k[static_cast(i)] * sign_vx; - const float64_t ref_smooth_k = reference_trajectory.smooth_k[static_cast(i)] * sign_vx; + const double ref_k = reference_trajectory.k[static_cast(i)] * sign_vx; + const double ref_smooth_k = reference_trajectory.smooth_k[static_cast(i)] * sign_vx; /* get discrete state matrix A, B, C, W */ m_vehicle_model_ptr->setVelocity(ref_vx); @@ -581,18 +578,18 @@ MPCMatrix MPC::generateMPCMatrix( R_adaptive(0, 0) += ref_vx_squared * getWeightSteerInputSqVel(ref_k); /* update mpc matrix */ - int64_t idx_x_i = i * DIM_X; - int64_t idx_x_i_prev = (i - 1) * DIM_X; - int64_t idx_u_i = i * DIM_U; - int64_t idx_y_i = i * DIM_Y; + int idx_x_i = i * DIM_X; + int idx_x_i_prev = (i - 1) * DIM_X; + int idx_u_i = i * DIM_U; + int idx_y_i = i * DIM_Y; if (i == 0) { m.Aex.block(0, 0, DIM_X, DIM_X) = Ad; m.Bex.block(0, 0, DIM_X, DIM_U) = Bd; m.Wex.block(0, 0, DIM_X, 1) = Wd; } else { m.Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * m.Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X); - for (int64_t j = 0; j < i; ++j) { - int64_t idx_u_j = j * DIM_U; + for (int j = 0; j < i; ++j) { + int idx_u_j = j * DIM_U; m.Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) = Ad * m.Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U); } @@ -613,10 +610,10 @@ MPCMatrix MPC::generateMPCMatrix( } /* add lateral jerk : weight for (v * {u(i) - u(i-1)} )^2 */ - for (int64_t i = 0; i < N - 1; ++i) { - const float64_t ref_vx = reference_trajectory.vx[static_cast(i)]; - const float64_t ref_k = reference_trajectory.k[static_cast(i)] * sign_vx; - const float64_t j = ref_vx * ref_vx * getWeightLatJerk(ref_k) / (DT * DT); + for (int i = 0; i < N - 1; ++i) { + const double ref_vx = reference_trajectory.vx[static_cast(i)]; + const double ref_k = reference_trajectory.k[static_cast(i)] * sign_vx; + const double j = ref_vx * ref_vx * getWeightLatJerk(ref_k) / (DT * DT); const Eigen::Matrix2d J = (Eigen::Matrix2d() << j, -j, -j, j).finished(); m.R2ex.block(i, i, 2, 2) += J; } @@ -648,8 +645,8 @@ MPCMatrix MPC::generateMPCMatrix( * ~~~ * [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U) */ -bool8_t MPC::executeOptimization( - const MPCMatrix & m, const Eigen::VectorXd & x0, const float64_t prediction_dt, +bool MPC::executeOptimization( + const MPCMatrix & m, const Eigen::VectorXd & x0, const double prediction_dt, Eigen::VectorXd * Uex) { using Eigen::MatrixXd; @@ -661,7 +658,7 @@ bool8_t MPC::executeOptimization( return false; } - const int64_t DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU(); + const int DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU(); // cost function: 1/2 * Uex' * H * Uex + f' * Uex, H = B' * C' * Q * C * B + R const MatrixXd CB = m.Cex * m.Bex; @@ -676,7 +673,7 @@ bool8_t MPC::executeOptimization( addSteerWeightF(prediction_dt, &f); MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N); - for (int64_t i = 1; i < DIM_U_N; i++) { + for (int i = 1; i < DIM_U_N; i++) { A(i, i - 1) = -1.0; } @@ -688,7 +685,7 @@ bool8_t MPC::executeOptimization( ubA(0, 0) = m_raw_steer_cmd_prev + m_steer_rate_lim * m_ctrl_period; auto t_start = std::chrono::system_clock::now(); - bool8_t solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, *Uex); + bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, *Uex); auto t_end = std::chrono::system_clock::now(); if (!solve_result) { RCLCPP_WARN_SKIPFIRST_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "qp solver error"); @@ -708,18 +705,18 @@ bool8_t MPC::executeOptimization( return true; } -void MPC::addSteerWeightR(const float64_t prediction_dt, Eigen::MatrixXd * R_ptr) const +void MPC::addSteerWeightR(const double prediction_dt, Eigen::MatrixXd * R_ptr) const { - const int64_t N = m_param.prediction_horizon; - const float64_t DT = prediction_dt; + const int N = m_param.prediction_horizon; + const double DT = prediction_dt; auto & R = *R_ptr; /* add steering rate : weight for (u(i) - u(i-1) / dt )^2 */ { - const float64_t steer_rate_r = m_param.weight_steer_rate / (DT * DT); + const double steer_rate_r = m_param.weight_steer_rate / (DT * DT); const Eigen::Matrix2d D = steer_rate_r * (Eigen::Matrix2d() << 1.0, -1.0, -1.0, 1.0).finished(); - for (int64_t i = 0; i < N - 1; ++i) { + for (int i = 0; i < N - 1; ++i) { R.block(i, i, 2, 2) += D; } if (N > 1) { @@ -730,15 +727,15 @@ void MPC::addSteerWeightR(const float64_t prediction_dt, Eigen::MatrixXd * R_ptr /* add steering acceleration : weight for { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2 */ { - const float64_t w = m_param.weight_steer_acc; - const float64_t steer_acc_r = w / std::pow(DT, 4); - const float64_t steer_acc_r_cp1 = w / (std::pow(DT, 3) * m_ctrl_period); - const float64_t steer_acc_r_cp2 = w / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2)); - const float64_t steer_acc_r_cp4 = w / std::pow(m_ctrl_period, 4); + const double w = m_param.weight_steer_acc; + const double steer_acc_r = w / std::pow(DT, 4); + const double steer_acc_r_cp1 = w / (std::pow(DT, 3) * m_ctrl_period); + const double steer_acc_r_cp2 = w / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2)); + const double steer_acc_r_cp4 = w / std::pow(m_ctrl_period, 4); const Eigen::Matrix3d D = steer_acc_r * (Eigen::Matrix3d() << 1.0, -2.0, 1.0, -2.0, 4.0, -2.0, 1.0, -2.0, 1.0).finished(); - for (int64_t i = 1; i < N - 1; ++i) { + for (int i = 1; i < N - 1; ++i) { R.block(i - 1, i - 1, 3, 3) += D; } if (N > 1) { @@ -753,23 +750,23 @@ void MPC::addSteerWeightR(const float64_t prediction_dt, Eigen::MatrixXd * R_ptr } } -void MPC::addSteerWeightF(const float64_t prediction_dt, Eigen::MatrixXd * f_ptr) const +void MPC::addSteerWeightF(const double prediction_dt, Eigen::MatrixXd * f_ptr) const { if (f_ptr->rows() < 2) { return; } - const float64_t DT = prediction_dt; + const double DT = prediction_dt; auto & f = *f_ptr; // steer rate for i = 0 f(0, 0) += -2.0 * m_param.weight_steer_rate / (std::pow(DT, 2)) * 0.5; - // const float64_t steer_acc_r = m_param.weight_steer_acc / std::pow(DT, 4); - const float64_t steer_acc_r_cp1 = m_param.weight_steer_acc / (std::pow(DT, 3) * m_ctrl_period); - const float64_t steer_acc_r_cp2 = + // const double steer_acc_r = m_param.weight_steer_acc / std::pow(DT, 4); + const double steer_acc_r_cp1 = m_param.weight_steer_acc / (std::pow(DT, 3) * m_ctrl_period); + const double steer_acc_r_cp2 = m_param.weight_steer_acc / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2)); - const float64_t steer_acc_r_cp4 = m_param.weight_steer_acc / std::pow(m_ctrl_period, 4); + const double steer_acc_r_cp4 = m_param.weight_steer_acc / std::pow(m_ctrl_period, 4); // steer acc i = 0 f(0, 0) += ((-2.0 * m_raw_steer_cmd_prev + m_raw_steer_cmd_pprev) * steer_acc_r_cp4) * 0.5; @@ -779,8 +776,8 @@ void MPC::addSteerWeightF(const float64_t prediction_dt, Eigen::MatrixXd * f_ptr f(0, 1) += (2.0 * m_raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5; } -float64_t MPC::getPredictionDeltaTime( - const float64_t start_time, const trajectory_follower::MPCTrajectory & input, +double MPC::getPredictionDeltaTime( + const double start_time, const trajectory_follower::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose) const { // Calculate the time min_prediction_length ahead from current_pose @@ -789,20 +786,19 @@ float64_t MPC::getPredictionDeltaTime( input, autoware_traj); const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - float64_t sum_dist = 0; - const float64_t target_time = [&]() { - const float64_t t_ext = - 100.0; // extra time to prevent mpc calculation failure due to short time + double sum_dist = 0; + const double target_time = [&]() { + const double t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) { - const float64_t segment_dist = + const double segment_dist = std::hypot(input.x.at(i) - input.x.at(i - 1), input.y.at(i) - input.y.at(i - 1)); sum_dist += segment_dist; if (m_param.min_prediction_length < sum_dist) { - const float64_t prev_sum_dist = sum_dist - segment_dist; - const float64_t ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist; - const float64_t relative_time_at_i = i == input.relative_time.size() - 1 - ? input.relative_time.at(i) - t_ext - : input.relative_time.at(i); + const double prev_sum_dist = sum_dist - segment_dist; + const double ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist; + const double relative_time_at_i = i == input.relative_time.size() - 1 + ? input.relative_time.at(i) - t_ext + : input.relative_time.at(i); return input.relative_time.at(i - 1) + (relative_time_at_i - input.relative_time.at(i - 1)) * ratio; } @@ -811,15 +807,15 @@ float64_t MPC::getPredictionDeltaTime( }(); // Calculate delta time for min_prediction_length - const float64_t dt = - (target_time - start_time) / static_cast(m_param.prediction_horizon - 1); + const double dt = + (target_time - start_time) / static_cast(m_param.prediction_horizon - 1); return std::max(dt, m_param.prediction_dt); } -float64_t MPC::calcDesiredSteeringRate( +double MPC::calcDesiredSteeringRate( const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const float64_t u_filtered, const float current_steer, const float64_t predict_dt) const + const double u_filtered, const float current_steer, const double predict_dt) const { if (m_vehicle_model_type != "kinematics") { // not supported yet. Use old implementation. @@ -840,7 +836,7 @@ float64_t MPC::calcDesiredSteeringRate( return steer_rate; } -bool8_t MPC::isValid(const MPCMatrix & m) const +bool MPC::isValid(const MPCMatrix & m) const { if ( m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() || diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index fa158d66b1b44..a90fa0fd61da6 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -55,35 +55,32 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} using std::placeholders::_1; m_mpc.m_ctrl_period = node_->get_parameter("ctrl_period").as_double(); - m_enable_path_smoothing = node_->declare_parameter("enable_path_smoothing"); - m_path_filter_moving_ave_num = node_->declare_parameter("path_filter_moving_ave_num"); - m_curvature_smoothing_num_traj = - node_->declare_parameter("curvature_smoothing_num_traj"); + m_enable_path_smoothing = node_->declare_parameter("enable_path_smoothing"); + m_path_filter_moving_ave_num = node_->declare_parameter("path_filter_moving_ave_num"); + m_curvature_smoothing_num_traj = node_->declare_parameter("curvature_smoothing_num_traj"); m_curvature_smoothing_num_ref_steer = - node_->declare_parameter("curvature_smoothing_num_ref_steer"); - m_traj_resample_dist = node_->declare_parameter("traj_resample_dist"); - m_mpc.m_admissible_position_error = - node_->declare_parameter("admissible_position_error"); - m_mpc.m_admissible_yaw_error_rad = - node_->declare_parameter("admissible_yaw_error_rad"); - m_mpc.m_use_steer_prediction = node_->declare_parameter("use_steer_prediction"); - m_mpc.m_param.steer_tau = node_->declare_parameter("vehicle_model_steer_tau"); + node_->declare_parameter("curvature_smoothing_num_ref_steer"); + m_traj_resample_dist = node_->declare_parameter("traj_resample_dist"); + m_mpc.m_admissible_position_error = node_->declare_parameter("admissible_position_error"); + m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter("admissible_yaw_error_rad"); + m_mpc.m_use_steer_prediction = node_->declare_parameter("use_steer_prediction"); + m_mpc.m_param.steer_tau = node_->declare_parameter("vehicle_model_steer_tau"); /* stop state parameters */ - m_stop_state_entry_ego_speed = node_->declare_parameter("stop_state_entry_ego_speed"); + m_stop_state_entry_ego_speed = node_->declare_parameter("stop_state_entry_ego_speed"); m_stop_state_entry_target_speed = - node_->declare_parameter("stop_state_entry_target_speed"); - m_converged_steer_rad = node_->declare_parameter("converged_steer_rad"); + node_->declare_parameter("stop_state_entry_target_speed"); + m_converged_steer_rad = node_->declare_parameter("converged_steer_rad"); m_keep_steer_control_until_converged = - node_->declare_parameter("keep_steer_control_until_converged"); - m_new_traj_duration_time = node_->declare_parameter("new_traj_duration_time"); // [s] - m_new_traj_end_dist = node_->declare_parameter("new_traj_end_dist"); // [m] + node_->declare_parameter("keep_steer_control_until_converged"); + m_new_traj_duration_time = node_->declare_parameter("new_traj_duration_time"); // [s] + m_new_traj_end_dist = node_->declare_parameter("new_traj_end_dist"); // [m] /* mpc parameters */ const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); - const float64_t wheelbase = vehicle_info.wheel_base_m; - const float64_t steer_rate_lim_dps = node_->declare_parameter("steer_rate_lim_dps"); - constexpr float64_t deg2rad = static_cast(autoware::common::types::PI) / 180.0; + const double wheelbase = vehicle_info.wheel_base_m; + const double steer_rate_lim_dps = node_->declare_parameter("steer_rate_lim_dps"); + constexpr double deg2rad = static_cast(M_PI) / 180.0; m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad; m_mpc.m_steer_rate_lim = steer_rate_lim_dps * deg2rad; @@ -98,12 +95,12 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} vehicle_model_ptr = std::make_shared( wheelbase, m_mpc.m_steer_lim); } else if (vehicle_model_type == "dynamics") { - const float64_t mass_fl = node_->declare_parameter("vehicle.mass_fl"); - const float64_t mass_fr = node_->declare_parameter("vehicle.mass_fr"); - const float64_t mass_rl = node_->declare_parameter("vehicle.mass_rl"); - const float64_t mass_rr = node_->declare_parameter("vehicle.mass_rr"); - const float64_t cf = node_->declare_parameter("vehicle.cf"); - const float64_t cr = node_->declare_parameter("vehicle.cr"); + const double mass_fl = node_->declare_parameter("vehicle.mass_fl"); + const double mass_fr = node_->declare_parameter("vehicle.mass_fr"); + const double mass_rl = node_->declare_parameter("vehicle.mass_rl"); + const double mass_rr = node_->declare_parameter("vehicle.mass_rr"); + const double cf = node_->declare_parameter("vehicle.cf"); + const double cr = node_->declare_parameter("vehicle.cr"); // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time // // NOLINT @@ -126,18 +123,18 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* delay compensation */ { - const float64_t delay_tmp = node_->declare_parameter("input_delay"); - const float64_t delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); + const double delay_tmp = node_->declare_parameter("input_delay"); + const double delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); } /* initialize lowpass filter */ { - const float64_t steering_lpf_cutoff_hz = - node_->declare_parameter("steering_lpf_cutoff_hz"); - const float64_t error_deriv_lpf_cutoff_hz = - node_->declare_parameter("error_deriv_lpf_cutoff_hz"); + const double steering_lpf_cutoff_hz = + node_->declare_parameter("steering_lpf_cutoff_hz"); + const double error_deriv_lpf_cutoff_hz = + node_->declare_parameter("error_deriv_lpf_cutoff_hz"); m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); } @@ -189,7 +186,7 @@ boost::optional MpcLateralController::run() m_is_ctrl_cmd_prev_initialized = true; } - const bool8_t is_mpc_solved = m_mpc.calculateMPC( + const bool is_mpc_solved = m_mpc.calculateMPC( *m_current_steering_ptr, m_current_kinematic_state_ptr->twist.twist.linear.x, m_current_kinematic_state_ptr->pose.pose, ctrl_cmd, predicted_traj, debug_values); @@ -247,7 +244,7 @@ bool MpcLateralController::isSteerConverged( return is_converged; } -bool8_t MpcLateralController::checkData() const +bool MpcLateralController::checkData() const { if (!m_mpc.hasVehicleModel()) { RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a vehicle model"); @@ -312,8 +309,8 @@ void MpcLateralController::setTrajectory( const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) - rclcpp::Time(m_trajectory_buffer.front().header.stamp); - const float64_t first_trajectory_duration_time = 5.0; - const float64_t duration_time = + const double first_trajectory_duration_time = 5.0; + const double duration_time = m_has_received_first_trajectory ? m_new_traj_duration_time : first_trajectory_duration_time; if (time_diff.seconds() < duration_time) { m_has_received_first_trajectory = true; @@ -341,7 +338,7 @@ MpcLateralController::getInitialControlCommand() const return cmd; } -bool8_t MpcLateralController::isStoppedState() const +bool MpcLateralController::isStoppedState() const { // If the nearest index is not found, return false if (m_current_trajectory_ptr->points.empty()) { @@ -356,8 +353,8 @@ bool8_t MpcLateralController::isStoppedState() const m_current_trajectory_ptr->points, m_current_kinematic_state_ptr->pose.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - const float64_t current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x; - const float64_t target_vel = + const double current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x; + const double target_vel = m_current_trajectory_ptr->points.at(static_cast(nearest)).longitudinal_velocity_mps; const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command @@ -399,48 +396,47 @@ void MpcLateralController::publishDebugValues( void MpcLateralController::declareMPCparameters() { - m_mpc.m_param.prediction_horizon = node_->declare_parameter("mpc_prediction_horizon"); - m_mpc.m_param.prediction_dt = node_->declare_parameter("mpc_prediction_dt"); - m_mpc.m_param.weight_lat_error = node_->declare_parameter("mpc_weight_lat_error"); - m_mpc.m_param.weight_heading_error = - node_->declare_parameter("mpc_weight_heading_error"); + m_mpc.m_param.prediction_horizon = node_->declare_parameter("mpc_prediction_horizon"); + m_mpc.m_param.prediction_dt = node_->declare_parameter("mpc_prediction_dt"); + m_mpc.m_param.weight_lat_error = node_->declare_parameter("mpc_weight_lat_error"); + m_mpc.m_param.weight_heading_error = node_->declare_parameter("mpc_weight_heading_error"); m_mpc.m_param.weight_heading_error_squared_vel = - node_->declare_parameter("mpc_weight_heading_error_squared_vel"); + node_->declare_parameter("mpc_weight_heading_error_squared_vel"); m_mpc.m_param.weight_steering_input = - node_->declare_parameter("mpc_weight_steering_input"); + node_->declare_parameter("mpc_weight_steering_input"); m_mpc.m_param.weight_steering_input_squared_vel = - node_->declare_parameter("mpc_weight_steering_input_squared_vel"); - m_mpc.m_param.weight_lat_jerk = node_->declare_parameter("mpc_weight_lat_jerk"); - m_mpc.m_param.weight_steer_rate = node_->declare_parameter("mpc_weight_steer_rate"); - m_mpc.m_param.weight_steer_acc = node_->declare_parameter("mpc_weight_steer_acc"); + node_->declare_parameter("mpc_weight_steering_input_squared_vel"); + m_mpc.m_param.weight_lat_jerk = node_->declare_parameter("mpc_weight_lat_jerk"); + m_mpc.m_param.weight_steer_rate = node_->declare_parameter("mpc_weight_steer_rate"); + m_mpc.m_param.weight_steer_acc = node_->declare_parameter("mpc_weight_steer_acc"); m_mpc.m_param.low_curvature_weight_lat_error = - node_->declare_parameter("mpc_low_curvature_weight_lat_error"); + node_->declare_parameter("mpc_low_curvature_weight_lat_error"); m_mpc.m_param.low_curvature_weight_heading_error = - node_->declare_parameter("mpc_low_curvature_weight_heading_error"); + node_->declare_parameter("mpc_low_curvature_weight_heading_error"); m_mpc.m_param.low_curvature_weight_heading_error_squared_vel = - node_->declare_parameter("mpc_low_curvature_weight_heading_error_squared_vel"); + node_->declare_parameter("mpc_low_curvature_weight_heading_error_squared_vel"); m_mpc.m_param.low_curvature_weight_steering_input = - node_->declare_parameter("mpc_low_curvature_weight_steering_input"); + node_->declare_parameter("mpc_low_curvature_weight_steering_input"); m_mpc.m_param.low_curvature_weight_steering_input_squared_vel = - node_->declare_parameter("mpc_low_curvature_weight_steering_input_squared_vel"); + node_->declare_parameter("mpc_low_curvature_weight_steering_input_squared_vel"); m_mpc.m_param.low_curvature_weight_lat_jerk = - node_->declare_parameter("mpc_low_curvature_weight_lat_jerk"); + node_->declare_parameter("mpc_low_curvature_weight_lat_jerk"); m_mpc.m_param.low_curvature_weight_steer_rate = - node_->declare_parameter("mpc_low_curvature_weight_steer_rate"); + node_->declare_parameter("mpc_low_curvature_weight_steer_rate"); m_mpc.m_param.low_curvature_weight_steer_acc = - node_->declare_parameter("mpc_low_curvature_weight_steer_acc"); + node_->declare_parameter("mpc_low_curvature_weight_steer_acc"); m_mpc.m_param.low_curvature_thresh_curvature = - node_->declare_parameter("mpc_low_curvature_thresh_curvature"); + node_->declare_parameter("mpc_low_curvature_thresh_curvature"); m_mpc.m_param.weight_terminal_lat_error = - node_->declare_parameter("mpc_weight_terminal_lat_error"); + node_->declare_parameter("mpc_weight_terminal_lat_error"); m_mpc.m_param.weight_terminal_heading_error = - node_->declare_parameter("mpc_weight_terminal_heading_error"); - m_mpc.m_param.zero_ff_steer_deg = node_->declare_parameter("mpc_zero_ff_steer_deg"); - m_mpc.m_param.acceleration_limit = node_->declare_parameter("mpc_acceleration_limit"); + node_->declare_parameter("mpc_weight_terminal_heading_error"); + m_mpc.m_param.zero_ff_steer_deg = node_->declare_parameter("mpc_zero_ff_steer_deg"); + m_mpc.m_param.acceleration_limit = node_->declare_parameter("mpc_acceleration_limit"); m_mpc.m_param.velocity_time_constant = - node_->declare_parameter("mpc_velocity_time_constant"); + node_->declare_parameter("mpc_velocity_time_constant"); m_mpc.m_param.min_prediction_length = - node_->declare_parameter("mpc_min_prediction_length"); + node_->declare_parameter("mpc_min_prediction_length"); } rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( @@ -497,11 +493,11 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( // initialize input buffer update_param(parameters, "input_delay", param.input_delay); - const float64_t delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); - const float64_t delay = delay_step * m_mpc.m_ctrl_period; + const double delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); + const double delay = delay_step * m_mpc.m_ctrl_period; if (param.input_delay != delay) { param.input_delay = delay; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); } // transaction succeeds, now assign values @@ -529,7 +525,7 @@ bool MpcLateralController::isTrajectoryShapeChanged() const return false; } -bool8_t MpcLateralController::isValidTrajectory( +bool MpcLateralController::isValidTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & traj) const { for (const auto & p : traj.points) { diff --git a/control/trajectory_follower/src/mpc_trajectory.cpp b/control/trajectory_follower/src/mpc_trajectory.cpp index d22bd4004705b..cf0a45763a7ff 100644 --- a/control/trajectory_follower/src/mpc_trajectory.cpp +++ b/control/trajectory_follower/src/mpc_trajectory.cpp @@ -23,8 +23,8 @@ namespace control namespace trajectory_follower { void MPCTrajectory::push_back( - const float64_t & xp, const float64_t & yp, const float64_t & zp, const float64_t & yawp, - const float64_t & vxp, const float64_t & kp, const float64_t & smooth_kp, const float64_t & tp) + const double & xp, const double & yp, const double & zp, const double & yawp, const double & vxp, + const double & kp, const double & smooth_kp, const double & tp) { x.push_back(xp); y.push_back(yp); diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index c29f6e46967c8..aece2545c388e 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -34,50 +34,49 @@ namespace MPCUtils { using autoware::common::geometry::distance_2d; -geometry_msgs::msg::Quaternion getQuaternionFromYaw(const float64_t & yaw) +geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw) { tf2::Quaternion q; q.setRPY(0, 0, yaw); return tf2::toMsg(q); } -void convertEulerAngleToMonotonic(std::vector * a) +void convertEulerAngleToMonotonic(std::vector * a) { if (!a) { return; } - for (uint64_t i = 1; i < a->size(); ++i) { - const float64_t da = a->at(i) - a->at(i - 1); + for (uint i = 1; i < a->size(); ++i) { + const double da = a->at(i) - a->at(i - 1); a->at(i) = a->at(i - 1) + autoware::common::helper_functions::wrap_angle(da); } } -float64_t calcLateralError( +double calcLateralError( const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & ref_pose) { - const float64_t err_x = ego_pose.position.x - ref_pose.position.x; - const float64_t err_y = ego_pose.position.y - ref_pose.position.y; - const float64_t ref_yaw = tf2::getYaw(ref_pose.orientation); - const float64_t lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y; + const double err_x = ego_pose.position.x - ref_pose.position.x; + const double err_y = ego_pose.position.y - ref_pose.position.y; + const double ref_yaw = tf2::getYaw(ref_pose.orientation); + const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y; return lat_err; } -void calcMPCTrajectoryArclength( - const MPCTrajectory & trajectory, std::vector * arclength) +void calcMPCTrajectoryArclength(const MPCTrajectory & trajectory, std::vector * arclength) { - float64_t dist = 0.0; + double dist = 0.0; arclength->clear(); arclength->push_back(dist); - for (uint64_t i = 1; i < trajectory.size(); ++i) { - const float64_t dx = trajectory.x.at(i) - trajectory.x.at(i - 1); - const float64_t dy = trajectory.y.at(i) - trajectory.y.at(i - 1); + for (uint i = 1; i < trajectory.size(); ++i) { + const double dx = trajectory.x.at(i) - trajectory.x.at(i - 1); + const double dy = trajectory.y.at(i) - trajectory.y.at(i - 1); dist += std::sqrt(dx * dx + dy * dy); arclength->push_back(dist); } } -bool8_t resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const float64_t resample_interval_dist, MPCTrajectory * output) +bool resampleMPCTrajectoryByDistance( + const MPCTrajectory & input, const double resample_interval_dist, MPCTrajectory * output) { if (!output) { return false; @@ -86,19 +85,19 @@ bool8_t resampleMPCTrajectoryByDistance( *output = input; return true; } - std::vector input_arclength; + std::vector input_arclength; calcMPCTrajectoryArclength(input, &input_arclength); if (input_arclength.empty()) { return false; } - std::vector output_arclength; - for (float64_t s = 0; s < input_arclength.back(); s += resample_interval_dist) { + std::vector output_arclength; + for (double s = 0; s < input_arclength.back(); s += resample_interval_dist) { output_arclength.push_back(s); } - std::vector input_yaw = input.yaw; + std::vector input_yaw = input.yaw; convertEulerAngleToMonotonic(&input_yaw); output->x = interpolation::spline(input_arclength, input.x, output_arclength); @@ -114,9 +113,9 @@ bool8_t resampleMPCTrajectoryByDistance( return true; } -bool8_t linearInterpMPCTrajectory( - const std::vector & in_index, const MPCTrajectory & in_traj, - const std::vector & out_index, MPCTrajectory * out_traj) +bool linearInterpMPCTrajectory( + const std::vector & in_index, const MPCTrajectory & in_traj, + const std::vector & out_index, MPCTrajectory * out_traj) { if (!out_traj) { return false; @@ -127,7 +126,7 @@ bool8_t linearInterpMPCTrajectory( return true; } - std::vector in_traj_yaw = in_traj.yaw; + std::vector in_traj_yaw = in_traj.yaw; convertEulerAngleToMonotonic(&in_traj_yaw); if ( @@ -162,9 +161,9 @@ void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift) } // interpolate yaw - for (int64_t i = 1; i < static_cast(traj->yaw.size()) - 1; ++i) { - const float64_t dx = traj->x[static_cast(i + 1)] - traj->x[static_cast(i - 1)]; - const float64_t dy = traj->y[static_cast(i + 1)] - traj->y[static_cast(i - 1)]; + for (int i = 1; i < static_cast(traj->yaw.size()) - 1; ++i) { + const double dx = traj->x[static_cast(i + 1)] - traj->x[static_cast(i - 1)]; + const double dy = traj->y[static_cast(i + 1)] - traj->y[static_cast(i - 1)]; traj->yaw[static_cast(i)] = is_forward_shift ? std::atan2(dy, dx) : std::atan2(dy, dx) + M_PI; } @@ -174,7 +173,7 @@ void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift) } } -bool8_t calcTrajectoryCurvature( +bool calcTrajectoryCurvature( const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer, MPCTrajectory * traj) { @@ -187,15 +186,15 @@ bool8_t calcTrajectoryCurvature( return true; } -std::vector calcTrajectoryCurvature( +std::vector calcTrajectoryCurvature( const size_t curvature_smoothing_num, const MPCTrajectory & traj) { - std::vector curvature_vec(traj.x.size()); + std::vector curvature_vec(traj.x.size()); /* calculate curvature by circle fitting from three points */ geometry_msgs::msg::Point p1, p2, p3; const size_t max_smoothing_num = - static_cast(std::floor(0.5 * (static_cast(traj.x.size() - 1)))); + static_cast(std::floor(0.5 * (static_cast(traj.x.size() - 1)))); const size_t L = std::min(curvature_smoothing_num, max_smoothing_num); for (size_t i = L; i < traj.x.size() - L; ++i) { const size_t curr_idx = i; @@ -207,11 +206,10 @@ std::vector calcTrajectoryCurvature( p1.y = traj.y[prev_idx]; p2.y = traj.y[curr_idx]; p3.y = traj.y[next_idx]; - const float64_t den = std::max( - distance_2d(p1, p2) * distance_2d(p2, p3) * - distance_2d(p3, p1), - std::numeric_limits::epsilon()); - const float64_t curvature = + const double den = std::max( + distance_2d(p1, p2) * distance_2d(p2, p3) * distance_2d(p3, p1), + std::numeric_limits::epsilon()); + const double curvature = 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; curvature_vec.at(curr_idx) = curvature; } @@ -225,25 +223,25 @@ std::vector calcTrajectoryCurvature( return curvature_vec; } -bool8_t convertToMPCTrajectory( +bool convertToMPCTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output) { output.clear(); for (const autoware_auto_planning_msgs::msg::TrajectoryPoint & p : input.points) { - const float64_t x = p.pose.position.x; - const float64_t y = p.pose.position.y; - const float64_t z = 0.0; - const float64_t yaw = tf2::getYaw(p.pose.orientation); - const float64_t vx = p.longitudinal_velocity_mps; - const float64_t k = 0.0; - const float64_t t = 0.0; + const double x = p.pose.position.x; + const double y = p.pose.position.y; + const double z = 0.0; + const double yaw = tf2::getYaw(p.pose.orientation); + const double vx = p.longitudinal_velocity_mps; + const double k = 0.0; + const double t = 0.0; output.push_back(x, y, z, yaw, vx, k, k, t); } calcMPCTrajectoryTime(output); return true; } -bool8_t convertToAutowareTrajectory( +bool convertToAutowareTrajectory( const MPCTrajectory & input, autoware_auto_planning_msgs::msg::Trajectory & output) { output.points.clear(); @@ -261,17 +259,17 @@ bool8_t convertToAutowareTrajectory( return true; } -bool8_t calcMPCTrajectoryTime(MPCTrajectory & traj) +bool calcMPCTrajectoryTime(MPCTrajectory & traj) { - float64_t t = 0.0; + double t = 0.0; traj.relative_time.clear(); traj.relative_time.push_back(t); for (size_t i = 0; i < traj.x.size() - 1; ++i) { - const float64_t dx = traj.x.at(i + 1) - traj.x.at(i); - const float64_t dy = traj.y.at(i + 1) - traj.y.at(i); - const float64_t dz = traj.z.at(i + 1) - traj.z.at(i); - const float64_t dist = std::sqrt(dx * dx + dy * dy + dz * dz); - const float64_t v = std::max(std::fabs(traj.vx.at(i)), 0.1); + const double dx = traj.x.at(i + 1) - traj.x.at(i); + const double dy = traj.y.at(i + 1) - traj.y.at(i); + const double dz = traj.z.at(i + 1) - traj.z.at(i); + const double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + const double v = std::max(std::fabs(traj.vx.at(i)), 0.1); t += (dist / v); traj.relative_time.push_back(t); } @@ -279,29 +277,27 @@ bool8_t calcMPCTrajectoryTime(MPCTrajectory & traj) } void dynamicSmoothingVelocity( - const size_t start_idx, const float64_t start_vel, const float64_t acc_lim, const float64_t tau, + const size_t start_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj) { - float64_t curr_v = start_vel; + double curr_v = start_vel; traj.vx.at(start_idx) = start_vel; for (size_t i = start_idx + 1; i < traj.size(); ++i) { - const float64_t ds = - std::hypot(traj.x.at(i) - traj.x.at(i - 1), traj.y.at(i) - traj.y.at(i - 1)); - const float64_t dt = - ds / std::max(std::fabs(curr_v), std::numeric_limits::epsilon()); - const float64_t a = tau / std::max(tau + dt, std::numeric_limits::epsilon()); - const float64_t updated_v = a * curr_v + (1.0 - a) * traj.vx.at(i); - const float64_t dv = std::max(-acc_lim * dt, std::min(acc_lim * dt, updated_v - curr_v)); + const double ds = std::hypot(traj.x.at(i) - traj.x.at(i - 1), traj.y.at(i) - traj.y.at(i - 1)); + const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits::epsilon()); + const double a = tau / std::max(tau + dt, std::numeric_limits::epsilon()); + const double updated_v = a * curr_v + (1.0 - a) * traj.vx.at(i); + const double dv = std::max(-acc_lim * dt, std::min(acc_lim * dt, updated_v - curr_v)); curr_v = curr_v + dv; traj.vx.at(i) = curr_v; } calcMPCTrajectoryTime(traj); } -bool8_t calcNearestPoseInterp( +bool calcNearestPoseInterp( const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, - geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time, + 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, rclcpp::Clock & clock) { if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) { @@ -330,25 +326,25 @@ bool8_t calcNearestPoseInterp( auto calcSquaredDist = [](const geometry_msgs::msg::Pose & p, const MPCTrajectory & t, const size_t idx) { - const float64_t dx = p.position.x - t.x[idx]; - const float64_t dy = p.position.y - t.y[idx]; + const double dx = p.position.x - t.x[idx]; + const double dy = p.position.y - t.y[idx]; return dx * dx + dy * dy; }; /* get second nearest index = next to nearest_index */ const size_t next = static_cast( - std::min(static_cast(*nearest_index) + 1, static_cast(traj_size) - 1)); + std::min(static_cast(*nearest_index) + 1, static_cast(traj_size) - 1)); const size_t prev = - static_cast(std::max(static_cast(*nearest_index) - 1, int64_t(0))); - const float64_t dist_to_next = calcSquaredDist(self_pose, traj, next); - const float64_t dist_to_prev = calcSquaredDist(self_pose, traj, prev); + static_cast(std::max(static_cast(*nearest_index) - 1, static_cast(0))); + const double dist_to_next = calcSquaredDist(self_pose, traj, next); + const double dist_to_prev = calcSquaredDist(self_pose, traj, prev); const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev; - const float64_t a_sq = calcSquaredDist(self_pose, traj, *nearest_index); - const float64_t b_sq = calcSquaredDist(self_pose, traj, second_nearest_index); - const float64_t dx3 = traj.x[*nearest_index] - traj.x[second_nearest_index]; - const float64_t dy3 = traj.y[*nearest_index] - traj.y[second_nearest_index]; - const float64_t c_sq = dx3 * dx3 + dy3 * dy3; + const double a_sq = calcSquaredDist(self_pose, traj, *nearest_index); + const double b_sq = calcSquaredDist(self_pose, traj, second_nearest_index); + const double dx3 = traj.x[*nearest_index] - traj.x[second_nearest_index]; + const double dy3 = traj.y[*nearest_index] - traj.y[second_nearest_index]; + const double c_sq = dx3 * dx3 + dy3 * dy3; /* if distance between two points are too close */ if (c_sq < 1.0E-5) { @@ -360,14 +356,14 @@ bool8_t calcNearestPoseInterp( } /* linear interpolation */ - const float64_t alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0); + const double alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0); nearest_pose->position.x = alpha * traj.x[*nearest_index] + (1 - alpha) * traj.x[second_nearest_index]; nearest_pose->position.y = alpha * traj.y[*nearest_index] + (1 - alpha) * traj.y[second_nearest_index]; - const float64_t tmp_yaw_err = autoware::common::helper_functions::wrap_angle( + const double tmp_yaw_err = autoware::common::helper_functions::wrap_angle( traj.yaw[*nearest_index] - traj.yaw[second_nearest_index]); - const float64_t nearest_yaw = autoware::common::helper_functions::wrap_angle( + const double nearest_yaw = autoware::common::helper_functions::wrap_angle( traj.yaw[second_nearest_index] + alpha * tmp_yaw_err); nearest_pose->orientation = getQuaternionFromYaw(nearest_yaw); *nearest_time = alpha * traj.relative_time[*nearest_index] + @@ -375,21 +371,20 @@ bool8_t calcNearestPoseInterp( return true; } -float64_t calcStopDistance( - const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int64_t origin) +double calcStopDistance( + const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin) { constexpr float zero_velocity = std::numeric_limits::epsilon(); const float origin_velocity = current_trajectory.points.at(static_cast(origin)).longitudinal_velocity_mps; - float64_t stop_dist = 0.0; + double stop_dist = 0.0; // search forward if (std::fabs(origin_velocity) > zero_velocity) { - for (int64_t i = origin + 1; i < static_cast(current_trajectory.points.size()) - 1; - ++i) { + for (int i = origin + 1; i < static_cast(current_trajectory.points.size()) - 1; ++i) { const auto & p0 = current_trajectory.points.at(static_cast(i)); const auto & p1 = current_trajectory.points.at(static_cast(i - 1)); - stop_dist += distance_2d(p0, p1); + stop_dist += distance_2d(p0, p1); if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) { break; } @@ -398,13 +393,13 @@ float64_t calcStopDistance( } // search backward - for (int64_t i = origin - 1; 0 < i; --i) { + for (int i = origin - 1; 0 < i; --i) { const auto & p0 = current_trajectory.points.at(static_cast(i)); const auto & p1 = current_trajectory.points.at(static_cast(i + 1)); if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) { break; } - stop_dist -= distance_2d(p0, p1); + stop_dist -= distance_2d(p0, p1); } return stop_dist; } diff --git a/control/trajectory_follower/src/pid.cpp b/control/trajectory_follower/src/pid.cpp index badb3fb51a1a0..a44a3d4694729 100644 --- a/control/trajectory_follower/src/pid.cpp +++ b/control/trajectory_follower/src/pid.cpp @@ -37,9 +37,9 @@ PIDController::PIDController() { } -float64_t PIDController::calculate( - const float64_t error, const float64_t dt, const bool8_t enable_integration, - std::vector & pid_contributions) +double PIDController::calculate( + const double error, const double dt, const bool enable_integration, + std::vector & pid_contributions) { if (!m_is_gains_set || !m_is_limits_set) { throw std::runtime_error("Trying to calculate uninitialized PID"); @@ -47,23 +47,23 @@ float64_t PIDController::calculate( const auto & p = m_params; - float64_t ret_p = p.kp * error; + double ret_p = p.kp * error; ret_p = std::min(std::max(ret_p, p.min_ret_p), p.max_ret_p); if (enable_integration) { m_error_integral += error * dt; m_error_integral = std::min(std::max(m_error_integral, p.min_ret_i / p.ki), p.max_ret_i / p.ki); } - const float64_t ret_i = p.ki * m_error_integral; + const double ret_i = p.ki * m_error_integral; - float64_t error_differential; + double error_differential; if (m_is_first_time) { error_differential = 0; m_is_first_time = false; } else { error_differential = (error - m_prev_error) / dt; } - float64_t ret_d = p.kd * error_differential; + double ret_d = p.kd * error_differential; ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); m_prev_error = error; @@ -73,13 +73,13 @@ float64_t PIDController::calculate( pid_contributions.at(1) = ret_i; pid_contributions.at(2) = ret_d; - float64_t ret = ret_p + ret_i + ret_d; + double ret = ret_p + ret_i + ret_d; ret = std::min(std::max(ret, p.min_ret), p.max_ret); return ret; } -void PIDController::setGains(const float64_t kp, const float64_t ki, const float64_t kd) +void PIDController::setGains(const double kp, const double ki, const double kd) { m_params.kp = kp; m_params.ki = ki; @@ -88,9 +88,8 @@ void PIDController::setGains(const float64_t kp, const float64_t ki, const float } void PIDController::setLimits( - const float64_t max_ret, const float64_t min_ret, const float64_t max_ret_p, - const float64_t min_ret_p, const float64_t max_ret_i, const float64_t min_ret_i, - const float64_t max_ret_d, const float64_t min_ret_d) + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d) { m_params.max_ret = max_ret; m_params.min_ret = min_ret; diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index ff5c6aefe25cf..bde2139bcaa78 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -43,103 +43,101 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) m_wheel_base = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; // parameters for delay compensation - m_delay_compensation_time = - node_->declare_parameter("delay_compensation_time"); // [s] + m_delay_compensation_time = node_->declare_parameter("delay_compensation_time"); // [s] // parameters to enable functions - m_enable_smooth_stop = node_->declare_parameter("enable_smooth_stop"); - m_enable_overshoot_emergency = node_->declare_parameter("enable_overshoot_emergency"); + m_enable_smooth_stop = node_->declare_parameter("enable_smooth_stop"); + m_enable_overshoot_emergency = node_->declare_parameter("enable_overshoot_emergency"); m_enable_large_tracking_error_emergency = - node_->declare_parameter("enable_large_tracking_error_emergency"); - m_enable_slope_compensation = node_->declare_parameter("enable_slope_compensation"); + node_->declare_parameter("enable_large_tracking_error_emergency"); + m_enable_slope_compensation = node_->declare_parameter("enable_slope_compensation"); m_enable_keep_stopped_until_steer_convergence = - node_->declare_parameter("enable_keep_stopped_until_steer_convergence"); + node_->declare_parameter("enable_keep_stopped_until_steer_convergence"); // parameters for state transition { auto & p = m_state_transition_params; // drive - p.drive_state_stop_dist = node_->declare_parameter("drive_state_stop_dist"); // [m] + p.drive_state_stop_dist = node_->declare_parameter("drive_state_stop_dist"); // [m] p.drive_state_offset_stop_dist = - node_->declare_parameter("drive_state_offset_stop_dist"); // [m] + node_->declare_parameter("drive_state_offset_stop_dist"); // [m] // stopping p.stopping_state_stop_dist = - node_->declare_parameter("stopping_state_stop_dist"); // [m] + node_->declare_parameter("stopping_state_stop_dist"); // [m] p.stopped_state_entry_duration_time = - node_->declare_parameter("stopped_state_entry_duration_time"); // [s] + node_->declare_parameter("stopped_state_entry_duration_time"); // [s] // stop p.stopped_state_entry_vel = - node_->declare_parameter("stopped_state_entry_vel"); // [m/s] + node_->declare_parameter("stopped_state_entry_vel"); // [m/s] p.stopped_state_entry_acc = - node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] + node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] // emergency p.emergency_state_overshoot_stop_dist = - node_->declare_parameter("emergency_state_overshoot_stop_dist"); // [m] + node_->declare_parameter("emergency_state_overshoot_stop_dist"); // [m] p.emergency_state_traj_trans_dev = - node_->declare_parameter("emergency_state_traj_trans_dev"); // [m] + node_->declare_parameter("emergency_state_traj_trans_dev"); // [m] p.emergency_state_traj_rot_dev = - node_->declare_parameter("emergency_state_traj_rot_dev"); // [m] + node_->declare_parameter("emergency_state_traj_rot_dev"); // [m] } // parameters for drive state { // initialize PID gain - const float64_t kp{node_->declare_parameter("kp")}; - const float64_t ki{node_->declare_parameter("ki")}; - const float64_t kd{node_->declare_parameter("kd")}; + const double kp{node_->declare_parameter("kp")}; + const double ki{node_->declare_parameter("ki")}; + const double kd{node_->declare_parameter("kd")}; m_pid_vel.setGains(kp, ki, kd); // initialize PID limits - const float64_t max_pid{node_->declare_parameter("max_out")}; // [m/s^2] - const float64_t min_pid{node_->declare_parameter("min_out")}; // [m/s^2] - const float64_t max_p{node_->declare_parameter("max_p_effort")}; // [m/s^2] - const float64_t min_p{node_->declare_parameter("min_p_effort")}; // [m/s^2] - const float64_t max_i{node_->declare_parameter("max_i_effort")}; // [m/s^2] - const float64_t min_i{node_->declare_parameter("min_i_effort")}; // [m/s^2] - const float64_t max_d{node_->declare_parameter("max_d_effort")}; // [m/s^2] - const float64_t min_d{node_->declare_parameter("min_d_effort")}; // [m/s^2] + const double max_pid{node_->declare_parameter("max_out")}; // [m/s^2] + const double min_pid{node_->declare_parameter("min_out")}; // [m/s^2] + const double max_p{node_->declare_parameter("max_p_effort")}; // [m/s^2] + const double min_p{node_->declare_parameter("min_p_effort")}; // [m/s^2] + const double max_i{node_->declare_parameter("max_i_effort")}; // [m/s^2] + const double min_i{node_->declare_parameter("min_i_effort")}; // [m/s^2] + const double max_d{node_->declare_parameter("max_d_effort")}; // [m/s^2] + const double min_d{node_->declare_parameter("min_d_effort")}; // [m/s^2] m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); // set lowpass filter for vel error and pitch - const float64_t lpf_vel_error_gain{node_->declare_parameter("lpf_vel_error_gain")}; + 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); m_current_vel_threshold_pid_integrate = - node_->declare_parameter("current_vel_threshold_pid_integration"); // [m/s] + node_->declare_parameter("current_vel_threshold_pid_integration"); // [m/s] m_enable_brake_keeping_before_stop = - node_->declare_parameter("enable_brake_keeping_before_stop"); // [-] - m_brake_keeping_acc = node_->declare_parameter("brake_keeping_acc"); // [m/s^2] + node_->declare_parameter("enable_brake_keeping_before_stop"); // [-] + m_brake_keeping_acc = node_->declare_parameter("brake_keeping_acc"); // [m/s^2] } // parameters for smooth stop state { - const float64_t max_strong_acc{ - node_->declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] - const float64_t min_strong_acc{ - node_->declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] - const float64_t weak_acc{ - node_->declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] - const float64_t weak_stop_acc{ - node_->declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] - const float64_t strong_stop_acc{ - node_->declare_parameter("smooth_stop_strong_stop_acc")}; // [m/s^2] - - const float64_t max_fast_vel{ - node_->declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] - const float64_t min_running_vel{ - node_->declare_parameter("smooth_stop_min_running_vel")}; // [m/s] - const float64_t min_running_acc{ - node_->declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] - const float64_t weak_stop_time{ - node_->declare_parameter("smooth_stop_weak_stop_time")}; // [s] - - const float64_t weak_stop_dist{ - node_->declare_parameter("smooth_stop_weak_stop_dist")}; // [m] - const float64_t strong_stop_dist{ - node_->declare_parameter("smooth_stop_strong_stop_dist")}; // [m] + const double max_strong_acc{ + node_->declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] + const double min_strong_acc{ + node_->declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] + const double weak_acc{node_->declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] + const double weak_stop_acc{ + node_->declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] + const double strong_stop_acc{ + node_->declare_parameter("smooth_stop_strong_stop_acc")}; // [m/s^2] + + const double max_fast_vel{ + node_->declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] + const double min_running_vel{ + node_->declare_parameter("smooth_stop_min_running_vel")}; // [m/s] + const double min_running_acc{ + node_->declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] + const double weak_stop_time{ + node_->declare_parameter("smooth_stop_weak_stop_time")}; // [s] + + const double weak_stop_dist{ + node_->declare_parameter("smooth_stop_weak_stop_dist")}; // [m] + const double strong_stop_dist{ + node_->declare_parameter("smooth_stop_strong_stop_dist")}; // [m] m_smooth_stop.setParams( max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, @@ -149,33 +147,33 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters for stop state { auto & p = m_stopped_state_params; - p.vel = node_->declare_parameter("stopped_vel"); // [m/s] - p.acc = node_->declare_parameter("stopped_acc"); // [m/s^2] - p.jerk = node_->declare_parameter("stopped_jerk"); // [m/s^3] + p.vel = node_->declare_parameter("stopped_vel"); // [m/s] + p.acc = node_->declare_parameter("stopped_acc"); // [m/s^2] + p.jerk = node_->declare_parameter("stopped_jerk"); // [m/s^3] } // parameters for emergency state { auto & p = m_emergency_state_params; - p.vel = node_->declare_parameter("emergency_vel"); // [m/s] - p.acc = node_->declare_parameter("emergency_acc"); // [m/s^2] - p.jerk = node_->declare_parameter("emergency_jerk"); // [m/s^3] + p.vel = node_->declare_parameter("emergency_vel"); // [m/s] + p.acc = node_->declare_parameter("emergency_acc"); // [m/s^2] + p.jerk = node_->declare_parameter("emergency_jerk"); // [m/s^3] } // parameters for acceleration limit - m_max_acc = node_->declare_parameter("max_acc"); // [m/s^2] - m_min_acc = node_->declare_parameter("min_acc"); // [m/s^2] + m_max_acc = node_->declare_parameter("max_acc"); // [m/s^2] + m_min_acc = node_->declare_parameter("min_acc"); // [m/s^2] // parameters for jerk limit - m_max_jerk = node_->declare_parameter("max_jerk"); // [m/s^3] - m_min_jerk = node_->declare_parameter("min_jerk"); // [m/s^3] + m_max_jerk = node_->declare_parameter("max_jerk"); // [m/s^3] + m_min_jerk = node_->declare_parameter("min_jerk"); // [m/s^3] // parameters for slope compensation - m_use_traj_for_pitch = node_->declare_parameter("use_trajectory_for_pitch_calculation"); - const float64_t lpf_pitch_gain{node_->declare_parameter("lpf_pitch_gain")}; + 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_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] - m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] + m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] + m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] // ego nearest index search m_ego_nearest_dist_threshold = @@ -243,7 +241,7 @@ void PidLongitudinalController::setTrajectory( rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback( const std::vector & parameters) { - auto update_param = [&](const std::string & name, float64_t & v) { + auto update_param = [&](const std::string & name, double & v) { auto it = std::find_if( parameters.cbegin(), parameters.cend(), [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); @@ -272,22 +270,22 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // drive state { - float64_t kp{node_->get_parameter("kp").as_double()}; - float64_t ki{node_->get_parameter("ki").as_double()}; - float64_t kd{node_->get_parameter("kd").as_double()}; + double kp{node_->get_parameter("kp").as_double()}; + double ki{node_->get_parameter("ki").as_double()}; + double kd{node_->get_parameter("kd").as_double()}; update_param("kp", kp); update_param("ki", ki); update_param("kd", kd); m_pid_vel.setGains(kp, ki, kd); - float64_t max_pid{node_->get_parameter("max_out").as_double()}; - float64_t min_pid{node_->get_parameter("min_out").as_double()}; - float64_t max_p{node_->get_parameter("max_p_effort").as_double()}; - float64_t min_p{node_->get_parameter("min_p_effort").as_double()}; - float64_t max_i{node_->get_parameter("max_i_effort").as_double()}; - float64_t min_i{node_->get_parameter("min_i_effort").as_double()}; - float64_t max_d{node_->get_parameter("max_d_effort").as_double()}; - float64_t min_d{node_->get_parameter("min_d_effort").as_double()}; + double max_pid{node_->get_parameter("max_out").as_double()}; + double min_pid{node_->get_parameter("min_out").as_double()}; + double max_p{node_->get_parameter("max_p_effort").as_double()}; + double min_p{node_->get_parameter("min_p_effort").as_double()}; + double max_i{node_->get_parameter("max_i_effort").as_double()}; + double min_i{node_->get_parameter("min_i_effort").as_double()}; + double max_d{node_->get_parameter("max_d_effort").as_double()}; + double min_d{node_->get_parameter("min_d_effort").as_double()}; update_param("max_out", max_pid); update_param("min_out", min_pid); update_param("max_p_effort", max_p); @@ -303,17 +301,17 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // stopping state { - float64_t max_strong_acc{node_->get_parameter("smooth_stop_max_strong_acc").as_double()}; - float64_t min_strong_acc{node_->get_parameter("smooth_stop_min_strong_acc").as_double()}; - float64_t weak_acc{node_->get_parameter("smooth_stop_weak_acc").as_double()}; - float64_t weak_stop_acc{node_->get_parameter("smooth_stop_weak_stop_acc").as_double()}; - float64_t strong_stop_acc{node_->get_parameter("smooth_stop_strong_stop_acc").as_double()}; - float64_t max_fast_vel{node_->get_parameter("smooth_stop_max_fast_vel").as_double()}; - float64_t min_running_vel{node_->get_parameter("smooth_stop_min_running_vel").as_double()}; - float64_t min_running_acc{node_->get_parameter("smooth_stop_min_running_acc").as_double()}; - float64_t weak_stop_time{node_->get_parameter("smooth_stop_weak_stop_time").as_double()}; - float64_t weak_stop_dist{node_->get_parameter("smooth_stop_weak_stop_dist").as_double()}; - float64_t strong_stop_dist{node_->get_parameter("smooth_stop_strong_stop_dist").as_double()}; + double max_strong_acc{node_->get_parameter("smooth_stop_max_strong_acc").as_double()}; + double min_strong_acc{node_->get_parameter("smooth_stop_min_strong_acc").as_double()}; + double weak_acc{node_->get_parameter("smooth_stop_weak_acc").as_double()}; + double weak_stop_acc{node_->get_parameter("smooth_stop_weak_stop_acc").as_double()}; + double strong_stop_acc{node_->get_parameter("smooth_stop_strong_stop_acc").as_double()}; + double max_fast_vel{node_->get_parameter("smooth_stop_max_fast_vel").as_double()}; + double min_running_vel{node_->get_parameter("smooth_stop_min_running_vel").as_double()}; + double min_running_acc{node_->get_parameter("smooth_stop_min_running_acc").as_double()}; + double weak_stop_time{node_->get_parameter("smooth_stop_weak_stop_time").as_double()}; + double weak_stop_dist{node_->get_parameter("smooth_stop_weak_stop_dist").as_double()}; + double strong_stop_dist{node_->get_parameter("smooth_stop_strong_stop_dist").as_double()}; update_param("smooth_stop_max_strong_acc", max_strong_acc); update_param("smooth_stop_min_strong_acc", min_strong_acc); update_param("smooth_stop_weak_acc", weak_acc); @@ -457,9 +455,9 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData current_pose, *m_trajectory_ptr, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch - const float64_t raw_pitch = + const double raw_pitch = trajectory_follower::longitudinal_utils::getPitchByPose(current_pose.orientation); - const float64_t traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj( + const double traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj( *m_trajectory_ptr, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); @@ -468,13 +466,13 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData } PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd( - const float64_t dt) const + const double dt) const { // These accelerations are without slope compensation const auto & p = m_emergency_state_params; - const float64_t vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + const double vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); - const float64_t acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + const double acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); RCLCPP_ERROR_THROTTLE( @@ -486,39 +484,39 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm void PidLongitudinalController::updateControlState(const ControlData & control_data) { - const float64_t current_vel = control_data.current_motion.vel; - const float64_t current_acc = control_data.current_motion.acc; - const float64_t stop_dist = control_data.stop_dist; + const double current_vel = control_data.current_motion.vel; + const double current_acc = control_data.current_motion.acc; + const double stop_dist = control_data.stop_dist; // flags for state transition const auto & p = m_state_transition_params; - const bool8_t departure_condition_from_stopping = + const bool departure_condition_from_stopping = stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; - const bool8_t departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; + const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; - const bool8_t keep_stopped_condition = + const bool keep_stopped_condition = m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; - const bool8_t stopping_condition = stop_dist < p.stopping_state_stop_dist; + const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; if ( std::fabs(current_vel) > p.stopped_state_entry_vel || std::fabs(current_acc) > p.stopped_state_entry_acc) { m_last_running_time = std::make_shared(node_->now()); } - const bool8_t stopped_condition = + const bool stopped_condition = m_last_running_time ? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; static constexpr double vel_epsilon = 1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity - const float64_t current_vel_cmd = + const double current_vel_cmd = std::fabs(m_trajectory_ptr->points.at(control_data.nearest_idx).longitudinal_velocity_mps); - const bool8_t emergency_condition = m_enable_overshoot_emergency && - stop_dist < -p.emergency_state_overshoot_stop_dist && - current_vel_cmd < vel_epsilon; - const bool8_t has_nonzero_target_vel = std::abs(current_vel_cmd) > 1.0e-5; + const bool emergency_condition = m_enable_overshoot_emergency && + stop_dist < -p.emergency_state_overshoot_stop_dist && + current_vel_cmd < vel_epsilon; + const bool has_nonzero_target_vel = std::abs(current_vel_cmd) > 1.0e-5; const auto changeState = [this](const auto s) { if (s != m_control_state) { @@ -544,9 +542,9 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (m_enable_smooth_stop) { if (stopping_condition) { // predictions after input time delay - const float64_t pred_vel_in_target = + const double pred_vel_in_target = predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); - const float64_t pred_stop_dist = + const double pred_stop_dist = control_data.stop_dist - 0.5 * (pred_vel_in_target + current_vel) * m_delay_compensation_time; m_smooth_stop.init(pred_vel_in_target, pred_stop_dist); @@ -621,8 +619,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data) { const size_t nearest_idx = control_data.nearest_idx; - const float64_t current_vel = control_data.current_motion.vel; - const float64_t current_acc = control_data.current_motion.acc; + const double current_vel = control_data.current_motion.vel; + const double current_acc = control_data.current_motion.acc; // velocity and acceleration command Motion raw_ctrl_cmd{}; @@ -638,7 +636,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( target_motion = keepBrakeBeforeStop(*m_trajectory_ptr, target_motion, nearest_idx); - const float64_t pred_vel_in_target = + 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); @@ -676,7 +674,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( m_prev_raw_ctrl_cmd = raw_ctrl_cmd; // apply slope compensation and filter acceleration and jerk - const float64_t filtered_acc_cmd = calcFilteredAcc(raw_ctrl_cmd.acc, control_data); + const double filtered_acc_cmd = calcFilteredAcc(raw_ctrl_cmd.acc, control_data); const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd}; // update debug visualization @@ -687,7 +685,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( // Do not use nearest_idx here autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController::createCtrlCmdMsg( - const Motion & ctrl_cmd, const float64_t & current_vel) + const Motion & ctrl_cmd, const double & current_vel) { // publish control command autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; @@ -713,10 +711,9 @@ void PidLongitudinalController::publishDebugData( // set debug values m_debug_values.setValues(DebugValues::TYPE::DT, control_data.dt); m_debug_values.setValues(DebugValues::TYPE::CALCULATED_ACC, control_data.current_motion.acc); - m_debug_values.setValues(DebugValues::TYPE::SHIFT, static_cast(control_data.shift)); + m_debug_values.setValues(DebugValues::TYPE::SHIFT, static_cast(control_data.shift)); m_debug_values.setValues(DebugValues::TYPE::STOP_DIST, control_data.stop_dist); - m_debug_values.setValues( - DebugValues::TYPE::CONTROL_STATE, static_cast(m_control_state)); + m_debug_values.setValues(DebugValues::TYPE::CONTROL_STATE, static_cast(m_control_state)); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc); // publish debug values @@ -735,9 +732,9 @@ void PidLongitudinalController::publishDebugData( m_pub_slope->publish(slope_msg); } -float64_t PidLongitudinalController::getDt() +double PidLongitudinalController::getDt() { - float64_t dt; + double dt; if (!m_prev_control_time) { dt = m_longitudinal_ctrl_period; m_prev_control_time = std::make_shared(node_->now()); @@ -745,17 +742,17 @@ float64_t PidLongitudinalController::getDt() dt = (node_->now() - *m_prev_control_time).seconds(); *m_prev_control_time = node_->now(); } - const float64_t max_dt = m_longitudinal_ctrl_period * 2.0; - const float64_t min_dt = m_longitudinal_ctrl_period * 0.5; + const double max_dt = m_longitudinal_ctrl_period * 2.0; + const double min_dt = m_longitudinal_ctrl_period * 0.5; return std::max(std::min(dt, max_dt), min_dt); } enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift( const size_t nearest_idx) const { - constexpr float64_t epsilon = 1e-5; + constexpr double epsilon = 1e-5; - const float64_t target_vel = m_trajectory_ptr->points.at(nearest_idx).longitudinal_velocity_mps; + const double target_vel = m_trajectory_ptr->points.at(nearest_idx).longitudinal_velocity_mps; if (target_vel > epsilon) { return Shift::Forward; @@ -766,29 +763,29 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift return m_prev_shift; } -float64_t PidLongitudinalController::calcFilteredAcc( - const float64_t raw_acc, const ControlData & control_data) +double PidLongitudinalController::calcFilteredAcc( + const double raw_acc, const ControlData & control_data) { using trajectory_follower::DebugValues; - const float64_t acc_max_filtered = std::clamp(raw_acc, m_min_acc, m_max_acc); + 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); // store ctrl cmd without slope filter storeAccelCmd(acc_max_filtered); - const float64_t acc_slope_filtered = + const double acc_slope_filtered = applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); // This jerk filter must be applied after slope compensation - const float64_t acc_jerk_filtered = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + 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); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); return acc_jerk_filtered; } -void PidLongitudinalController::storeAccelCmd(const float64_t accel) +void PidLongitudinalController::storeAccelCmd(const double accel) { if (m_control_state == ControlState::DRIVE) { // convert format @@ -812,17 +809,17 @@ void PidLongitudinalController::storeAccelCmd(const float64_t accel) } } -float64_t PidLongitudinalController::applySlopeCompensation( - const float64_t input_acc, const float64_t pitch, const Shift shift) const +double PidLongitudinalController::applySlopeCompensation( + const double input_acc, const double pitch, const Shift shift) const { if (!m_enable_slope_compensation) { return input_acc; } - const float64_t pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad); + const double pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad); // Acceleration command is always positive independent of direction (= shift) when car is running - float64_t sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0); - float64_t compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited); + double sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0); + double compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited); return compensated_acc; } @@ -841,7 +838,7 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop return output_motion; } - float64_t min_acc_before_stop = std::numeric_limits::max(); + double min_acc_before_stop = std::numeric_limits::max(); size_t min_acc_idx = std::numeric_limits::max(); for (int i = static_cast(*stop_idx); i >= 0; --i) { const auto ui = static_cast(i); @@ -852,7 +849,7 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop min_acc_idx = ui; } - const float64_t brake_keeping_acc = std::max(m_brake_keeping_acc, min_acc_before_stop); + const double brake_keeping_acc = std::max(m_brake_keeping_acc, min_acc_before_stop); if (nearest_idx >= min_acc_idx && target_motion.acc > brake_keeping_acc) { output_motion.acc = brake_keeping_acc; } @@ -874,24 +871,24 @@ PidLongitudinalController::calcInterpolatedTargetValue( traj.points, pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); } -float64_t PidLongitudinalController::predictedVelocityInTargetPoint( - const Motion current_motion, const float64_t delay_compensation_time) const +double PidLongitudinalController::predictedVelocityInTargetPoint( + const Motion current_motion, const double delay_compensation_time) const { - const float64_t current_vel = current_motion.vel; - const float64_t current_acc = current_motion.acc; + const double current_vel = current_motion.vel; + const double current_acc = current_motion.acc; if (std::fabs(current_vel) < 1e-01) { // when velocity is low, no prediction return current_vel; } - const float64_t current_vel_abs = std::fabs(current_vel); + const double current_vel_abs = std::fabs(current_vel); if (m_ctrl_cmd_vec.size() == 0) { - const float64_t pred_vel = current_vel + current_acc * delay_compensation_time; + const double pred_vel = current_vel + current_acc * delay_compensation_time; // avoid to change sign of current_vel and pred_vel return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; } - float64_t pred_vel = current_vel_abs; + double pred_vel = current_vel_abs; const auto past_delay_time = node_->now() - rclcpp::Duration::from_seconds(delay_compensation_time); @@ -899,22 +896,22 @@ float64_t PidLongitudinalController::predictedVelocityInTargetPoint( if ((node_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { if (i == 0) { // size of m_ctrl_cmd_vec is less than m_delay_compensation_time - pred_vel = current_vel_abs + static_cast(m_ctrl_cmd_vec.at(i).acceleration) * - delay_compensation_time; + pred_vel = current_vel_abs + + static_cast(m_ctrl_cmd_vec.at(i).acceleration) * delay_compensation_time; return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; } // add velocity to accel * dt - const float64_t acc = m_ctrl_cmd_vec.at(i - 1).acceleration; + const double acc = m_ctrl_cmd_vec.at(i - 1).acceleration; const auto curr_time_i = rclcpp::Time(m_ctrl_cmd_vec.at(i).stamp); - const float64_t time_to_next_acc = std::min( + const double time_to_next_acc = std::min( (curr_time_i - rclcpp::Time(m_ctrl_cmd_vec.at(i - 1).stamp)).seconds(), (curr_time_i - past_delay_time).seconds()); pred_vel += acc * time_to_next_acc; } } - const float64_t last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; - const float64_t time_to_current = + const double last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; + const double time_to_current = (node_->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); pred_vel += last_acc * time_to_current; @@ -922,19 +919,19 @@ float64_t PidLongitudinalController::predictedVelocityInTargetPoint( return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; } -float64_t PidLongitudinalController::applyVelocityFeedback( - const Motion target_motion, const float64_t dt, const float64_t current_vel) +double PidLongitudinalController::applyVelocityFeedback( + const Motion target_motion, const double dt, const double current_vel) { using trajectory_follower::DebugValues; - const float64_t current_vel_abs = std::fabs(current_vel); - const float64_t target_vel_abs = std::fabs(target_motion.vel); - const bool8_t enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate); - const float64_t error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); + 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); + const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); - std::vector pid_contributions(3); - const float64_t pid_acc = + std::vector pid_contributions(3); + const double pid_acc = m_pid_vel.calculate(error_vel_filtered, dt, enable_integration, pid_contributions); - const float64_t feedback_acc = target_motion.acc + pid_acc; + const double feedback_acc = target_motion.acc + pid_acc; m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PID_APPLIED, feedback_acc); m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL_FILTERED, error_vel_filtered); @@ -949,10 +946,10 @@ float64_t PidLongitudinalController::applyVelocityFeedback( } void PidLongitudinalController::updatePitchDebugValues( - const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch) + const double pitch, const double traj_pitch, const double raw_pitch) { using trajectory_follower::DebugValues; - const float64_t to_degrees = (180.0 / static_cast(autoware::common::types::PI)); + 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); m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch); @@ -966,7 +963,7 @@ void PidLongitudinalController::updateDebugVelAcc( const ControlData & control_data) { using trajectory_follower::DebugValues; - const float64_t current_vel = control_data.current_motion.vel; + const double current_vel = control_data.current_motion.vel; const auto interpolated_point = calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose); diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp b/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp index 28cbbb9f59aae..cbfd57e998503 100644 --- a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp +++ b/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp @@ -26,7 +26,7 @@ namespace control namespace trajectory_follower { QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger} {} -bool8_t QPSolverOSQP::solve( +bool QPSolverOSQP::solve( const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) @@ -37,17 +37,17 @@ bool8_t QPSolverOSQP::solve( Eigen::MatrixXd Identity = Eigen::MatrixXd::Identity(dim_u, dim_u); // convert matrix to vector for osqpsolver - std::vector f(&f_vec(0), f_vec.data() + f_vec.cols() * f_vec.rows()); + std::vector f(&f_vec(0), f_vec.data() + f_vec.cols() * f_vec.rows()); - std::vector lower_bound; - std::vector upper_bound; + std::vector lower_bound; + std::vector upper_bound; - for (int64_t i = 0; i < dim_u; ++i) { + for (int i = 0; i < dim_u; ++i) { lower_bound.push_back(lb(i)); upper_bound.push_back(ub(i)); } - for (int64_t i = 0; i < col_a; ++i) { + for (int i = 0; i < col_a; ++i) { lower_bound.push_back(lb_a(i)); upper_bound.push_back(ub_a(i)); } @@ -58,24 +58,24 @@ bool8_t QPSolverOSQP::solve( /* execute optimization */ auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound); - std::vector U_osqp = std::get<0>(result); - u = Eigen::Map>( + std::vector U_osqp = std::get<0>(result); + u = Eigen::Map>( &U_osqp[0], static_cast(U_osqp.size()), 1); - const int64_t status_val = std::get<3>(result); + const int status_val = std::get<3>(result); if (status_val != 1) { // TODO(Horibe): Should return false and the failure must be handled in an appropriate way. RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str()); } // polish status: successful (1), unperformed (0), (-1) unsuccessful - int64_t status_polish = std::get<2>(result); + int status_polish = std::get<2>(result); if (status_polish == -1) { - RCLCPP_WARN(logger_, "osqp status_polish = %ld (unsuccessful)", status_polish); + RCLCPP_WARN(logger_, "osqp status_polish = %d (unsuccessful)", status_polish); return false; } if (status_polish == 0) { - RCLCPP_WARN(logger_, "osqp status_polish = %ld (unperformed)", status_polish); + RCLCPP_WARN(logger_, "osqp status_polish = %d (unperformed)", status_polish); return true; } return true; diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp index 4f10a43875aba..18692d175922b 100644 --- a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp @@ -23,7 +23,7 @@ namespace control namespace trajectory_follower { QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() {} -bool8_t QPSolverEigenLeastSquareLLT::solve( +bool QPSolverEigenLeastSquareLLT::solve( const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & /*a*/, const Eigen::VectorXd & /*lb*/, const Eigen::VectorXd & /*ub*/, const Eigen::VectorXd & /*lb_a*/, const Eigen::VectorXd & /*ub_a*/, Eigen::VectorXd & u) diff --git a/control/trajectory_follower/src/smooth_stop.cpp b/control/trajectory_follower/src/smooth_stop.cpp index fec11d6d71d18..9b5dc14f75248 100644 --- a/control/trajectory_follower/src/smooth_stop.cpp +++ b/control/trajectory_follower/src/smooth_stop.cpp @@ -30,12 +30,12 @@ namespace control { namespace trajectory_follower { -void SmoothStop::init(const float64_t pred_vel_in_target, const float64_t pred_stop_dist) +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()) { + if (pred_stop_dist < std::numeric_limits::epsilon()) { m_strong_acc = m_params.min_strong_acc; return; } @@ -45,10 +45,9 @@ void SmoothStop::init(const float64_t pred_vel_in_target, const float64_t pred_s } void SmoothStop::setParams( - float64_t max_strong_acc, float64_t min_strong_acc, float64_t weak_acc, float64_t weak_stop_acc, - float64_t strong_stop_acc, float64_t min_fast_vel, float64_t min_running_vel, - float64_t min_running_acc, float64_t weak_stop_time, float64_t weak_stop_dist, - float64_t strong_stop_dist) + 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; @@ -67,28 +66,28 @@ void SmoothStop::setParams( m_is_set_params = true; } -std::experimental::optional SmoothStop::calcTimeToStop( - const std::vector> & vel_hist) const +std::experimental::optional SmoothStop::calcTimeToStop( + const std::vector> & vel_hist) const { if (!m_is_set_params) { throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); } // return when vel_hist is empty - const float64_t vel_hist_size = static_cast(vel_hist.size()); + 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(); - float64_t mean_t = 0.0; - float64_t mean_v = 0.0; - float64_t sum_tv = 0.0; - float64_t sum_tt = 0.0; + 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 float64_t t = (vel.first - current_ros_time).seconds(); - const float64_t v = vel.second; + 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; @@ -98,24 +97,22 @@ std::experimental::optional SmoothStop::calcTimeToStop( // 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()) { + if (std::abs(vel_hist_size * mean_t * mean_t - sum_tt) < std::numeric_limits::epsilon()) { return {}; } // calculate coefficients of linear function (v = at + b) - const float64_t a = + const double a = (vel_hist_size * mean_t * mean_v - sum_tv) / (vel_hist_size * mean_t * mean_t - sum_tt); - const float64_t b = mean_v - a * mean_t; + 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()) { + if (std::abs(a) < std::numeric_limits::epsilon()) { return {}; } // calculate time to stop by substituting v = 0 for v = at + b - const float64_t time_to_stop = -b / a; + const double time_to_stop = -b / a; if (time_to_stop > 0) { return time_to_stop; } @@ -123,9 +120,9 @@ std::experimental::optional SmoothStop::calcTimeToStop( return {}; } -float64_t SmoothStop::calculate( - const float64_t stop_dist, const float64_t current_vel, const float64_t current_acc, - const std::vector> & vel_hist, const float64_t delay_time) +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"); @@ -135,9 +132,9 @@ float64_t SmoothStop::calculate( const auto time_to_stop = calcTimeToStop(vel_hist); // calculate some flags - const bool8_t is_fast_vel = std::abs(current_vel) > m_params.min_fast_vel; - const bool8_t is_running = std::abs(current_vel) > m_params.min_running_vel || - std::abs(current_acc) > m_params.min_running_acc; + 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 diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 9576952034b1d..fa7e354b6d318 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -25,12 +25,12 @@ namespace control namespace trajectory_follower { DynamicsBicycleModel::DynamicsBicycleModel( - const float64_t wheelbase, const float64_t mass_fl, const float64_t mass_fr, - const float64_t mass_rl, const float64_t mass_rr, const float64_t cf, const float64_t cr) + const double wheelbase, const double mass_fl, const double mass_fr, const double mass_rl, + const double mass_rr, const double cf, const double cr) : VehicleModelInterface(/* dim_x */ 4, /* dim_u */ 1, /* dim_y */ 2, wheelbase) { - const float64_t mass_front = mass_fl + mass_fr; - const float64_t mass_rear = mass_rl + mass_rr; + const double mass_front = mass_fl + mass_fr; + const double mass_rear = mass_rl + mass_rr; m_mass = mass_front + mass_rear; m_lf = m_wheelbase * (1.0 - mass_front / m_mass); @@ -42,13 +42,13 @@ DynamicsBicycleModel::DynamicsBicycleModel( void DynamicsBicycleModel::calculateDiscreteMatrix( Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const float64_t dt) + const double dt) { /* * x[k+1] = a_d*x[k] + b_d*u + w_d */ - const float64_t vel = std::max(m_velocity, 0.01); + const double vel = std::max(m_velocity, 0.01); a_d = Eigen::MatrixXd::Zero(m_dim_x, m_dim_x); a_d(0, 1) = 1.0; @@ -87,8 +87,8 @@ void DynamicsBicycleModel::calculateDiscreteMatrix( void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { - const float64_t vel = std::max(m_velocity, 0.01); - const float64_t Kv = + const double vel = std::max(m_velocity, 0.01); + const double Kv = m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase); u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature; } diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 152a5aa91cf3a..73c3aabea3fb0 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -25,7 +25,7 @@ namespace control namespace trajectory_follower { KinematicsBicycleModel::KinematicsBicycleModel( - const float64_t wheelbase, const float64_t steer_lim, const float64_t steer_tau) + const double wheelbase, const double steer_lim, const double steer_tau) : VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2, wheelbase) { m_steer_lim = steer_lim; @@ -34,17 +34,17 @@ KinematicsBicycleModel::KinematicsBicycleModel( void KinematicsBicycleModel::calculateDiscreteMatrix( Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const float64_t dt) + const double dt) { - auto sign = [](float64_t x) { return (x > 0.0) - (x < 0.0); }; + auto sign = [](double x) { return (x > 0.0) - (x < 0.0); }; /* Linearize delta around delta_r (reference delta) */ - float64_t delta_r = atan(m_wheelbase * m_curvature); + double delta_r = atan(m_wheelbase * m_curvature); if (std::abs(delta_r) >= m_steer_lim) { - delta_r = m_steer_lim * static_cast(sign(delta_r)); + delta_r = m_steer_lim * static_cast(sign(delta_r)); } - float64_t cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); - float64_t velocity = m_velocity; + double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); + double velocity = m_velocity; if (std::abs(m_velocity) < 1e-04) { velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1); } diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 0c0af519a9198..d9a399c5740c2 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -25,7 +25,7 @@ namespace control namespace trajectory_follower { KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( - const float64_t wheelbase, const float64_t steer_lim) + const double wheelbase, const double steer_lim) : VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2, wheelbase) { m_steer_lim = steer_lim; @@ -33,16 +33,16 @@ KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( void KinematicsBicycleModelNoDelay::calculateDiscreteMatrix( Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const float64_t dt) + const double dt) { - auto sign = [](float64_t x) { return (x > 0.0) - (x < 0.0); }; + auto sign = [](double x) { return (x > 0.0) - (x < 0.0); }; /* Linearize delta around delta_r (reference delta) */ - float64_t delta_r = atan(m_wheelbase * m_curvature); + double delta_r = atan(m_wheelbase * m_curvature); if (std::abs(delta_r) >= m_steer_lim) { - delta_r = m_steer_lim * static_cast(sign(delta_r)); + delta_r = m_steer_lim * static_cast(sign(delta_r)); } - float64_t cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); + double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); a_d << 0.0, m_velocity, 0.0, 0.0; diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp index 122040be315db..52a0d5c7f9c8b 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp @@ -22,17 +22,16 @@ namespace control { namespace trajectory_follower { -VehicleModelInterface::VehicleModelInterface( - int64_t dim_x, int64_t dim_u, int64_t dim_y, float64_t wheelbase) +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) { } -int64_t VehicleModelInterface::getDimX() { return m_dim_x; } -int64_t VehicleModelInterface::getDimU() { return m_dim_u; } -int64_t VehicleModelInterface::getDimY() { return m_dim_y; } -float64_t VehicleModelInterface::getWheelbase() { return m_wheelbase; } -void VehicleModelInterface::setVelocity(const float64_t velocity) { m_velocity = velocity; } -void VehicleModelInterface::setCurvature(const float64_t curvature) { m_curvature = curvature; } +int VehicleModelInterface::getDimX() { return m_dim_x; } +int VehicleModelInterface::getDimU() { return m_dim_u; } +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 control } // namespace motion diff --git a/control/trajectory_follower/test/test_interpolate.cpp b/control/trajectory_follower/test/test_interpolate.cpp index 9b704b0c35c99..69715b024d0dd 100644 --- a/control/trajectory_follower/test/test_interpolate.cpp +++ b/control/trajectory_follower/test/test_interpolate.cpp @@ -12,23 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "common/types.hpp" #include "gtest/gtest.h" #include "trajectory_follower/interpolate.hpp" #include -using autoware::common::types::float64_t; TEST(TestInterpolate, Nominal) { using autoware::motion::control::trajectory_follower::linearInterpolate; // Simple case { - std::vector original_indexes = {1.0, 2.0, 3.0}; - std::vector original_values = {1.0, 2.0, 3.0}; - std::vector target_indexes = {1.5, 2.5}; - std::vector target_values; + std::vector original_indexes = {1.0, 2.0, 3.0}; + std::vector original_values = {1.0, 2.0, 3.0}; + std::vector target_indexes = {1.5, 2.5}; + std::vector target_values; ASSERT_TRUE( linearInterpolate(original_indexes, original_values, target_indexes, target_values)); @@ -39,10 +37,10 @@ TEST(TestInterpolate, Nominal) } // Non regular indexes { - std::vector original_indexes = {1.0, 1.5, 3.0}; - std::vector original_values = {1.0, 2.0, 3.5}; - std::vector target_indexes = {1.25, 2.5, 3.0}; - std::vector target_values; + std::vector original_indexes = {1.0, 1.5, 3.0}; + std::vector original_values = {1.0, 2.0, 3.5}; + std::vector target_indexes = {1.25, 2.5, 3.0}; + std::vector target_values; ASSERT_TRUE( linearInterpolate(original_indexes, original_values, target_indexes, target_values)); @@ -53,10 +51,10 @@ TEST(TestInterpolate, Nominal) } // Single index query { - std::vector original_indexes = {1.0, 1.5, 3.0}; - std::vector original_values = {1.0, 2.0, 3.5}; - float64_t target_index = 1.25; - float64_t target_value; + std::vector original_indexes = {1.0, 1.5, 3.0}; + std::vector original_values = {1.0, 2.0, 3.5}; + double target_index = 1.25; + double target_value; ASSERT_TRUE(linearInterpolate(original_indexes, original_values, target_index, target_value)); EXPECT_EQ(target_value, 1.5); @@ -66,7 +64,7 @@ TEST(TestInterpolate, Failure) { using autoware::motion::control::trajectory_follower::linearInterpolate; - std::vector target_values; + std::vector target_values; // Non increasing indexes ASSERT_FALSE( diff --git a/control/trajectory_follower/test/test_lowpass_filter.cpp b/control/trajectory_follower/test/test_lowpass_filter.cpp index ad950d4704081..691cdebb9b94d 100644 --- a/control/trajectory_follower/test/test_lowpass_filter.cpp +++ b/control/trajectory_follower/test/test_lowpass_filter.cpp @@ -12,19 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "common/types.hpp" #include "gtest/gtest.h" #include "trajectory_follower/lowpass_filter.hpp" #include -using autoware::common::types::float64_t; - TEST(TestLowpassFilter, LowpassFilter1d) { using autoware::motion::control::trajectory_follower::LowpassFilter1d; - const float64_t epsilon = 1e-6; + const double epsilon = 1e-6; LowpassFilter1d lowpass_filter_1d(0.0, 0.1); // initial state @@ -47,14 +44,14 @@ TEST(TestLowpassFilter, MoveAverageFilter) namespace MoveAverageFilter = autoware::motion::control::trajectory_follower::MoveAverageFilter; { // Fail case: window size higher than the vector size - const int64_t window_size = 5; - std::vector vec = {1.0, 2.0, 3.0, 4.0}; + 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; { - const int64_t window_size = 0; - const std::vector original_vec = {1.0, 3.0, 4.0, 6.0}; - std::vector filtered_vec = original_vec; + const int window_size = 0; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0}; + std::vector filtered_vec = original_vec; EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); ASSERT_EQ(filtered_vec.size(), original_vec.size()); for (size_t i = 0; i < filtered_vec.size(); ++i) { @@ -62,9 +59,9 @@ TEST(TestLowpassFilter, MoveAverageFilter) } } { - const int64_t window_size = 1; - const std::vector original_vec = {1.0, 3.0, 4.0, 6.0}; - std::vector filtered_vec = original_vec; + const int window_size = 1; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0}; + std::vector filtered_vec = original_vec; EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); ASSERT_EQ(filtered_vec.size(), original_vec.size()); EXPECT_EQ(filtered_vec[0], original_vec[0]); @@ -73,9 +70,9 @@ TEST(TestLowpassFilter, MoveAverageFilter) EXPECT_EQ(filtered_vec[3], original_vec[3]); } { - const int64_t window_size = 2; - const std::vector original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0}; - std::vector filtered_vec = original_vec; + const int window_size = 2; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0}; + std::vector filtered_vec = original_vec; EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); ASSERT_EQ(filtered_vec.size(), original_vec.size()); EXPECT_EQ(filtered_vec[0], original_vec[0]); @@ -89,15 +86,15 @@ TEST(TestLowpassFilter, MoveAverageFilter) TEST(TestLowpassFilter, Butterworth2dFilter) { using autoware::motion::control::trajectory_follower::Butterworth2dFilter; - const float64_t dt = 1.0; - const float64_t cutoff_hz = 1.0; + const double dt = 1.0; + const double cutoff_hz = 1.0; Butterworth2dFilter filter(dt, cutoff_hz); - for (float64_t i = 1.0; i < 10.0; ++i) { + for (double i = 1.0; i < 10.0; ++i) { EXPECT_LT(filter.filter(i), i); } - const std::vector original_vec = {1.0, 2.0, 3.0, 4.0}; - std::vector filtered_vec; + const std::vector original_vec = {1.0, 2.0, 3.0, 4.0}; + std::vector filtered_vec; filter.filt_vector(original_vec, filtered_vec); ASSERT_EQ(filtered_vec.size(), original_vec.size()); EXPECT_EQ(filtered_vec[0], original_vec[0]); @@ -109,7 +106,7 @@ TEST(TestLowpassFilter, Butterworth2dFilter) filter.filtfilt_vector(original_vec, filtered_vec); ASSERT_EQ(filtered_vec.size(), original_vec.size()); - std::vector coefficients; + std::vector coefficients; filter.getCoefficients(coefficients); EXPECT_EQ(coefficients.size(), size_t(6)); } diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/trajectory_follower/test/test_mpc.cpp index 12e2e4b8a5643..909133dcd65bd 100644 --- a/control/trajectory_follower/test/test_mpc.cpp +++ b/control/trajectory_follower/test/test_mpc.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "common/types.hpp" #include "gtest/gtest.h" #include "trajectory_follower/mpc.hpp" #include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" @@ -38,8 +37,7 @@ namespace { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; @@ -59,33 +57,33 @@ class MPCTest : public ::testing::Test SteeringReport neutral_steer; Pose pose_zero; PoseStamped::SharedPtr pose_zero_ptr; - float64_t default_velocity = 1.0; + double default_velocity = 1.0; rclcpp::Logger logger = rclcpp::get_logger("mpc_test_logger"); // Vehicle model parameters - float64_t wheelbase = 2.7; - float64_t steer_limit = 1.0; - float64_t steer_tau = 0.1; - float64_t mass_fl = 600.0; - float64_t mass_fr = 600.0; - float64_t mass_rl = 600.0; - float64_t mass_rr = 600.0; - float64_t cf = 155494.663; - float64_t cr = 155494.663; + double wheelbase = 2.7; + double steer_limit = 1.0; + double steer_tau = 0.1; + double mass_fl = 600.0; + double mass_fr = 600.0; + double mass_rl = 600.0; + double mass_rr = 600.0; + double cf = 155494.663; + double cr = 155494.663; // Filters parameter - float64_t steering_lpf_cutoff_hz = 3.0; - float64_t error_deriv_lpf_cutoff_hz = 5.0; + double steering_lpf_cutoff_hz = 3.0; + double error_deriv_lpf_cutoff_hz = 5.0; // Test Parameters - float64_t admissible_position_error = 5.0; - float64_t admissible_yaw_error_rad = M_PI_2; - float64_t steer_lim = 0.610865; // 35 degrees - float64_t steer_rate_lim = 2.61799; // 150 degrees - float64_t ctrl_period = 0.03; - float64_t traj_resample_dist = 0.1; - int64_t path_filter_moving_ave_num = 35; - int64_t curvature_smoothing_num_traj = 1; - int64_t curvature_smoothing_num_ref_steer = 35; - bool8_t enable_path_smoothing = true; - bool8_t use_steer_prediction = true; + double admissible_position_error = 5.0; + double admissible_yaw_error_rad = M_PI_2; + double steer_lim = 0.610865; // 35 degrees + double steer_rate_lim = 2.61799; // 150 degrees + double ctrl_period = 0.03; + double traj_resample_dist = 0.1; + int path_filter_moving_ave_num = 35; + int curvature_smoothing_num_traj = 1; + int curvature_smoothing_num_ref_steer = 35; + bool enable_path_smoothing = true; + bool use_steer_prediction = true; void SetUp() override { diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp index d4a20bff24bd7..346fe9a92d56a 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp @@ -56,8 +56,7 @@ using trajectory_follower::LateralOutput; using trajectory_follower::LongitudinalOutput; namespace trajectory_follower_nodes { -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; + namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; /// \classController diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index d27db7a8089c1..0bd1a694f1bd7 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -81,7 +81,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control // Timer { const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(ctrl_period)); + std::chrono::duration(ctrl_period)); timer_control_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); } diff --git a/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp b/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp index 043a4408938ea..1ccffcaf7f3e3 100644 --- a/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp +++ b/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp @@ -31,7 +31,7 @@ using FakeNodeFixture = autoware::tools::testing::FakeTestNode; inline void waitForMessage( const std::shared_ptr & node, FakeNodeFixture * fixture, const bool & received_flag, - const std::chrono::duration max_wait_time = std::chrono::seconds{10LL}, + const std::chrono::duration max_wait_time = std::chrono::seconds{10LL}, const bool fail_on_timeout = true) { const auto dt{std::chrono::milliseconds{100LL}};