From 8be4e356de9febd6c7c9140f271eae4156bd56e8 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Tue, 9 Aug 2022 17:06:59 +0900
Subject: [PATCH] feat(motion_utils): add new nearest index function (#1396)

* feat(motion_utils): add new nearest index function

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add tests

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix rebase

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add README

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* minor update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix ci error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix for traffic nearest index

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix for markdownlint error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 common/motion_utils/README.md                 | 100 ++++
 .../trajectory/path_with_lane_id.hpp          |  99 ++++
 .../motion_utils/trajectory/trajectory.hpp    | 121 ++++
 .../motion_utils/media/ego_nearest_search.svg |   3 +
 .../src/trajectory/test_path_with_lane_id.cpp | 163 ++++++
 .../test/src/trajectory/test_trajectory.cpp   | 526 ++++++++++++++++++
 6 files changed, 1012 insertions(+)
 create mode 100644 common/motion_utils/README.md
 create mode 100644 common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp
 create mode 100644 common/motion_utils/media/ego_nearest_search.svg
 create mode 100644 common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp

diff --git a/common/motion_utils/README.md b/common/motion_utils/README.md
new file mode 100644
index 0000000000000..154da4bdf4387
--- /dev/null
+++ b/common/motion_utils/README.md
@@ -0,0 +1,100 @@
+# Motion Utils package
+
+## Nearest index search
+
+In this section, the nearest index and nearest segment index search is explained.
+
+We have the same functions for the nearest index search and nearest segment index search.
+Taking for the example the nearest index search, we have two types of functions.
+
+The first function finds the nearest index with distance and yaw thresholds.
+
+```cpp
+template <class T>
+size_t findFirstNearestIndexWithSoftConstraints(
+  const T & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
+```
+
+This function finds the first local solution within thresholds.
+The reason to find the first local one is to deal with some edge cases explained in the next subsection.
+
+There are default parameters for thresholds arguments so that you can decide which thresholds to pass to the function.
+
+1. When both the distance and yaw thresholds are given.
+   - First, try to find the nearest index with both the distance and yaw thresholds.
+   - If not found, try to find again with only the distance threshold.
+   - If not found, find without any thresholds.
+2. When only distance are given.
+   - First, try to find the nearest index the distance threshold.
+   - If not found, find without any thresholds.
+3. When no thresholds are given.
+   - Find the nearest index.
+
+The second function finds the nearest index in the lane whose id is `lane_id`.
+
+```cpp
+size_t findNearestIndexFromLaneId(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const geometry_msgs::msg::Point & pos, const int64_t lane_id);
+```
+
+### Application to various object
+
+Many node packages often calculate the nearest index of objects.
+We will explain the recommended method to calculate it.
+
+#### Nearest index for the ego
+
+Assuming that the path length before the ego is short enough, we expect to find the correct nearest index in the following edge cases by `findFirstNearestIndexWithSoftConstraints` with both distance and yaw thresholds.
+Blue circles describes the distance threshold from the base link position and two blue lines describe the yaw threshold against the base link orientation.
+Among points in these cases, the correct nearest point which is red can be found.
+
+![ego_nearest_search](./media/ego_nearest_search.svg)
+
+Therefore, the implementation is as follows.
+
+```cpp
+const size_t ego_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+const size_t ego_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+```
+
+#### Nearest index for dynamic objects
+
+For the ego nearest index, the orientation is considered in addition to the position since the ego is supposed to follow the points.
+However, for the dynamic objects (e.g., predicted object), sometimes its orientation may be different from the points order, e.g. the dynamic object driving backward although the ego is driving forward.
+
+Therefore, the yaw threshold should not be considered for the dynamic object.
+The implementation is as follows.
+
+```cpp
+const size_t dynamic_obj_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold);
+const size_t dynamic_obj_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold);
+```
+
+#### Nearest index for traffic objects
+
+In lanelet maps, traffic objects belong to the specific lane.
+With this specific lane's id, the correct nearest index can be found.
+
+The implementation is as follows.
+
+```cpp
+// first extract `lane_id` which the traffic object belong to.
+const size_t traffic_obj_nearest_idx = findNearestIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id);
+const size_t traffic_obj_nearest_seg_idx = findNearestSegmentIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id);
+```
+
+## Path/Trajectory length calculation between designated points
+
+Based on the discussion so far, the nearest index search algorithm is different depending on the object type.
+Therefore, we recommended using the wrapper utility functions which require the nearest index search (e.g., calculating the path length) with each nearest index search.
+
+For example, when we want to calculate the path length between the ego and the dynamic object, the implementation is as follows.
+
+```cpp
+const size_t ego_nearest_idx = findFirstNearestSegmentIndex(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+const size_t dyn_obj_nearest_seg_idx = findFirstNearestSegmentIndex(points, dyn_obj_pose, dyn_obj_nearest_dist_threshold);
+const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_nearest_seg_idx, dyn_obj_pose, dyn_obj_nearest_seg_idx);
+```
diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp
new file mode 100644
index 0000000000000..a10db47a377a2
--- /dev/null
+++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp
@@ -0,0 +1,99 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
+#define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
+
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp"
+
+#include <boost/optional.hpp>
+
+#include <utility>
+#include <vector>
+
+namespace motion_utils
+{
+inline boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id)
+{
+  size_t start_idx = 0;  // NOTE: to prevent from maybe-uninitialized error
+  size_t end_idx = 0;    // NOTE: to prevent from maybe-uninitialized error
+
+  bool found_first_idx = false;
+  for (size_t i = 0; i < path.points.size(); ++i) {
+    const auto & p = path.points.at(i);
+    for (const auto & id : p.lane_ids) {
+      if (id == lane_id) {
+        if (!found_first_idx) {
+          start_idx = i;
+          found_first_idx = true;
+        }
+        end_idx = i;
+      }
+    }
+  }
+
+  if (found_first_idx) {
+    return std::make_pair(start_idx, end_idx);
+  }
+
+  return {};
+}
+
+inline size_t findNearestIndexFromLaneId(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const geometry_msgs::msg::Point & pos, const int64_t lane_id)
+{
+  const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id);
+  if (opt_range) {
+    const size_t start_idx = opt_range->first;
+    const size_t end_idx = opt_range->second;
+
+    validateNonEmpty(path.points);
+
+    const auto sub_points = std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>{
+      path.points.begin() + start_idx, path.points.begin() + end_idx + 1};
+    validateNonEmpty(sub_points);
+
+    return start_idx + findNearestIndex(sub_points, pos);
+  }
+
+  return findNearestIndex(path.points, pos);
+}
+
+inline size_t findNearestSegmentIndexFromLaneId(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const geometry_msgs::msg::Point & pos, const int64_t lane_id)
+{
+  const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id);
+
+  if (nearest_idx == 0) {
+    return 0;
+  }
+  if (nearest_idx == path.points.size() - 1) {
+    return path.points.size() - 2;
+  }
+
+  const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos);
+
+  if (signed_length <= 0) {
+    return nearest_idx - 1;
+  }
+
+  return nearest_idx;
+}
+}  // namespace motion_utils
+
+#endif  // MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
index 912502ed9e61e..ae12808b29465 100644
--- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
+++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -1059,6 +1059,127 @@ inline boost::optional<size_t> insertStopPoint(
 
   return stop_idx;
 }
+
+template <class T>
+double calcSignedArcLength(
+  const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx,
+  const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx)
+{
+  validateNonEmpty(points);
+
+  const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx);
+  const double signed_length_src_offset =
+    calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point);
+  const double signed_length_dst_offset =
+    calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point);
+
+  return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
+}
+
+template <class T>
+size_t findFirstNearestIndexWithSoftConstraints(
+  const T & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max())
+{
+  validateNonEmpty(points);
+
+  {  // with dist and yaw thresholds
+    const double squared_dist_threshold = dist_threshold * dist_threshold;
+    double min_squared_dist = std::numeric_limits<double>::max();
+    size_t min_idx = 0;
+    bool is_within_constraints = false;
+    for (size_t i = 0; i < points.size(); ++i) {
+      const auto squared_dist =
+        tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position);
+      const auto yaw =
+        tier4_autoware_utils::calcYawDeviation(tier4_autoware_utils::getPose(points.at(i)), pose);
+
+      if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) {
+        if (is_within_constraints) {
+          break;
+        } else {
+          continue;
+        }
+      }
+
+      if (min_squared_dist <= squared_dist) {
+        continue;
+      }
+
+      min_squared_dist = squared_dist;
+      min_idx = i;
+      is_within_constraints = true;
+    }
+
+    // nearest index is found
+    if (is_within_constraints) {
+      return min_idx;
+    }
+  }
+
+  {  // with dist threshold
+    const double squared_dist_threshold = dist_threshold * dist_threshold;
+    double min_squared_dist = std::numeric_limits<double>::max();
+    size_t min_idx = 0;
+    bool is_within_constraints = false;
+    for (size_t i = 0; i < points.size(); ++i) {
+      const auto squared_dist =
+        tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position);
+
+      if (squared_dist_threshold < squared_dist) {
+        if (is_within_constraints) {
+          break;
+        } else {
+          continue;
+        }
+      }
+
+      if (min_squared_dist <= squared_dist) {
+        continue;
+      }
+
+      min_squared_dist = squared_dist;
+      min_idx = i;
+      is_within_constraints = true;
+    }
+
+    // nearest index is found
+    if (is_within_constraints) {
+      return min_idx;
+    }
+  }
+
+  // without any threshold
+  return findNearestIndex(points, pose.position);
+}
+
+template <class T>
+size_t findFirstNearestSegmentIndexWithSoftConstraints(
+  const T & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max())
+{
+  // find first nearest index with soft constraints (not segment index)
+  const size_t nearest_idx =
+    findFirstNearestIndexWithSoftConstraints(points, pose, dist_threshold, yaw_threshold);
+
+  // calculate segment index
+  if (nearest_idx == 0) {
+    return 0;
+  }
+  if (nearest_idx == points.size() - 1) {
+    return points.size() - 2;
+  }
+
+  const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, pose.position);
+
+  if (signed_length <= 0) {
+    return nearest_idx - 1;
+  }
+
+  return nearest_idx;
+}
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_
diff --git a/common/motion_utils/media/ego_nearest_search.svg b/common/motion_utils/media/ego_nearest_search.svg
new file mode 100644
index 0000000000000..164f420bf4853
--- /dev/null
+++ b/common/motion_utils/media/ego_nearest_search.svg
@@ -0,0 +1,3 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="960px" height="516px" viewBox="-0.5 -0.5 960 516" content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2022-08-08T08:09:40.739Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/13.0.1 Chrome/80.0.3987.163 Electron/8.2.1 Safari/537.36&quot; etag=&quot;PVmJU_0nYjGUdLlH0r9r&quot; version=&quot;13.0.1&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FmUAXtwAyzemAjJw8FDx&quot; name=&quot;Page-1&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;" style="background-color: rgb(255, 255, 255);"><defs/><g><rect x="795" y="394" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" transform="rotate(-186,820,409)" pointer-events="all"/><path d="M 238 268.35 L 238 456 Q 238 466 248 466 L 448 466 Q 458 466 458 456 L 458 416 Q 458 406 458 396 L 458 356 Q 458 346 448 346 L 18 346" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 238 259.35 L 242.5 268.35 L 233.5 268.35 Z" fill="#97d077" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><ellipse cx="238" cy="362.33" rx="5" ry="5" fill="#ff3333" stroke="#b85450" pointer-events="all"/><rect x="852" y="399" width="40" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 409px; margin-left: 853px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">nearest</div></div></div></foreignObject><text x="872" y="413" fill="#FF3333" font-family="Helvetica" font-size="12px" text-anchor="middle">nearest</text></switch></g><ellipse cx="211" cy="127" rx="95" ry="95" fill="none" stroke="#3333ff" stroke-dasharray="3 3" pointer-events="all"/><path d="M 254.38 73.51 Q 276 86 284 109 Q 292 132 278.04 156.42" fill="none" stroke="#3333ff" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 250.92 71.51 L 255.38 71.78 L 253.38 75.24 Z" fill="#3333ff" stroke="#3333ff" stroke-miterlimit="10" pointer-events="all"/><path d="M 276.55 159.03 L 276.8 154.56 L 278.04 156.42 L 280.28 156.55 Z" fill="#3333ff" stroke="#3333ff" stroke-miterlimit="10" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 104px; margin-left: 277px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #3333FF; line-height: 1.2; pointer-events: all; background-color: #ffffff; white-space: nowrap; ">2 * yaw<br style="font-size: 9px" />threshold</div></div></div></foreignObject><text x="277" y="107" fill="#3333FF" font-family="Helvetica" font-size="9px" text-anchor="middle">2 * yaw...</text></switch></g><path d="M 188 136 L 518 136 Q 528 136 528 146 L 528 216 Q 528 226 518 226 L 458 226 Q 448 226 440.19 219.75 L 355.81 152.25 Q 348 146 338 146 L 20.35 146" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 11.35 146 L 20.35 141.5 L 20.35 150.5 Z" fill="#97d077" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><ellipse cx="33" cy="136" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><path d="M 8 136 L 28 136" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><ellipse cx="183" cy="136" rx="5" ry="5" fill="#ff3333" stroke="#b85450" pointer-events="all"/><path d="M 38 136 L 178 136" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><ellipse cx="263" cy="136" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="343" cy="136" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="423" cy="136" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="493" cy="136" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="528" cy="168" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="408" cy="194" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="523" cy="225" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="465" cy="226" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="193" cy="146" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="111" cy="146" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="272" cy="146" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="356" cy="153" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="45" cy="147" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><rect x="148" y="113.67" width="40" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 124px; margin-left: 149px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">nearest</div></div></div></foreignObject><text x="168" y="127" fill="#FF3333" font-family="Helvetica" font-size="12px" text-anchor="middle">nearest</text></switch></g><ellipse cx="103" cy="136" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><path d="M 570.35 402 L 818 402 Q 828 402 838 402 L 849 402 Q 859 402 858.5 392.01 L 858.25 387.01 Q 858 382 848 382 L 843 382 Q 838 382 828 382 L 808 382" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 561.35 402 L 570.35 397.5 L 570.35 406.5 Z" fill="#97d077" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><ellipse cx="59" cy="346" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="149" cy="347" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="251" cy="346" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="238" cy="279" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="359" cy="346" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="458" cy="367" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="459" cy="449" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="245" cy="466" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="361" cy="466" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><rect x="206" y="342" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" transform="rotate(-96,231,357)" pointer-events="all"/><path d="M 937.65 121 L 694 121 Q 684 121 684 131 L 684 256 Q 684 266 694 266 L 838 266 Q 848 266 848 256 L 848 122 Q 848 112 838 112 L 581 112" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 946.65 121 L 937.65 125.5 L 937.65 116.5 Z" fill="#97d077" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/><rect x="248" y="352.33" width="40" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 362px; margin-left: 249px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">nearest</div></div></div></foreignObject><text x="268" y="366" fill="#FF3333" font-family="Helvetica" font-size="12px" text-anchor="middle">nearest</text></switch></g><ellipse cx="843" cy="402" rx="5" ry="5" fill="#ff3333" stroke="#b85450" pointer-events="all"/><ellipse cx="827" cy="382" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="778" cy="402" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="718" cy="402" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="655" cy="402" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="600" cy="112" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="599" cy="401" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="660" cy="112" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="793" cy="112" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="848" cy="135" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="848" cy="186" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="732" cy="266" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="805" cy="266" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="848" cy="236" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="684" cy="233" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="684" cy="172" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="709" cy="121" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="779" cy="121" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="839" cy="121" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><ellipse cx="900" cy="121" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/><rect x="680" y="87" width="40" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 97px; margin-left: 681px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">nearest</div></div></div></foreignObject><text x="700" y="101" fill="#FF3333" font-family="Helvetica" font-size="12px" text-anchor="middle">nearest</text></switch></g><ellipse cx="722" cy="112" rx="5" ry="5" fill="#ff3333" stroke="#b85450" pointer-events="all"/><path d="M 221 331.53 L 237 342.53 L 221 353.53 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" transform="rotate(-96.3,229,342.53)" pointer-events="all"/><rect x="714" y="103.67" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" pointer-events="all"/><path d="M 745 107.67 L 761 118.67 L 745 129.67 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" pointer-events="all"/><rect x="196" y="111.67" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" transform="rotate(-15,221,126.67)" pointer-events="all"/><path d="M 227 111.67 L 243 122.67 L 227 133.67 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" transform="rotate(-15,235,122.67)" pointer-events="all"/><path d="M 798 400 L 814 411 L 798 422 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" transform="rotate(-185.7,806,411)" pointer-events="all"/><path d="M 234 373 L 168.54 318.89" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 234 373 L 273.82 300.02" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><ellipse cx="229" cy="369" rx="95" ry="95" fill="none" stroke="#3333ff" stroke-dasharray="3 3" transform="rotate(-84.5,229,369)" pointer-events="all"/><path d="M 833 407 L 778.42 471.89" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 833 407 L 758.63 366.79" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><ellipse cx="828" cy="411" rx="95" ry="95" fill="none" stroke="#3333ff" stroke-dasharray="3 3" transform="rotate(-175,828,411)" pointer-events="all"/><path d="M 723 120 L 787.4 64.64" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 723 120 L 788.64 171.59" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><ellipse cx="728" cy="116" rx="95" ry="95" fill="none" stroke="#3333ff" stroke-dasharray="3 3" transform="rotate(15,728,116)" pointer-events="all"/><path d="M 206 131 L 255.08 62.02" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 204.38 127.21 L 169.87 46.43" fill="none" stroke="#3333ff" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 205.56 129.97 L 202.15 127.08 L 204.38 127.21 L 205.83 125.51 Z" fill="#3333ff" stroke="#3333ff" stroke-miterlimit="10" pointer-events="all"/><path d="M 168.69 43.67 L 172.1 46.56 L 169.87 46.43 L 168.42 48.13 Z" fill="#3333ff" stroke="#3333ff" stroke-miterlimit="10" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 77px; margin-left: 184px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #3333FF; line-height: 1.2; pointer-events: all; background-color: #ffffff; white-space: nowrap; ">distance<br style="font-size: 9px" />threshold</div></div></div></foreignObject><text x="184" y="79" fill="#3333FF" font-family="Helvetica" font-size="9px" text-anchor="middle">distance...</text></switch></g><path d="M 206 131 L 283.96 165" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/></g><switch><g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/><a transform="translate(0,-5)" xlink:href="https://desk.draw.io/support/solutions/articles/16000042487" target="_blank"><text text-anchor="middle" font-size="10px" x="50%" y="100%">Viewer does not support full SVG 1.1</text></a></switch></svg>
\ No newline at end of file
diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp
new file mode 100644
index 0000000000000..1eadf7105d087
--- /dev/null
+++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp
@@ -0,0 +1,163 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_utils/trajectory/path_with_lane_id.hpp"
+// #include "tier4_autoware_utils/geometry/geometry.hpp"
+// #include "tier4_autoware_utils/math/unit_conversion.hpp"
+
+#include <gtest/gtest.h>
+
+#include <limits>
+#include <vector>
+
+namespace
+{
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using tier4_autoware_utils::createPoint;
+
+geometry_msgs::msg::Pose createPose(
+  double x, double y, double z, double roll, double pitch, double yaw)
+{
+  geometry_msgs::msg::Pose p;
+  p.position = createPoint(x, y, z);
+  p.orientation = tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw);
+  return p;
+}
+
+PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double point_interval)
+{
+  PathWithLaneId path;
+  for (size_t i = 0; i < num_points; ++i) {
+    const double x = i * point_interval;
+    const double y = 0.0;
+
+    PathPointWithLaneId p;
+    p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, 0.0);
+    p.lane_ids.push_back(i);
+    path.points.push_back(p);
+  }
+
+  return path;
+}
+}  // namespace
+
+TEST(path_with_lane_id, getPathIndexRangeWithLaneId)
+{
+  using autoware_auto_planning_msgs::msg::PathWithLaneId;
+  using motion_utils::getPathIndexRangeWithLaneId;
+
+  // Usual cases
+  {
+    PathWithLaneId points;
+    points.points.resize(6);
+    points.points.at(0).lane_ids.push_back(3);
+    points.points.at(1).lane_ids.push_back(3);
+    points.points.at(2).lane_ids.push_back(1);
+    points.points.at(3).lane_ids.push_back(2);
+    points.points.at(4).lane_ids.push_back(2);
+    points.points.at(5).lane_ids.push_back(2);
+
+    {
+      const auto res = getPathIndexRangeWithLaneId(points, 3);
+      EXPECT_EQ(res->first, 0U);
+      EXPECT_EQ(res->second, 1U);
+    }
+    {
+      const auto res = getPathIndexRangeWithLaneId(points, 1);
+      EXPECT_EQ(res->first, 2U);
+      EXPECT_EQ(res->second, 2U);
+    }
+    {
+      const auto res = getPathIndexRangeWithLaneId(points, 2);
+      EXPECT_EQ(res->first, 3U);
+      EXPECT_EQ(res->second, 5U);
+    }
+    {
+      const auto res = getPathIndexRangeWithLaneId(points, 4);
+      EXPECT_EQ(res, boost::none);
+    }
+  }
+
+  // Empty points
+  {
+    PathWithLaneId points;
+    const auto res = getPathIndexRangeWithLaneId(points, 4);
+    EXPECT_EQ(res, boost::none);
+  }
+}
+
+TEST(path_with_lane_id, findNearestIndexFromLaneId)
+{
+  using motion_utils::findNearestIndexFromLaneId;
+  using motion_utils::findNearestSegmentIndexFromLaneId;
+
+  const auto path = generateTestPathWithLaneId(10, 1.0);
+
+  // Normal cases
+  {
+    auto modified_path = path;
+    for (size_t i = 0; i < 10; ++i) {
+      modified_path.points.at(i).lane_ids = {100};
+    }
+    EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U);
+    EXPECT_EQ(
+      findNearestSegmentIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U);
+  }
+
+  {
+    auto modified_path = path;
+    for (size_t i = 3; i < 6; ++i) {
+      modified_path.points.at(i).lane_ids = {100};
+    }
+    EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(4.1, 0.3, 0.0), 100), 4U);
+    EXPECT_EQ(
+      findNearestSegmentIndexFromLaneId(modified_path, createPoint(4.1, 0.3, 0.0), 100), 4U);
+  }
+
+  {
+    auto modified_path = path;
+    for (size_t i = 8; i < 9; ++i) {
+      modified_path.points.at(i).lane_ids = {100};
+    }
+    EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(8.5, -0.5, 0.0), 100), 8U);
+    EXPECT_EQ(
+      findNearestSegmentIndexFromLaneId(modified_path, createPoint(8.5, -0.5, 0.0), 100), 8U);
+  }
+
+  // Nearest is not within range
+  {
+    auto modified_path = path;
+    for (size_t i = 3; i < 9; ++i) {
+      modified_path.points.at(i).lane_ids = {100};
+    }
+    EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 3U);
+    EXPECT_EQ(
+      findNearestSegmentIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U);
+  }
+
+  // Path does not contain lane_id.
+  {
+    EXPECT_EQ(findNearestIndexFromLaneId(path, createPoint(2.4, 1.3, 0.0), 100), 2U);
+    EXPECT_EQ(findNearestSegmentIndexFromLaneId(path, createPoint(2.4, 1.3, 0.0), 100), 2U);
+  }
+
+  // Empty points
+  EXPECT_THROW(
+    findNearestIndexFromLaneId(PathWithLaneId{}, createPoint(2.4, 1.3, 0.0), 100),
+    std::invalid_argument);
+  EXPECT_THROW(
+    findNearestSegmentIndexFromLaneId(PathWithLaneId{}, createPoint(2.4, 1.3, 0.0), 100),
+    std::invalid_argument);
+}
diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp
index 8e3809f739936..068fbe3e42de1 100644
--- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp
+++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp
@@ -3489,3 +3489,529 @@ TEST(trajectory, insertStopPoint_from_a_pose)
     EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), boost::none);
   }
 }
+
+TEST(trajectory, findFirstNearestIndexWithSoftConstraints)
+{
+  using motion_utils::findFirstNearestIndexWithSoftConstraints;
+  using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints;
+  using tier4_autoware_utils::pi;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+
+  // Non overlapped points
+  {
+    // 1. Dist and yaw thresholds are given
+    // Normal cases
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4),
+      2U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4),
+      2U);
+
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0),
+      4U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0),
+      4U);
+
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1),
+      8U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1),
+      8U);
+
+    // Dist is out of range
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.4),
+      2U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.4),
+      2U);
+
+    // Yaw is out of range
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.2),
+      2U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.2),
+      2U);
+
+    // Dist and yaw is out of range
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2),
+      2U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2),
+      2U);
+
+    // Empty points
+    EXPECT_THROW(
+      findFirstNearestIndexWithSoftConstraints(
+        Trajectory{}.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2),
+      std::invalid_argument);
+    EXPECT_THROW(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        Trajectory{}.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2),
+      std::invalid_argument);
+
+    // 2. Dist threshold is given
+    // Normal cases
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0),
+      2U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0),
+      2U);
+
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5),
+      4U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5),
+      4U);
+
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0),
+      8U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0),
+      8U);
+
+    // Dist is out of range
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0),
+      2U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0),
+      2U);
+
+    // 3. No threshold is given
+    // Normal cases
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3)),
+      2U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3)),
+      2U);
+
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8)),
+      4U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8)),
+      4U);
+
+    EXPECT_EQ(
+      findFirstNearestIndexWithSoftConstraints(
+        traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0)),
+      8U);
+    EXPECT_EQ(
+      findFirstNearestSegmentIndexWithSoftConstraints(
+        traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0)),
+      8U);
+  }
+
+  // Vertically crossing points
+  {
+    //       ___
+    //      |  |
+    //   S__|__|
+    //      |
+    //      |
+    //      G
+    std::vector<geometry_msgs::msg::Pose> poses;
+    poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0));
+    poses.push_back(createPose(2.0, 1.0, 0.0, 0.0, 0.0, pi / 2.0));
+    poses.push_back(createPose(2.0, 2.0, 0.0, 0.0, 0.0, pi));
+    poses.push_back(createPose(1.0, 2.0, 0.0, 0.0, 0.0, pi));
+    poses.push_back(createPose(0.0, 2.0, 0.0, 0.0, 0.0, -pi / 2.0));
+    poses.push_back(createPose(0.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0));
+    poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, -pi / 2.0));
+    poses.push_back(createPose(0.0, -1.0, 0.0, 0.0, 0.0, -pi / 2.0));
+    poses.push_back(createPose(0.0, -2.0, 0.0, 0.0, 0.0, -pi / 2.0));
+
+    // 1. Dist and yaw thresholds are given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4),
+        2U);
+
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, -pi / 2.0), 1.0, 0.4),
+        10U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, -pi / 2.0), 1.0, 0.4),
+        9U);
+
+      // Several nearest index within threshold
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0),
+        2U);
+
+      // Dist is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4),
+        2U);
+
+      // Yaw is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.0),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.0),
+        2U);
+
+      // Dist and yaw is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 0.0, 0.0),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 0.0, 0.0),
+        2U);
+    }
+
+    // 2. Dist threshold is given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0),
+        2U);
+
+      // Several nearest index within threshold
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0),
+        2U);
+
+      // Dist is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0),
+        2U);
+    }
+
+    // 3. No threshold is given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0)),
+        2U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0)),
+        2U);
+    }
+  }
+
+  {
+    // Points has a loop structure with the opposite direction (= u-turn)
+    //         __
+    // S/G ___|_|
+
+    std::vector<geometry_msgs::msg::Pose> poses;
+    poses.push_back(createPose(-3.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0));
+    poses.push_back(createPose(1.0, 1.0, 0.0, 0.0, 0.0, pi));
+    poses.push_back(createPose(0.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0));
+    poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, pi));
+    poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, pi));
+    poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, pi));
+    poses.push_back(createPose(-3.0, 0.0, 0.0, 0.0, 0.0, pi));
+
+    // 1. Dist and yaw thresholds are given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4),
+        0U);
+
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 1.0, 0.4),
+        9U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 1.0, 0.4),
+        9U);
+
+      // Several nearest index within threshold
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 10.0, pi * 2.0),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 10.0, pi * 2.0),
+        0U);
+
+      // Dist is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4),
+        0U);
+
+      // Yaw is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 1.0, 0.0),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 1.0, 0.0),
+        0U);
+
+      // Dist and yaw is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 0.0, 0.0),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 0.0, 0.0),
+        0U);
+    }
+
+    // 2. Dist threshold is given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0),
+        0U);
+
+      // Several nearest index within threshold
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0),
+        0U);
+
+      // Dist is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0),
+        0U);
+    }
+
+    // 3. No threshold is given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0)),
+        1U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0)),
+        0U);
+    }
+  }
+
+  {  // Points has a loop structure with the same direction
+     //      ___
+     //     |  |
+     //  S__|__|__G
+    std::vector<geometry_msgs::msg::Pose> poses;
+    poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(4.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0));
+    poses.push_back(createPose(4.0, 1.0, 0.0, 0.0, 0.0, pi / 2.0));
+    poses.push_back(createPose(4.0, 2.0, 0.0, 0.0, 0.0, pi));
+    poses.push_back(createPose(3.0, 2.0, 0.0, 0.0, 0.0, pi));
+    poses.push_back(createPose(2.0, 2.0, 0.0, 0.0, 0.0, -pi / 2.0));
+    poses.push_back(createPose(2.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0));
+    poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(4.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+    poses.push_back(createPose(6.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+
+    // 1. Dist and yaw thresholds are given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4),
+        3U);
+
+      // Several nearest index within threshold
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0),
+        3U);
+
+      // Dist is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4),
+        3U);
+
+      // Yaw is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.0),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.0),
+        3U);
+
+      // Dist and yaw is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.0),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.0),
+        3U);
+    }
+
+    // 2. Dist threshold is given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0),
+        3U);
+
+      // Several nearest index within threshold
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0),
+        3U);
+
+      // Dist is out of range
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0),
+        3U);
+    }
+
+    // 3. No threshold is given
+    {
+      // Normal cases
+      EXPECT_EQ(
+        findFirstNearestIndexWithSoftConstraints(poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0)),
+        3U);
+      EXPECT_EQ(
+        findFirstNearestSegmentIndexWithSoftConstraints(
+          poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0)),
+        3U);
+    }
+  }
+}