diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 0854340ed24a5..186481e92cdc4 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -447,7 +447,8 @@ class MPC * @param param Trajectory filtering parameters. */ void setReferenceTrajectory( - const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param); + const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param, + const Odometry & current_kinematics); /** * @brief Reset the previous result of MPC. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index ee33062854ab9..911a39052f4dd 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -181,7 +181,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @brief Set the current trajectory using the received message. * @param msg Received trajectory message. */ - void setTrajectory(const Trajectory & msg); + void setTrajectory(const Trajectory & msg, const Odometry & current_kinematics); /** * @brief Check if the received data is valid. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 89136f294aa18..036e6a32a9b83 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -99,7 +99,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const double resample_interval_dist); + const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, + const double ego_offset_to_segment); /** * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired @@ -122,14 +123,14 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj); /** * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing - * @param [in] start_idx index of the trajectory point from which to start smoothing - * @param [in] start_vel initial velocity to set at the start_idx + * @param [in] start_seg_idx segment index of the trajectory point from which to start smoothing + * @param [in] start_vel initial velocity to set at the start_seg_idx * @param [in] acc_lim limit on the acceleration * @param [in] tau constant to control the smoothing (high-value = very smooth) * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity */ void dynamicSmoothingVelocity( - const size_t start_idx, const double start_vel, const double acc_lim, const double tau, + const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj); /** diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 643f8a6f91023..8ff323727f25e 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -173,13 +173,20 @@ Float32MultiArrayStamped MPC::generateDiagData( } void MPC::setReferenceTrajectory( - const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param) + const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param, + const Odometry & current_kinematics) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position); + const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg); // resampling - const auto [success_resample, mpc_traj_resampled] = - MPCUtils::resampleMPCTrajectoryByDistance(mpc_traj_raw, param.traj_resample_dist); + const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance( + mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment); if (!success_resample) { warn_throttle("[setReferenceTrajectory] spline error when resampling by distance"); return; @@ -389,13 +396,13 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter( return input; } - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); MPCTrajectory output = input; MPCUtils::dynamicSmoothingVelocity( - nearest_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit, + nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit, m_param.velocity_time_constant, output); auto last_point = output.back(); diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index af680b5050037..c86cc0ea91577 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -227,7 +227,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( trajectory_follower::InputData const & input_data) { // set input data - setTrajectory(input_data.current_trajectory); + setTrajectory(input_data.current_trajectory, input_data.current_odometry); m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; @@ -319,7 +319,7 @@ bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data) { - setTrajectory(input_data.current_trajectory); + setTrajectory(input_data.current_trajectory, input_data.current_odometry); m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; @@ -339,7 +339,8 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_ return true; } -void MpcLateralController::setTrajectory(const Trajectory & msg) +void MpcLateralController::setTrajectory( + const Trajectory & msg, const Odometry & current_kinematics) { m_current_trajectory = msg; @@ -353,7 +354,7 @@ void MpcLateralController::setTrajectory(const Trajectory & msg) return; } - m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param); + m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(m_current_trajectory); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index 891a48299c81d..8624fc448104a 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -27,6 +27,19 @@ namespace autoware::motion::control::mpc_lateral_controller { +namespace +{ +double calcLongitudinalOffset( + const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back, + const geometry_msgs::msg::Point & p_target) +{ + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; + + return segment_vec.dot(target_vec) / segment_vec.norm(); +} +} // namespace + namespace MPCUtils { using tier4_autoware_utils::calcDistance2d; @@ -77,7 +90,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const double resample_interval_dist) + const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, + const double ego_offset_to_segment) { MPCTrajectory output; @@ -92,7 +106,18 @@ std::pair resampleMPCTrajectoryByDistance( } std::vector output_arclength; - for (double s = 0; s < input_arclength.back(); s += resample_interval_dist) { + // To accurately sample the ego point, resample separately in the forward direction and the + // backward direction from the current position. + for (double s = std::clamp( + input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0, + input_arclength.back() - 1e-6); + 0 <= s; s -= resample_interval_dist) { + output_arclength.push_back(s); + } + std::reverse(output_arclength.begin(), output_arclength.end()); + for (double s = std::max(input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0) + + resample_interval_dist; + s < input_arclength.back(); s += resample_interval_dist) { output_arclength.push_back(s); } @@ -274,13 +299,17 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj) } void dynamicSmoothingVelocity( - const size_t start_idx, const double start_vel, const double acc_lim, const double tau, + const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj) { double curr_v = start_vel; - traj.vx.at(start_idx) = start_vel; + // set current velocity in both start and end point of the segment + traj.vx.at(start_seg_idx) = start_vel; + if (1 < traj.vx.size()) { + traj.vx.at(start_seg_idx + 1) = start_vel; + } - for (size_t i = start_idx + 1; i < traj.size(); ++i) { + for (size_t i = start_seg_idx + 2; i < traj.size(); ++i) { const double ds = calcDistance2d(traj, i, i - 1); const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits::epsilon()); const double a = tau / std::max(tau + dt, std::numeric_limits::epsilon()); @@ -320,29 +349,40 @@ bool calcNearestPoseInterp( return true; } - auto calcSquaredDist = [](const Pose & p, const MPCTrajectory & t, const size_t idx) { - const double dx = p.position.x - t.x.at(idx); - const double dy = p.position.y - t.y.at(idx); - return dx * dx + dy * dy; - }; - /* get second nearest index = next to nearest_index */ - const size_t next = static_cast( - std::min(static_cast(*nearest_index) + 1, static_cast(traj_size) - 1)); - const size_t prev = - static_cast(std::max(static_cast(*nearest_index) - 1, static_cast(0))); - const double dist_to_next = calcSquaredDist(self_pose, traj, next); - const double dist_to_prev = calcSquaredDist(self_pose, traj, prev); - const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev; - - const double a_sq = calcSquaredDist(self_pose, traj, *nearest_index); - const double b_sq = calcSquaredDist(self_pose, traj, second_nearest_index); - const double dx3 = traj.x.at(*nearest_index) - traj.x.at(second_nearest_index); - const double dy3 = traj.y.at(*nearest_index) - traj.y.at(second_nearest_index); - const double c_sq = dx3 * dx3 + dy3 * dy3; + const auto [prev, next] = [&]() -> std::pair { + if (*nearest_index == 0) { + return std::make_pair(0, 1); + } + if (*nearest_index == traj_size - 1) { + return std::make_pair(traj_size - 2, traj_size - 1); + } + geometry_msgs::msg::Point nearest_traj_point; + nearest_traj_point.x = traj.x.at(*nearest_index); + nearest_traj_point.y = traj.y.at(*nearest_index); + geometry_msgs::msg::Point next_nearest_traj_point; + next_nearest_traj_point.x = traj.x.at(*nearest_index + 1); + next_nearest_traj_point.y = traj.y.at(*nearest_index + 1); + + const double signed_length = + calcLongitudinalOffset(nearest_traj_point, next_nearest_traj_point, self_pose.position); + if (signed_length <= 0) { + return std::make_pair(*nearest_index - 1, *nearest_index); + } + return std::make_pair(*nearest_index, *nearest_index + 1); + }(); + + geometry_msgs::msg::Point next_traj_point; + next_traj_point.x = traj.x.at(next); + next_traj_point.y = traj.y.at(next); + geometry_msgs::msg::Point prev_traj_point; + prev_traj_point.x = traj.x.at(prev); + prev_traj_point.y = traj.y.at(prev); + const double traj_seg_length = + tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point); /* if distance between two points are too close */ - if (c_sq < 1.0E-5) { + if (traj_seg_length < 1.0E-5) { nearest_pose->position.x = traj.x.at(*nearest_index); nearest_pose->position.y = traj.y.at(*nearest_index); nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index)); @@ -351,18 +391,15 @@ bool calcNearestPoseInterp( } /* linear interpolation */ - const double alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0); - nearest_pose->position.x = - alpha * traj.x.at(*nearest_index) + (1 - alpha) * traj.x.at(second_nearest_index); - nearest_pose->position.y = - alpha * traj.y.at(*nearest_index) + (1 - alpha) * traj.y.at(second_nearest_index); - const double tmp_yaw_err = - normalizeRadian(traj.yaw.at(*nearest_index) - traj.yaw.at(second_nearest_index)); - const double nearest_yaw = - normalizeRadian(traj.yaw.at(second_nearest_index) + alpha * tmp_yaw_err); + const double ratio = std::clamp( + calcLongitudinalOffset(prev_traj_point, next_traj_point, self_pose.position) / traj_seg_length, + 0.0, 1.0); + nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next); + nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next); + const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next)); + const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err); nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw); - *nearest_time = alpha * traj.relative_time.at(*nearest_index) + - (1 - alpha) * traj.relative_time.at(second_nearest_index); + *nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next); return true; } diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 6f8a6fb598058..dade035daf26c 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -163,7 +163,9 @@ class MPCTest : public ::testing::Test mpc.initializeSteeringPredictor(); // Init trajectory - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); } nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) @@ -221,7 +223,9 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) // Init parameters and reference trajectory initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; @@ -237,7 +241,8 @@ TEST_F(MPCTest, OsqpCalculate) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); @@ -262,7 +267,9 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); @@ -300,7 +307,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) // Init filters mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Init trajectory - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; @@ -315,7 +323,9 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit);