From 0f5e363b3ba6f1199c998ac9771a69a69b154cbf Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 20 May 2024 18:02:41 +0900 Subject: [PATCH 01/11] align the MPC steering angle when the car is controlled manually. Signed-off-by: Zhe Shen --- control/autoware_mpc_lateral_controller/src/mpc.cpp | 4 ++++ .../src/mpc_lateral_controller.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 309704596086e..4efa0f4c94a3d 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -102,6 +102,10 @@ bool MPC::calculateMPC( // save previous input for the mpc rate limit m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); + if (!is_under_control) { + m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; + m_raw_steer_cmd_prev = mpc_data.steer; + } /* calculate predicted trajectory */ predicted_trajectory = 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 6aedb74e87c83..b77697faeb6bf 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -253,7 +253,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( } const bool is_mpc_solved = m_mpc->calculateMPC( - m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); + m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values, is_under_control); // reset previous MPC result // Note: When a large deviation from the trajectory occurs, the optimization stops and From 7c20ce0fc097ea50ac607fbf6f8cee9bc9939084 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 27 May 2024 14:37:33 +0900 Subject: [PATCH 02/11] update the condition for is_driving_manually Signed-off-by: Zhe Shen --- .../include/autoware/mpc_lateral_controller/mpc.hpp | 1 - control/autoware_mpc_lateral_controller/src/mpc.cpp | 2 +- .../src/mpc_lateral_controller.cpp | 5 ++++- 3 files changed, 5 insertions(+), 3 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..9a3c8c53597b7 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 @@ -443,7 +443,6 @@ class MPC bool calculateMPC( const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic); - /** * @brief Set the reference trajectory to be followed. * @param trajectory_msg The reference trajectory message. diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 4efa0f4c94a3d..1cefdfe0a10ea 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -102,7 +102,7 @@ bool MPC::calculateMPC( // save previous input for the mpc rate limit m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); - if (!is_under_control) { + if (is_driving_manually) { m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = mpc_data.steer; } 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 b77697faeb6bf..f50a0eaece8a9 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -252,8 +252,11 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_is_ctrl_cmd_prev_initialized = true; } + const bool is_driving_manually = input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL || + input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE; + const bool is_mpc_solved = m_mpc->calculateMPC( - m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values, is_under_control); + m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values, is_driving_manually); // reset previous MPC result // Note: When a large deviation from the trajectory occurs, the optimization stops and From 6e3b0c99cb52df73cd75a81e55a1e0544b02a47e Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 27 May 2024 14:54:44 +0900 Subject: [PATCH 03/11] STOP mode included Signed-off-by: Zhe Shen --- .../src/mpc_lateral_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 f50a0eaece8a9..1f177d67c6ce2 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -253,7 +253,8 @@ trajectory_follower::LateralOutput MpcLateralController::run( } const bool is_driving_manually = input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL || - input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE; + input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE || + input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::STOP; const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values, is_driving_manually); From 6167823a3dbca2ef373a27879a3cd02fd38c4636 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 27 May 2024 15:37:21 +0900 Subject: [PATCH 04/11] comment the is_driving_manually Signed-off-by: Zhe Shen --- control/autoware_mpc_lateral_controller/src/mpc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 1cefdfe0a10ea..4f539053cb9c2 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -102,7 +102,7 @@ bool MPC::calculateMPC( // save previous input for the mpc rate limit m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); - if (is_driving_manually) { + if (is_driving_manually) { // align the MPC steering angle to the actual steering angle when the vehicle is manually controlled m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = mpc_data.steer; } From 5a1880305ed0762788a8fa044bac108ade861339 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 3 Jun 2024 17:29:33 +0900 Subject: [PATCH 05/11] align the steering outside (after) the solver. Signed-off-by: Zhe Shen --- control/autoware_mpc_lateral_controller/src/mpc.cpp | 4 ---- .../src/mpc_lateral_controller.cpp | 4 ++-- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 4f539053cb9c2..309704596086e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -102,10 +102,6 @@ bool MPC::calculateMPC( // save previous input for the mpc rate limit m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); - if (is_driving_manually) { // align the MPC steering angle to the actual steering angle when the vehicle is manually controlled - m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; - m_raw_steer_cmd_prev = mpc_data.steer; - } /* calculate predicted trajectory */ predicted_trajectory = 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 1f177d67c6ce2..0b01996065c17 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -257,14 +257,14 @@ trajectory_follower::LateralOutput MpcLateralController::run( input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::STOP; const bool is_mpc_solved = m_mpc->calculateMPC( - m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values, is_driving_manually); + m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); // reset previous MPC result // Note: When a large deviation from the trajectory occurs, the optimization stops and // the vehicle will return to the path by re-planning the trajectory or external operation. // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. - if (!is_mpc_solved) { + if (!is_mpc_solved||is_driving_manually) { m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd); From 60442b7482de9720f228e566bd1dde3754d9ab69 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 5 Jun 2024 10:53:25 +0900 Subject: [PATCH 06/11] use the flag input_data.current_operation_mode.is_autoware_control_enabled Signed-off-by: Zhe Shen --- .../src/mpc_lateral_controller.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) 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 0b01996065c17..d85812f9959f8 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -252,10 +252,6 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_is_ctrl_cmd_prev_initialized = true; } - const bool is_driving_manually = input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL || - input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE || - input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::STOP; - const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); @@ -264,7 +260,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( // the vehicle will return to the path by re-planning the trajectory or external operation. // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. - if (!is_mpc_solved||is_driving_manually) { + if (!is_mpc_solved||!input_data.current_operation_mode.is_autoware_control_enabled) { m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd); From 1e8bddb3f7131dcdf379de4339712994d4f01b57 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 5 Jun 2024 11:01:25 +0900 Subject: [PATCH 07/11] correct a typo Signed-off-by: Zhe Shen --- control/autoware_mpc_lateral_controller/src/mpc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 309704596086e..23235fe445e24 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -244,7 +244,7 @@ void MPC::setReferenceTrajectory( void MPC::resetPrevResult(const SteeringReport & current_steer) { - // Consider limit. The prev value larger than limitation brakes the optimization constraint and + // Consider limit. The prev value larger than limitation breaks the optimization constraint and // results in optimization failure. const float steer_lim_f = static_cast(m_steer_lim); m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f); From cab2c35ada9b025454bb2538c2e43640896924fc Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 14 Jun 2024 14:55:36 +0900 Subject: [PATCH 08/11] correct the under control condition check Signed-off-by: Zhe Shen --- .../src/mpc_lateral_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 d85812f9959f8..51c717332edac 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -260,7 +260,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( // the vehicle will return to the path by re-planning the trajectory or external operation. // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. - if (!is_mpc_solved||!input_data.current_operation_mode.is_autoware_control_enabled) { + if (!is_mpc_solved||!is_under_control) { m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd); From a268542c0b7064a5c863a2d4823144dd971b95ad Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 17 Jun 2024 11:07:38 +0900 Subject: [PATCH 09/11] undo the space delete Signed-off-by: Zhe Shen --- .../include/autoware/mpc_lateral_controller/mpc.hpp | 1 + 1 file changed, 1 insertion(+) 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 9a3c8c53597b7..2ad8b70c77aef 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 @@ -443,6 +443,7 @@ class MPC bool calculateMPC( const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic); + /** * @brief Set the reference trajectory to be followed. * @param trajectory_msg The reference trajectory message. From 5e044bf420649666e37a22107655f91fcf1a67ef Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 17 Jun 2024 11:08:38 +0900 Subject: [PATCH 10/11] unchange the unrelevant line Signed-off-by: Zhe Shen --- .../include/autoware/mpc_lateral_controller/mpc.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2ad8b70c77aef..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 @@ -443,7 +443,7 @@ class MPC bool calculateMPC( const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic); - + /** * @brief Set the reference trajectory to be followed. * @param trajectory_msg The reference trajectory message. From 0eeaf6d0e4fd4e26e8f85564072c9a20cfe3be00 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Thu, 20 Jun 2024 09:03:52 +0900 Subject: [PATCH 11/11] pre-commit Signed-off-by: Zhe Shen --- .../src/mpc_lateral_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 51c717332edac..04ae688e0e54f 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -260,7 +260,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( // the vehicle will return to the path by re-planning the trajectory or external operation. // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. - if (!is_mpc_solved||!is_under_control) { + if (!is_mpc_solved || !is_under_control) { m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd);