From a74f2546ad02e3b206e55c34ebd85e7dbf034812 Mon Sep 17 00:00:00 2001
From: Hiroki OTA <hiroki.ota@tier4.jp>
Date: Tue, 28 Feb 2023 13:15:29 +0900
Subject: [PATCH] fix(obstacle_avoidance_planner): add guard for too large yaw
 deviation (backport #2579) (#289)

fix(obstacle_avoidance_planner): add guard for too large yaw deviation (#2579)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../obstacle_avoidance_planner/common_structs.hpp      |  1 +
 .../obstacle_avoidance_planner/src/mpt_optimizer.cpp   | 10 ++++++++++
 2 files changed, 11 insertions(+)

diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp
index da7f284085ef6..d293ebc72e2dc 100644
--- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp
+++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp
@@ -160,6 +160,7 @@ struct DebugData
 
   std::vector<geometry_msgs::msg::Pose> mpt_ref_poses;
   std::vector<double> lateral_errors;
+  std::vector<double> yaw_errors;
 
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> eb_traj;
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> mpt_fixed_traj;
diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
index 818bca69702e2..6c8643ef3785d 100644
--- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
+++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
@@ -292,6 +292,15 @@ boost::optional<MPTOptimizer::MPTTrajs> MPTOptimizer::getModelPredictiveTrajecto
       return boost::none;
     }
   }
+  constexpr double max_yaw_deviation = 50.0 / 180 * 3.14;
+  for (const double yaw_error : debug_data_ptr->yaw_errors) {
+    if (max_yaw_deviation < std::abs(yaw_error)) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("mpt_optimizer"),
+        "return boost::none since yaw deviation is too large.");
+      return boost::none;
+    }
+  }
 
   auto full_optimized_ref_points = fixed_ref_points;
   full_optimized_ref_points.insert(
@@ -1175,6 +1184,7 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> MPTOptimizer::get
     ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw);
     debug_data_ptr->mpt_ref_poses.push_back(ref_pose);
     debug_data_ptr->lateral_errors.push_back(lat_error);
+    debug_data_ptr->yaw_errors.push_back(yaw_error);
 
     ref_point.optimized_kinematic_state << lat_error_vec.at(i), yaw_error_vec.at(i);
     if (i >= fixed_ref_points.size()) {