diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 793548d619613..058eb45bfaaff 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -522,8 +522,6 @@ class MPC * @param clock The shared pointer to the RCLCPP clock. */ inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; } - - inline double get_wheelbase_length() { return m_vehicle_model_ptr->getWheelbase(); } }; // class MPC } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index dcdcc77baf5bd..977ec6242104a 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -419,7 +419,7 @@ bool MpcLateralController::isStoppedState() const m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - const auto wheelbase_length = m_mpc->get_wheelbase_length(); + static constexpr double distance_margin = 1.0; const double target_vel = std::invoke([&]() -> double { auto min_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; auto covered_distance = 0.0; @@ -427,7 +427,7 @@ bool MpcLateralController::isStoppedState() const min_vel = std::min(min_vel, m_current_trajectory.points.at(i).longitudinal_velocity_mps); covered_distance += autoware::universe_utils::calcDistance2d( m_current_trajectory.points.at(i - 1).pose, m_current_trajectory.points.at(i).pose); - if (covered_distance > wheelbase_length) break; + if (covered_distance > distance_margin) break; } return min_vel; });