From 5059649a2a725ecb429af37f83e58029e611140b Mon Sep 17 00:00:00 2001
From: Aglargil <34728006+Aglargil@users.noreply.github.com>
Date: Thu, 14 Sep 2023 22:22:49 +0800
Subject: [PATCH] Planning: reeds_shepp_path GenerateLocalConfigurations fix
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

修复当segs_lengths元素值为0时生成结果错误的问题

Fixes #15135
---
 .../coarse_trajectory_generator/reeds_shepp_path.cc          | 5 +++++
 1 file changed, 5 insertions(+)

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
index 62322854868..1167e4b39f6 100644
--- 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_;