From cb975c269ae648d77541f259566a9e0843255962 Mon Sep 17 00:00:00 2001
From: mohammad alqudah <alqudah.mohammad@tier4.jp>
Date: Thu, 19 Dec 2024 11:19:01 +0900
Subject: [PATCH 1/3] modify logic of function isStoppedState
Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
---
.../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 a37f6738d5b149e3fcc666e388b97b54f972834f Mon Sep 17 00:00:00 2001
From: mohammad alqudah <alqudah.mohammad@tier4.jp>
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 <alqudah.mohammad@tier4.jp>
---
.../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 00c5db654d6e850e9c7e31a08bdbcfa50bc70987 Mon Sep 17 00:00:00 2001
From: mohammad alqudah <alqudah.mohammad@tier4.jp>
Date: Fri, 20 Dec 2024 08:30:25 +0900
Subject: [PATCH 3/3] add comment to implementation
Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
---
.../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;