Temporarily hard code use all predicted path

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
maxime-clem · Jan 23, 2023 · 8f92e45 · 8f92e45
1 parent fa8905d
commit 8f92e45
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 14 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
@@ -435,20 +435,20 @@ bool isLaneChangePathSafe(
   const double check_end_time = lane_change_prepare_duration + lane_changing_safety_check_duration;
   const double min_lc_speed{lane_change_parameters.minimum_lane_change_velocity};
 
-  const auto get_pose = std::invoke([&]() {
-    Pose p;
-    double dist{0.0};
-    for (size_t i = 1; i < path.points.size(); ++i) {
-      dist += motion_utils::calcSignedArcLength(path.points, i - 1, i);
-      if (dist >= common_parameters.backward_path_length) {
-        return path.points.at(i).point.pose;
-      }
-    }
-    return path.points.front().point.pose;
-  });
+  // const auto get_pose = std::invoke([&]() {
+  //   Pose p;
+  //   double dist{0.0};
+  //   for (size_t i = 1; i < path.points.size(); ++i) {
+  //     dist += motion_utils::calcSignedArcLength(path.points, i - 1, i);
+  //     if (dist >= common_parameters.backward_path_length) {
+  //       return path.points.at(i).point.pose;
+  //     }
+  //   }
+  //   return path.points.front().point.pose;
+  // });
 
   const auto vehicle_predicted_path = util::convertToPredictedPath(
-    path, current_twist, get_pose, static_cast<double>(current_seg_idx), check_end_time,
+    path, current_twist, current_pose, static_cast<double>(current_seg_idx), check_end_time,
     time_resolution, acceleration, min_lc_speed);
   const auto prepare_phase_ignore_target_speed_thresh =
     lane_change_parameters.prepare_phase_ignore_target_speed_thresh;

diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
@@ -2343,10 +2343,10 @@ bool getObjectExpectedPoseAndConvertToPolygon(
 }
 
 std::vector<PredictedPath> getPredictedPathFromObj(
-  const PredictedObject & obj, const bool & is_use_all_predicted_path)
+  const PredictedObject & obj, [[maybe_unused]] const bool & is_use_all_predicted_path)
 {
   std::vector<PredictedPath> predicted_path_vec;
-  if (is_use_all_predicted_path) {
+  if (true) {
     std::copy_if(
       obj.kinematics.predicted_paths.cbegin(), obj.kinematics.predicted_paths.cend(),
       std::back_inserter(predicted_path_vec),