Planning: reeds_shepp_path GenerateLocalConfigurations fix

修复当segs_lengths元素值为0时生成结果错误的问题 Fixes #15135
ApolloAuto · Sep 14, 2023 · 5059649 · 5059649
1 parent a3c851f
commit 5059649
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/modules/planning/open_space/coarse_trajectory_generator/reeds_shepp_path.cc b/modules/planning/open_space/coarse_trajectory_generator/reeds_shepp_path.cc
@@ -923,6 +923,11 @@ bool ReedShepp::SetRSP(const int size, const double* lengths, const char* types,
 bool ReedShepp::GenerateLocalConfigurations(
     const std::shared_ptr<Node3d> start_node,
     const std::shared_ptr<Node3d> end_node, ReedSheppPath* shortest_path) {
+  for (size_t i = 0; i < shortest_path->segs_lengths.size(); ++i) {
+    if (shortest_path->segs_lengths.at(i) == 0) {
+      shortest_path->segs_lengths.at(i) = 0.0001;
+    }
+  }
   double step_scaled =
       planner_open_space_config_.warm_start_config().step_size() * max_kappa_;