fix(obstacle_avoidance_planner): fix bug of lerp when points' size = 1 (

tier4#664) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
boyali · Oct 3, 2022 · cd61bf1 · cd61bf1
1 parent 0dde480
commit cd61bf1
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
@@ -51,6 +51,10 @@ template <typename T>
 double lerpTwistX(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() == 1) {
+    return points.at(0).longitudinal_velocity_mps;
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =
@@ -70,6 +74,10 @@ template <typename T>
 double lerpPoseZ(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() == 1) {
+    return points.at(0).pose.position.z;
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =