From 509ab10de7cd3b15255cc82c37ffd80d8026ee1b Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Thu, 19 Dec 2024 11:19:01 +0900 Subject: [PATCH 1/3] modify logic of function isStoppedState Signed-off-by: mohammad alqudah --- .../autoware/mpc_lateral_controller/mpc.hpp | 2 ++ .../src/mpc_lateral_controller.cpp | 36 +++++++++++-------- 2 files changed, 23 insertions(+), 15 deletions(-) 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 058eb45bfaaff..793548d619613 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,6 +522,8 @@ 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 7634ebb74eca7..dcdcc77baf5bd 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -399,11 +399,18 @@ Lateral MpcLateralController::getInitialControlCommand() const bool MpcLateralController::isStoppedState() const { + const double current_vel = m_current_kinematic_state.twist.twist.linear.x; // If the nearest index is not found, return false - if (m_current_trajectory.points.empty()) { + if ( + m_current_trajectory.points.empty() || std::fabs(current_vel) > m_stop_state_entry_ego_speed) { return false; } + const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command + if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { + return false; // not stopState: keep control + } + // Note: This function used to take into account the distance to the stop line // for the stop state judgement. However, it has been removed since the steering // control was turned off when approaching/exceeding the stop line on a curve or @@ -412,21 +419,20 @@ 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 double current_vel = m_current_kinematic_state.twist.twist.linear.x; - const double target_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; - - const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command - if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { - return false; // not stopState: keep control - } + const auto wheelbase_length = m_mpc->get_wheelbase_length(); + const double target_vel = std::invoke([&]() -> double { + auto min_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; + auto covered_distance = 0.0; + for (auto i = nearest + 1; i < m_current_trajectory.points.size(); ++i) { + 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; + } + return min_vel; + }); - if ( - std::fabs(current_vel) < m_stop_state_entry_ego_speed && - std::fabs(target_vel) < m_stop_state_entry_target_speed) { - return true; - } else { - return false; - } + return std::fabs(target_vel) < m_stop_state_entry_target_speed; } Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd) From a2dec7a797eb3698c879fff497033a1ddee5cd55 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 20 Dec 2024 08:22:16 +0900 Subject: [PATCH 2/3] use a constant distance margin instead of wheelbase length Signed-off-by: mohammad alqudah --- .../include/autoware/mpc_lateral_controller/mpc.hpp | 2 -- .../src/mpc_lateral_controller.cpp | 4 ++-- 2 files changed, 2 insertions(+), 4 deletions(-) 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; }); From d5e08bbbc672be288071a77a8424bad51014b698 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 20 Dec 2024 08:30:25 +0900 Subject: [PATCH 3/3] add comment to implementation Signed-off-by: mohammad alqudah --- .../src/mpc_lateral_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) 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 977ec6242104a..f8673c3c79c73 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -419,6 +419,9 @@ bool MpcLateralController::isStoppedState() const m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + // It is possible that stop is executed earlier than stop point, and velocity controller + // will not start when the distance from ego to stop point is less than 0.5 meter. + // So we use a distance margin to ensure we can detect stopped state. 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;