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

autowarefoundation#2579) Signed-off-by: Takayuki Murooka <[email protected]> Signed-off-by: Takayuki Murooka <[email protected]>
sujithvemi · Dec 26, 2022 · c98bdbc · c98bdbc
1 parent 773ef53
commit c98bdbc
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 0 deletions.
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
@@ -153,6 +153,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
@@ -314,6 +314,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.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(
@@ -1197,6 +1206,7 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> MPTOptimizer::get
     ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw);
     debug_data.mpt_ref_poses.push_back(ref_pose);
     debug_data.lateral_errors.push_back(lat_error);
+    debug_data.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()) {