Add diff th param

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -33,6 +33,7 @@
       # trajectory generation near terminal using frenet planner
       frenet:
         enable: true
+        th_yaw_diff: 10.0  # [deg]
 
       # safety check
       safety_check:

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp

@@ -116,6 +116,7 @@ struct SafetyParameters
 struct FrenetPlannerParameters
 {
   bool enable{true};
+  double th_yaw_diff_deg{10.0};
 };
 
 struct TrajectoryParameters

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp

@@ -95,8 +95,8 @@ LaneChangePath get_candidate_path(
  *                                       if the path fails any constraints.
  */
 LaneChangePath construct_candidate_path(
-  const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment,
-  const PathWithLaneId & target_lane_reference_path,
+  const LaneChangeInfo & lane_change_info, const CommonDataPtr & common_data_ptr,
+  const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path,
   const std::vector<std::vector<int64_t>> & sorted_lane_ids);
 
 /// @brief generate lane change candidate paths using the Frenet planner

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -212,8 +212,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
     *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio"));
 
   // trajectory generation near terminal using frenet planner
-  p.frenet.enable =
-    getOrDeclareParameter<bool>(*node, parameter("frenet.enable"));
+  p.frenet.enable = getOrDeclareParameter<bool>(*node, parameter("frenet.enable"));
 
   // lane change cancel
   p.cancel.enable_on_prepare_phase =

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -41,7 +41,6 @@
 
 #include <boost/geometry/algorithms/buffer.hpp>
 
-#include <fmt/format.h>
 #include <lanelet2_core/geometry/Point.h>
 #include <lanelet2_core/geometry/Polygon.h>
 #include <lanelet2_core/primitives/LineString.h>

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

@@ -28,6 +28,8 @@
 #include <range/v3/numeric/accumulate.hpp>
 #include <range/v3/view.hpp>
 
+#include <fmt/format.h>
+
 #include <algorithm>
 #include <limits>
 #include <vector>
@@ -302,12 +304,12 @@ LaneChangePath get_candidate_path(
   }
 
   return utils::lane_change::construct_candidate_path(
-    lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids);
+    lane_change_info, common_data_ptr, prep_segment, target_lane_reference_path, sorted_lane_ids);
 }
 
 LaneChangePath construct_candidate_path(
-  const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment,
-  const PathWithLaneId & target_lane_reference_path,
+  const LaneChangeInfo & lane_change_info, const CommonDataPtr & common_data_ptr,
+  const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path,
   const std::vector<std::vector<int64_t>> & sorted_lane_ids)
 {
   const auto & shift_line = lane_change_info.shift_line;
@@ -343,14 +345,15 @@ LaneChangePath construct_candidate_path(
     point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids;
   }
 
-  constexpr auto yaw_diff_th = autoware::universe_utils::deg2rad(5.0);
+  const auto th_yaw_diff_deg = common_data_ptr->lc_param_ptr->frenet.th_yaw_diff_deg;
   if (
-    const auto yaw_diff_opt =
-      exceed_yaw_threshold(prepare_segment, shifted_path.path, yaw_diff_th)) {
-    std::stringstream err_msg;
-    err_msg << "Excessive yaw difference " << yaw_diff_opt.value() << " which exceeds the "
-            << yaw_diff_th << " radian threshold.";
-    throw std::logic_error(err_msg.str());
+    const auto yaw_diff_opt = exceed_yaw_threshold(
+      prepare_segment, shifted_path.path, autoware::universe_utils::deg2rad(th_yaw_diff_deg))) {
+    const auto yaw_diff_deg = autoware::universe_utils::rad2deg(yaw_diff_opt.value());
+    const auto err_msg = fmt::format(
+      "Excessive yaw difference {yaw_diff:2.2f}[deg]. The threshold is {th_yaw_diff:2.2f}[deg].",
+      fmt::arg("yaw_diff", yaw_diff_deg), fmt::arg("th_yaw_diff", th_yaw_diff_deg));
+    throw std::logic_error(err_msg);
   }
 
   LaneChangePath candidate_path;