fix(obstacle_avoidance_planner): add guard for too large yaw deviatio…

…n (backport autowarefoundation#2579) (#289)

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

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>

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;

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()) {