complete to implementation

Signed-off-by: Yuki Takagi <[email protected]>
autowarefoundation · Nov 2, 2023 · cde9731 · cde9731
1 parent 9d938e4
commit cde9731
Show file tree

Hide file tree

Showing 3 changed files with 179 additions and 207 deletions.
diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -220,6 +220,7 @@ boost::optional<size_t> searchZeroVelocityIndex(
 
   constexpr double epsilon = 1e-3;
   for (size_t i = src_idx; i < dst_idx; ++i) {
+    std::cerr << std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) << std::endl;
     if (std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) < epsilon) {
       return i;
     }

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -1106,7 +1106,7 @@ std::optional<double> ObstacleCruisePlannerNode::calcTrajDistToSecurePolysDist(
         }
         last_dist = sec_i_dist;
       }
-      return 0.0;
+      return last_dist - dist_to_be_secured;
     }
   }
   return std::nullopt;