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 8685d6264993b..dddb9673d4489 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 @@ -60,25 +60,17 @@ 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 elevation angle */ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); -/** - * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and - * acceleration for delayed time - */ -Pose calcPoseAfterTimeDelay( - const Pose & current_pose, const double delay_time, const double current_vel, - const double current_acc); - /** * @brief apply linear interpolation to orientation * @param [in] o_from first orientation @@ -93,7 +85,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; @@ -112,6 +104,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( @@ -125,7 +119,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); } /** @@ -149,6 +143,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 41d98a4d15541..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); + 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 70cdbcaf17bd2..73f9a56a4f6bb 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -84,36 +84,25 @@ 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) { return 0.0; } - for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance2d( - trajectory.points.at(nearest_idx), trajectory.points.at(i)); - if (dist > wheel_base) { - // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i)); - } - } - - // close to goal - for (size_t i = trajectory.points.size() - 1; i > 0; --i) { - const double dist = - tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i)); - - if (dist > wheel_base) { - // calculate pitch from trajectory - // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) - return calcElevationAngle(trajectory.points.at(i), trajectory.points.back()); - } + // if start_idx is the last idx of the trajectory + if (start_idx == trajectory.points.size() - 1) { + return calcElevationAngle(trajectory.points.at(start_idx - 1), trajectory.points.at(start_idx)); } + // find the trajectory point after wheelbase + const auto pose_after_distance = + findTrajectoryPoseAfterDistance(start_idx, wheel_base, trajectory); + TrajectoryPoint point_after_distance = trajectory.points.back(); + point_after_distance.pose = pose_after_distance; // calculate pitch from trajectory between the beginning and end of trajectory - return calcElevationAngle(trajectory.points.at(0), trajectory.points.back()); + return calcElevationAngle(trajectory.points.at(start_idx), point_after_distance); } double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) @@ -128,33 +117,6 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint return pitch; } -Pose calcPoseAfterTimeDelay( - const Pose & current_pose, const double delay_time, const double current_vel, - const double current_acc) -{ - if (delay_time <= 0.0) { - return current_pose; - } - - // 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_time ? time_to_stop : delay_time; - // simple linear prediction - const double yaw = tf2::getYaw(current_pose.orientation); - const double running_distance = delay_time_calculation * current_vel + 0.5 * current_acc * - delay_time_calculation * - delay_time_calculation; - const double dx = running_distance * std::cos(yaw); - const double dy = running_distance * std::sin(yaw); - - auto pred_pose = current_pose; - pred_pose.position.x += dx; - pred_pose.position.y += dy; - return pred_pose; -} - double lerp(const double v_from, const double v_to, const double ratio) { return v_from + (v_to - v_from) * ratio; @@ -187,5 +149,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 6d3489811b195..a09cc36c51f82 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; @@ -546,8 +588,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; @@ -620,40 +661,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{}; - 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}; + 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}; - 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); + if (m_control_state == ControlState::DRIVE) { + raw_ctrl_cmd.vel = + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; + raw_ctrl_cmd.acc = applyVelocityFeedback(control_data); - raw_ctrl_cmd.vel = target_motion.vel; - raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); 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( @@ -671,6 +703,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; @@ -679,7 +713,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; } @@ -696,7 +730,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()); } @@ -748,11 +783,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; @@ -772,8 +808,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 @@ -823,15 +862,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; @@ -856,13 +895,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 @@ -870,67 +909,73 @@ 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, float(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) +double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data) { - const double current_vel_abs = std::fabs(current_vel); - const double target_vel_abs = std::fabs(target_motion.vel); + const double current_vel_abs = std::fabs(control_data.current_motion.vel); + const double target_vel_abs = std::fabs( + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps); + const double vel_after_delay_abs = std::fabs(control_data.state_after_delay.vel); const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; const bool enable_integration = - (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control; - const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); + (vel_after_delay_abs > m_current_vel_threshold_pid_integrate) && is_under_control; + const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - vel_after_delay_abs); 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. @@ -941,7 +986,8 @@ double PidLongitudinalController::applyVelocityFeedback( constexpr double ff_scale_min = 0.5; // for safety const double ff_scale = std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max); - const double ff_acc = target_motion.acc * ff_scale; + 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; @@ -968,21 +1014,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 d9fc404ce6abe..55755a7f3e9ba 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, calcElevationAngle) @@ -185,136 +183,6 @@ TEST(TestLongitudinalControllerUtils, calcElevationAngle) EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); } -TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) -{ - using geometry_msgs::msg::Pose; - const double abs_err = 1e-7; - Pose current_pose; - current_pose.position.x = 0.0; - current_pose.position.y = 0.0; - current_pose.position.z = 0.0; - tf2::Quaternion quaternion_tf; - quaternion_tf.setRPY(0.0, 0.0, 0.0); - current_pose.orientation = tf2::toMsg(quaternion_tf); - - // With a delay acceleration and/or a velocity of 0.0 there is no change of position - double delay_time = 0.0; - double current_vel = 0.0; - double current_acc = 0.0; - Pose delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - delay_time = 1.0; - current_vel = 0.0; - current_acc = 0.0; - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - delay_time = 0.0; - current_vel = 1.0; - current_acc = 0.0; - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - // With both delay and velocity: change of position - delay_time = 1.0; - current_vel = 1.0; - current_acc = 0.0; - - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - // With all, acceleration, delay and velocity: change of position - delay_time = 1.0; - current_vel = 1.0; - current_acc = 1.0; - - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR( - delayed_pose.position.x, - current_pose.position.x + current_vel * delay_time + - 0.5 * current_acc * delay_time * delay_time, - abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - // Vary the yaw - quaternion_tf.setRPY(0.0, 0.0, M_PI); - current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR( - delayed_pose.position.x, - current_pose.position.x - current_vel * delay_time - - 0.5 * current_acc * delay_time * delay_time, - abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - quaternion_tf.setRPY(0.0, 0.0, M_PI_2); - current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR( - delayed_pose.position.y, - current_pose.position.y + current_vel * delay_time + - 0.5 * current_acc * delay_time * delay_time, - abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - quaternion_tf.setRPY(0.0, 0.0, -M_PI_2); - current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR( - delayed_pose.position.y, - current_pose.position.y - current_vel * delay_time - - 0.5 * current_acc * delay_time * delay_time, - abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - // Vary the pitch : no effect /!\ NOTE: bug with roll of +-PI/2 which rotates the yaw by PI - quaternion_tf.setRPY(0.0, M_PI_4, 0.0); - current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR( - delayed_pose.position.x, - current_pose.position.x + current_vel * delay_time + - 0.5 * current_acc * delay_time * delay_time, - abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); - - // Vary the roll : no effect - quaternion_tf.setRPY(M_PI_2, 0.0, 0.0); - current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); - EXPECT_NEAR( - delayed_pose.position.x, - current_pose.position.x + current_vel * delay_time + - 0.5 * current_acc * delay_time * delay_time, - abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); - EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); -} - TEST(TestLongitudinalControllerUtils, lerpOrientation) { geometry_msgs::msg::Quaternion result; @@ -371,105 +239,123 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, M_PI_4 / 2); } -TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) + TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using geometry_msgs::msg::Pose; - const double abs_err = 1e-15; - decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; - TrajectoryPoint p; - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 10.0; - p.acceleration_mps2 = 10.0; - points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 20.0; - p.acceleration_mps2 = 20.0; - points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 1.0; - p.longitudinal_velocity_mps = 30.0; - p.acceleration_mps2 = 30.0; - points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 1.0; - p.longitudinal_velocity_mps = 40.0; - p.acceleration_mps2 = 40.0; - points.push_back(p); - TrajectoryPoint result; - 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.x = 1.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, 20.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err); - - pose.position.x = 1.0; - 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, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err); - - pose.position.x = 2.0; - 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, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err); - - // Interpolate between trajectory points - pose.position.x = 0.5; - 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, 15.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); - pose.position.x = 0.75; - 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, 17.5, abs_err); - EXPECT_NEAR(result.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); - - // 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); -} + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using geometry_msgs::msg::Pose; + const double abs_err = 1e-15; + decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; + TrajectoryPoint p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.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); + 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; + 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.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.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.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.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.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.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.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) {