From 0befda23024da7c878c7b9c147aa1178a0417c1c Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 26 Sep 2023 19:56:17 +0300 Subject: [PATCH] feat(pid_longitudinal_controller): improve delay and slope compensation Signed-off-by: Berkay Karaman --- .../longitudinal_controller_utils.hpp | 22 +- .../pid_longitudinal_controller.hpp | 44 +-- .../src/longitudinal_controller_utils.cpp | 44 ++- .../src/pid_longitudinal_controller.cpp | 270 +++++++++++------- .../test_longitudinal_controller_utils.cpp | 108 ++++--- 5 files changed, 306 insertions(+), 182 deletions(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 9c01f7bc26c4b..d0d012633b4c0 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -28,6 +28,7 @@ #include #include +#include namespace autoware::motion::control::pid_longitudinal_controller { @@ -60,11 +61,11 @@ double getPitchByPose(const Quaternion & quaternion); * @brief calculate pitch angle from trajectory on map * NOTE: there is currently no z information so this always returns 0.0 * @param [in] trajectory input trajectory - * @param [in] closest_idx nearest index to current vehicle position + * @param [in] start_idx nearest index to current vehicle position * @param [in] wheel_base length of wheel base */ double getPitchByTraj( - const Trajectory & trajectory, const size_t closest_idx, const double wheel_base); + const Trajectory & trajectory, const size_t start_idx, const double wheel_base); /** * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and @@ -88,7 +89,7 @@ Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, c * @param [in] point Interpolated point is nearest to this point. */ template -TrajectoryPoint lerpTrajectoryPoint( +std::pair lerpTrajectoryPoint( const T & points, const Pose & pose, const double max_dist, const double max_yaw) { TrajectoryPoint interpolated_point; @@ -107,6 +108,8 @@ TrajectoryPoint lerpTrajectoryPoint( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); interpolated_point.pose.position.y = interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); + interpolated_point.pose.position.z = interpolation::lerp( + points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio); interpolated_point.pose.orientation = lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); interpolated_point.longitudinal_velocity_mps = interpolation::lerp( @@ -120,7 +123,7 @@ TrajectoryPoint lerpTrajectoryPoint( points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio); } - return interpolated_point; + return std::make_pair(interpolated_point, seg_idx); } /** @@ -144,6 +147,17 @@ double applyDiffLimitFilter( double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val); + +/** + * @brief calculate the projected pose after distance from the current index + * @param [in] src_idx current index + * @param [in] distance distance to project + * @param [in] trajectory reference trajectory + */ +geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( + const size_t src_idx, const double distance, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 921cd3995f6a9..c3a5c8052f01a 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -65,13 +65,25 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double vel{0.0}; double acc{0.0}; }; - + struct StateAfterDelay + { + StateAfterDelay(const double velocity, const double acceleration, const double distance) + : vel(velocity), acc(acceleration), running_distance(distance) + { + } + double vel{0.0}; + double acc{0.0}; + double running_distance{0.0}; + }; enum class Shift { Forward = 0, Reverse }; struct ControlData { bool is_far_from_trajectory{false}; + autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx + size_t target_idx{0}; + StateAfterDelay state_after_delay{0.0, 0.0, 0.0}; Motion current_motion{}; Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation double stop_dist{0.0}; // signed distance that is positive when car is before the stopline @@ -179,7 +191,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_min_jerk; // slope compensation - bool m_use_traj_for_pitch; std::shared_ptr m_lpf_pitch{nullptr}; double m_max_pitch_rad; double m_min_pitch_rad; @@ -271,11 +282,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief calculate control command based on the current control state - * @param [in] current_pose current ego pose * @param [in] control_data control data */ - Motion calcCtrlCmd( - const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); + Motion calcCtrlCmd(const ControlData & control_data); /** * @brief publish control command @@ -304,9 +313,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief calculate direction (forward or backward) that vehicle moves - * @param [in] nearest_idx nearest index on trajectory to vehicle + * @param [in] control_data data for control calculation */ - enum Shift getCurrentShift(const size_t nearest_idx) const; + enum Shift getCurrentShift(const ControlData & control_data) const; /** * @brief filter acceleration command with limitation of acceleration and jerk, and slope @@ -336,8 +345,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] motion delay compensated target motion */ Motion keepBrakeBeforeStop( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, - const size_t nearest_idx) const; + const ControlData & control_data, const Motion & target_motion, const size_t nearest_idx) const; /** * @brief interpolate trajectory point that is nearest to vehicle @@ -345,7 +353,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] point vehicle position * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position */ - autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue( + std::pair + calcInterpolatedTrajPointAndSegment( const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const; @@ -354,18 +363,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] current_motion current velocity and acceleration of the vehicle * @param [in] delay_compensation_time predicted time delay */ - double predictedVelocityInTargetPoint( + StateAfterDelay predictedStateAfterDelay( const Motion current_motion, const double delay_compensation_time) const; /** * @brief calculate velocity feedback with feed forward and pid controller - * @param [in] target_motion reference velocity and acceleration. This acceleration will be used - * as feed forward. - * @param [in] dt time step to use - * @param [in] current_vel current velocity of the vehicle + * @param [in] control_data data for control calculation */ - double applyVelocityFeedback( - const Motion target_motion, const double dt, const double current_vel, const Shift & shift); + double applyVelocityFeedback(const ControlData & control_data); /** * @brief update variables for debugging about pitch @@ -378,12 +383,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief update variables for velocity and acceleration * @param [in] ctrl_cmd latest calculated control command - * @param [in] current_pose current pose of the vehicle * @param [in] control_data data for control calculation */ - void updateDebugVelAcc( - const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, - const ControlData & control_data); + void updateDebugVelAcc(const ControlData & control_data); }; } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 9791a1f0b5e3e..96645fbbb31a9 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -84,7 +84,7 @@ double getPitchByPose(const Quaternion & quaternion_msg) } double getPitchByTraj( - const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base) + const Trajectory & trajectory, const size_t start_idx, const double wheel_base) { // cannot calculate pitch if (trajectory.points.size() <= 1) { @@ -92,17 +92,17 @@ double getPitchByTraj( } const auto [prev_idx, next_idx] = [&]() { - for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { + for (size_t i = start_idx + 1; i < trajectory.points.size(); ++i) { const double dist = tier4_autoware_utils::calcDistance2d( - trajectory.points.at(nearest_idx), trajectory.points.at(i)); + trajectory.points.at(start_idx), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return std::make_pair(nearest_idx, i); + return std::make_pair(start_idx, i); } } // NOTE: The ego pose is close to the goal. return std::make_pair( - std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); + std::min(start_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); }(); return tier4_autoware_utils::calcElevationAngle( @@ -168,5 +168,39 @@ double applyDiffLimitFilter( const double min_val = -max_val; return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); } + +geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( + const size_t src_idx, const double distance, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +{ + double remain_dist = distance; + geometry_msgs::msg::Pose p = trajectory.points.back().pose; + for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) { + const double dist = tier4_autoware_utils::calcDistance3d( + trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose); + if (remain_dist < dist) { + if (remain_dist <= 0.0) { + return trajectory.points.at(i).pose; + } + double ratio = remain_dist / dist; + p = trajectory.points.at(i).pose; + p.position.x = interpolation::lerp( + trajectory.points.at(i).pose.position.x, trajectory.points.at(i + 1).pose.position.x, + ratio); + p.position.y = interpolation::lerp( + trajectory.points.at(i).pose.position.y, trajectory.points.at(i + 1).pose.position.y, + ratio); + p.position.z = interpolation::lerp( + trajectory.points.at(i).pose.position.z, trajectory.points.at(i + 1).pose.position.z, + ratio); + p.orientation = lerpOrientation( + trajectory.points.at(i).pose.orientation, trajectory.points.at(i + 1).pose.orientation, + ratio); + break; + } + remain_dist -= dist; + } + return p; +} } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 644effce50824..dc129d2997aba 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -163,7 +163,6 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) 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 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] @@ -395,7 +394,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( updateControlState(control_data); // calculate control command - const Motion ctrl_cmd = calcCtrlCmd(current_pose, control_data); + const Motion ctrl_cmd = calcCtrlCmd(control_data); // publish control command const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel); @@ -422,11 +421,19 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // current velocity and acceleration control_data.current_motion.vel = m_current_kinematic_state.twist.twist.linear.x; control_data.current_motion.acc = m_current_accel.accel.accel.linear.x; + control_data.interpolated_traj = m_trajectory; // nearest idx - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_trajectory.points, current_pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - const auto & nearest_point = m_trajectory.points.at(nearest_idx).pose; + const auto current_interpolated_pose = + calcInterpolatedTrajPointAndSegment(control_data.interpolated_traj, current_pose); + const size_t nearest_idx = current_interpolated_pose.second + 1; + const auto & nearest_point = current_interpolated_pose.first; + + // Insert the interpolated point + control_data.interpolated_traj.points.insert( + control_data.interpolated_traj.points.begin() + uint(nearest_idx), nearest_point); + control_data.nearest_idx = nearest_idx; + control_data.target_idx = control_data.nearest_idx; // check if the deviation is worth emergency m_diagnostic_data.trans_deviation = @@ -434,7 +441,7 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData const bool is_dist_deviation_large = m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation; m_diagnostic_data.rot_deviation = std::abs(tier4_autoware_utils::normalizeRadian( - tf2::getYaw(nearest_point.orientation) - tf2::getYaw(current_pose.orientation))); + tf2::getYaw(nearest_point.pose.orientation) - tf2::getYaw(current_pose.orientation))); const bool is_yaw_deviation_large = m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation; @@ -443,10 +450,34 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData control_data.is_far_from_trajectory = true; return control_data; } - control_data.nearest_idx = nearest_idx; + + // Delay compensation - Calculate the distance we got, predicted velocity and predicted + // acceleration after delay + control_data.state_after_delay = + predictedStateAfterDelay(control_data.current_motion, m_delay_compensation_time); + + // calculate the target motion + constexpr double min_running_dist = 0.01; + if (control_data.state_after_delay.running_distance > min_running_dist) { + const auto target_pose = longitudinal_utils::findTrajectoryPoseAfterDistance( + control_data.nearest_idx, control_data.state_after_delay.running_distance, + control_data.interpolated_traj); + const auto target_interpolated_point = + calcInterpolatedTrajPointAndSegment(control_data.interpolated_traj, target_pose); + control_data.target_idx = target_interpolated_point.second + 1; + control_data.interpolated_traj.points.insert( + control_data.interpolated_traj.points.begin() + uint(control_data.target_idx), + target_interpolated_point.first); + } + + // send debug values + m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, control_data.state_after_delay.vel); + m_debug_values.setValues( + DebugValues::TYPE::TARGET_VEL, + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps); // shift - control_data.shift = getCurrentShift(control_data.nearest_idx); + control_data.shift = getCurrentShift(control_data); if (control_data.shift != m_prev_shift) { m_pid_vel.reset(); } @@ -454,13 +485,24 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // distance to stopline control_data.stop_dist = longitudinal_utils::calcStopDistance( - current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + control_data.interpolated_traj.points.at(control_data.nearest_idx).pose, + control_data.interpolated_traj, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation); - const double traj_pitch = - longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base); - control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); + const double traj_pitch = longitudinal_utils::getPitchByTraj( + control_data.interpolated_traj, control_data.target_idx, m_wheel_base); + + // if vehicle is not in driving state, send the raw slope angle + if (m_control_state == ControlState::DRIVE) { + control_data.slope_angle = traj_pitch; + m_lpf_pitch->filter(control_data.slope_angle); + } else { + control_data.slope_angle = raw_pitch; + // send filtered slope angle to smooth transition between raw and traj pitch + control_data.slope_angle = m_lpf_pitch->filter(control_data.slope_angle); + } + updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); return control_data; @@ -550,8 +592,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (m_enable_smooth_stop) { if (stopping_condition) { // predictions after input time delay - const double pred_vel_in_target = - predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); + const double pred_vel_in_target = control_data.state_after_delay.vel; const double pred_stop_dist = control_data.stop_dist - 0.5 * (pred_vel_in_target + current_vel) * m_delay_compensation_time; @@ -630,41 +671,31 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d } PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( - const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data) + const ControlData & control_data) { - const size_t nearest_idx = control_data.nearest_idx; - const double current_vel = control_data.current_motion.vel; - const double current_acc = control_data.current_motion.acc; + const size_t target_idx = control_data.target_idx; // velocity and acceleration command - Motion raw_ctrl_cmd{}; - Motion target_motion{}; + Motion raw_ctrl_cmd{ + control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps, + control_data.interpolated_traj.points.at(target_idx).acceleration_mps2}; + if (m_control_state == ControlState::DRIVE) { - const auto target_pose = longitudinal_utils::calcPoseAfterTimeDelay( - current_pose, m_delay_compensation_time, current_vel, current_acc); - const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose); - target_motion = Motion{ - target_interpolated_point.longitudinal_velocity_mps, - target_interpolated_point.acceleration_mps2}; - - target_motion = keepBrakeBeforeStop(m_trajectory, target_motion, nearest_idx); - - const double pred_vel_in_target = - predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); - m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); - - raw_ctrl_cmd.vel = target_motion.vel; - raw_ctrl_cmd.acc = - applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift); + raw_ctrl_cmd.vel = + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; + raw_ctrl_cmd.acc = applyVelocityFeedback(control_data); + RCLCPP_DEBUG( logger_, "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " "feedback_ctrl_cmd.ac: %3.3f", - raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, current_vel, target_motion.vel, + raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, control_data.current_motion.vel, + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps, raw_ctrl_cmd.acc); } else if (m_control_state == ControlState::STOPPING) { raw_ctrl_cmd.acc = m_smooth_stop.calculate( - control_data.stop_dist, current_vel, current_acc, m_vel_hist, m_delay_compensation_time); + control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, + m_vel_hist, m_delay_compensation_time); raw_ctrl_cmd.vel = m_stopped_state_params.vel; RCLCPP_DEBUG( @@ -682,6 +713,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); } + raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx); + // store acceleration without slope compensation m_prev_raw_ctrl_cmd = raw_ctrl_cmd; @@ -690,7 +723,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd}; // update debug visualization - updateDebugVelAcc(target_motion, current_pose, control_data); + updateDebugVelAcc(control_data); return filtered_ctrl_cmd; } @@ -707,7 +740,8 @@ autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController:: // store current velocity history m_vel_hist.push_back({clock_->now(), current_vel}); - while (m_vel_hist.size() > static_cast(0.5 / m_longitudinal_ctrl_period)) { + while (m_vel_hist.size() > + static_cast(m_delay_compensation_time / m_longitudinal_ctrl_period)) { m_vel_hist.erase(m_vel_hist.begin()); } @@ -759,11 +793,12 @@ double PidLongitudinalController::getDt() } enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift( - const size_t nearest_idx) const + const ControlData & control_data) const { constexpr double epsilon = 1e-5; - const double target_vel = m_trajectory.points.at(nearest_idx).longitudinal_velocity_mps; + const double target_vel = + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; if (target_vel > epsilon) { return Shift::Forward; @@ -783,8 +818,11 @@ double PidLongitudinalController::calcFilteredAcc( // store ctrl cmd without slope filter storeAccelCmd(acc_max_filtered); + // if current state is STOPPED, do not apply slope compensation to avoid sudden acceleration const double acc_slope_filtered = - applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift); + m_control_state == ControlState::STOPPED + ? acc_max_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 @@ -834,15 +872,15 @@ double PidLongitudinalController::applySlopeCompensation( } PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, - const size_t nearest_idx) const + const ControlData & control_data, const Motion & target_motion, const size_t nearest_idx) const { Motion output_motion = target_motion; if (m_enable_brake_keeping_before_stop == false) { return output_motion; } - // const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj.points); + const auto traj = control_data.interpolated_traj; + const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj.points); if (!stop_idx) { return output_motion; @@ -867,13 +905,13 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop return output_motion; } -autoware_auto_planning_msgs::msg::TrajectoryPoint -PidLongitudinalController::calcInterpolatedTargetValue( +std::pair +PidLongitudinalController::calcInterpolatedTrajPointAndSegment( const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const { if (traj.points.size() == 1) { - return traj.points.at(0); + return std::make_pair(traj.points.at(0), 0); } // apply linear interpolation @@ -881,69 +919,84 @@ PidLongitudinalController::calcInterpolatedTargetValue( traj.points, pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); } -double PidLongitudinalController::predictedVelocityInTargetPoint( +PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedStateAfterDelay( const Motion current_motion, const double delay_compensation_time) const { 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 double current_vel_abs = std::fabs(current_vel); - if (m_ctrl_cmd_vec.size() == 0) { - const double pred_vel = current_vel + current_acc * delay_compensation_time; + double running_distance = 0.0; + double pred_vel = current_vel; + double pred_acc = current_acc; + + if (m_ctrl_cmd_vec.empty() || m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) { + // check time to stop + const double time_to_stop = -current_vel / current_acc; + const double delay_time_calculation = + time_to_stop > 0.0 && time_to_stop < delay_compensation_time ? time_to_stop + : delay_compensation_time; + // simple linear prediction + pred_vel = current_vel + current_acc * delay_time_calculation; + running_distance = abs( + delay_time_calculation * current_vel + + 0.5 * current_acc * delay_time_calculation * delay_time_calculation); // avoid to change sign of current_vel and pred_vel - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; + return StateAfterDelay{pred_vel, pred_acc, running_distance}; } - double pred_vel = current_vel_abs; - - const auto past_delay_time = - clock_->now() - rclcpp::Duration::from_seconds(delay_compensation_time); for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) { if ((clock_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { - if (i == 0) { - // size of m_ctrl_cmd_vec is less than m_delay_compensation_time - pred_vel = current_vel_abs + - static_cast(m_ctrl_cmd_vec.at(i).acceleration) * delay_compensation_time; - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; - } // add velocity to accel * dt - const 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 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 double time_to_next_acc = + (i == m_ctrl_cmd_vec.size() - 1) + ? std::min( + (clock_->now() - m_ctrl_cmd_vec.back().stamp).seconds(), m_delay_compensation_time) + : std::min( + (rclcpp::Time(m_ctrl_cmd_vec.at(i + 1).stamp) - + rclcpp::Time(m_ctrl_cmd_vec.at(i).stamp)) + .seconds(), + delay_compensation_time); + const double acc = m_ctrl_cmd_vec.at(i).acceleration; + // because acc_cmd is positive when vehicle is running backward + pred_acc = std::copysignf(1.0, static_cast(pred_vel)) * acc; + running_distance += + abs(abs(pred_vel) * time_to_next_acc + 0.5 * acc * time_to_next_acc * time_to_next_acc); + pred_vel += pred_vel < 0.0 ? (-acc * time_to_next_acc) : (acc * time_to_next_acc); + if (pred_vel / current_vel < 0.0) { + // sign of velocity is changed + pred_vel = 0.0; + break; + } } } - const double last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; - const double time_to_current = - (clock_->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); - pred_vel += last_acc * time_to_current; - - // avoid to change sign of current_vel and pred_vel - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; + return StateAfterDelay{pred_vel, pred_acc, running_distance}; } -double PidLongitudinalController::applyVelocityFeedback( - const Motion target_motion, const double dt, const double current_vel, const Shift & shift) +double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data) { - // NOTE: Acceleration command is always positive even if the ego drives backward. - const double vel_sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0.0); - const double diff_vel = (target_motion.vel - current_vel) * vel_sign; - const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && - m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + const double current_vel = control_data.current_motion.vel; + const double target_vel = + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; + + const double vel_after_delay = control_data.state_after_delay.vel; + const bool is_under_control = + m_current_operation_mode.is_autoware_control_enabled && + m_current_operation_mode.mode == + OperationModeState::AUTONOMOUS; // NOTE: Acceleration command is always positive even if the + // ego drives backward. + const double vel_sign = (control_data.shift == Shift::Forward) + ? 1.0 + : (control_data.shift == Shift::Reverse ? -1.0 : 0.0); + const double diff_vel = (target_vel - vel_after_delay) * vel_sign; + const bool enable_integration = - (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control; + (abs(vel_after_delay) > m_current_vel_threshold_pid_integrate) && is_under_control; + const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3); const double pid_acc = - m_pid_vel.calculate(error_vel_filtered, dt, enable_integration, pid_contributions); + m_pid_vel.calculate(error_vel_filtered, control_data.dt, enable_integration, pid_contributions); // Feedforward scaling: // This is for the coordinate conversion where feedforward is applied, from Time to Arclength. @@ -953,8 +1006,9 @@ double PidLongitudinalController::applyVelocityFeedback( constexpr double ff_scale_max = 2.0; // for safety constexpr double ff_scale_min = 0.5; // for safety const double ff_scale = std::clamp( - std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max); - const double ff_acc = target_motion.acc * ff_scale; + std::abs(current_vel) / std::max(std::abs(target_vel), 0.1), ff_scale_min, ff_scale_max); + const double ff_acc = + control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale; const double feedback_acc = ff_acc + pid_acc; @@ -981,21 +1035,25 @@ void PidLongitudinalController::updatePitchDebugValues( m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); } -void PidLongitudinalController::updateDebugVelAcc( - const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, - const ControlData & control_data) +void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_data) { - const double current_vel = control_data.current_motion.vel; - - const auto interpolated_point = calcInterpolatedTargetValue(m_trajectory, current_pose); - - m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); - m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); - m_debug_values.setValues(DebugValues::TYPE::TARGET_ACC, target_motion.acc); + m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, control_data.current_motion.vel); + m_debug_values.setValues( + DebugValues::TYPE::TARGET_VEL, + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps); + m_debug_values.setValues( + DebugValues::TYPE::TARGET_ACC, + control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2); + m_debug_values.setValues( + DebugValues::TYPE::NEAREST_VEL, + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps); + m_debug_values.setValues( + DebugValues::TYPE::NEAREST_ACC, + control_data.interpolated_traj.points.at(control_data.nearest_idx).acceleration_mps2); m_debug_values.setValues( - DebugValues::TYPE::NEAREST_VEL, interpolated_point.longitudinal_velocity_mps); - m_debug_values.setValues(DebugValues::TYPE::NEAREST_ACC, interpolated_point.acceleration_mps2); - m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel); + DebugValues::TYPE::ERROR_VEL, + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps - + control_data.current_motion.vel); } void PidLongitudinalController::setupDiagnosticUpdater() diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 5c7698180f82b..d410a0aef4142 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -115,7 +115,7 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj) { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; - const double wheel_base = 0.9; + const double wheel_base = 1.0; /** * Trajectory: * 1 X @@ -130,32 +130,30 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj) point.pose.position.z = 0.0; traj.points.push_back(point); // non stopping trajectory: stop distance = trajectory length - point.pose.position.x = 1.0; + point.pose.position.x = 0.6; point.pose.position.y = 0.0; - point.pose.position.z = 1.0; + point.pose.position.z = 0.8; traj.points.push_back(point); - point.pose.position.x = 2.0; + point.pose.position.x = 1.2; point.pose.position.y = 0.0; point.pose.position.z = 0.0; traj.points.push_back(point); - point.pose.position.x = 3.0; + point.pose.position.x = 1.8; point.pose.position.y = 0.0; - point.pose.position.z = 0.5; + point.pose.position.z = 0.8; traj.points.push_back(point); size_t closest_idx = 0; EXPECT_DOUBLE_EQ( - std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4); + longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6)); closest_idx = 1; EXPECT_DOUBLE_EQ( - std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4); + longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(0.8, 0.6)); closest_idx = 2; EXPECT_DOUBLE_EQ( - std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), - std::atan2(0.5, 1)); + longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6)); closest_idx = 3; EXPECT_DOUBLE_EQ( - std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), - std::atan2(0.5, 1)); + longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6)); } TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) @@ -353,95 +351,113 @@ TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) TrajectoryPoint p; p.pose.position.x = 0.0; p.pose.position.y = 0.0; + p.pose.position.z = 0.0; p.longitudinal_velocity_mps = 10.0; p.acceleration_mps2 = 10.0; points.push_back(p); p.pose.position.x = 1.0; p.pose.position.y = 0.0; + p.pose.position.z = 0.0; p.longitudinal_velocity_mps = 20.0; p.acceleration_mps2 = 20.0; points.push_back(p); p.pose.position.x = 1.0; p.pose.position.y = 1.0; + p.pose.position.z = 1.0; p.longitudinal_velocity_mps = 30.0; p.acceleration_mps2 = 30.0; points.push_back(p); p.pose.position.x = 2.0; p.pose.position.y = 1.0; + p.pose.position.z = 2.0; p.longitudinal_velocity_mps = 40.0; p.acceleration_mps2 = 40.0; points.push_back(p); - TrajectoryPoint result; Pose pose; double max_dist = 3.0; double max_yaw = 0.7; // Points on the trajectory gives back the original trajectory points values pose.position.x = 0.0; pose.position.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 10.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 10.0, abs_err); + pose.position.z = 0.0; + + auto result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 10.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 10.0, abs_err); pose.position.x = 1.0; pose.position.y = 0.0; + pose.position.z = 0.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 20.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 20.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 20.0, abs_err); pose.position.x = 1.0; pose.position.y = 1.0; + pose.position.z = 1.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 30.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 30.0, abs_err); pose.position.x = 2.0; pose.position.y = 1.0; + pose.position.z = 2.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 40.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 40.0, abs_err); // Interpolate between trajectory points pose.position.x = 0.5; pose.position.y = 0.0; + pose.position.z = 0.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err); pose.position.x = 0.75; pose.position.y = 0.0; + pose.position.z = 0.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 17.5, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 17.5, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 17.5, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 17.5, abs_err); // Interpolate away from the trajectory (interpolated point is projected) pose.position.x = 0.5; pose.position.y = -1.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, 0.0, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err); // Ambiguous projections: possibility with the lowest index is used pose.position.x = 0.5; pose.position.y = 0.5; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, 0.0, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err); } TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter)