From 2d99ea8d088ac9c4e13f4461bdcf5cefbe71c1c9 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 20 Aug 2023 14:05:21 +0900 Subject: [PATCH 1/3] fix(mpc): use optimization result when polish failed Signed-off-by: Takamasa Horibe --- .../src/qp_solver/qp_solver_osqp.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index dba803e8f5efc..2ddd29b2acd2c 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -72,12 +72,10 @@ bool QPSolverOSQP::solve( // polish status: successful (1), unperformed (0), (-1) unsuccessful int status_polish = std::get<2>(result); - if (status_polish == -1) { - RCLCPP_WARN(logger_, "osqp status_polish = %d (unsuccessful)", status_polish); - return false; - } - if (status_polish == 0) { - RCLCPP_WARN(logger_, "osqp status_polish = %d (unperformed)", status_polish); + if (status_polish == -1 || status_polish == 0) { + const auto s = (status_polish == 0) ? "Polish process is not performed in osqp." + : "Polish process failed in osqp."; + RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate."); return true; } return true; From 7b84fd44026944e392d8bb7a3568a77e5e73dded Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 20 Aug 2023 15:43:07 +0900 Subject: [PATCH 2/3] fix arg Signed-off-by: Takamasa Horibe --- control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 2ddd29b2acd2c..7c76d67e75aa3 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -75,7 +75,7 @@ bool QPSolverOSQP::solve( if (status_polish == -1 || status_polish == 0) { const auto s = (status_polish == 0) ? "Polish process is not performed in osqp." : "Polish process failed in osqp."; - RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate."); + RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate.", s); return true; } return true; From f1254dbc3aa30905a8c7508846edb9c34f3c63c2 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 20 Aug 2023 18:43:08 +0900 Subject: [PATCH 3/3] fix test Signed-off-by: Takamasa Horibe --- control/mpc_lateral_controller/test/test_mpc.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 6066cd3419ef3..6f8a6fb598058 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -252,9 +252,8 @@ TEST_F(MPCTest, OsqpCalculate) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - // with OSQP this function returns false despite finding correct solutions const auto odom = makeOdometry(pose_zero, default_velocity); - EXPECT_FALSE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); }