fix(autoware_path_optimizer): incorrect application of input velocity due to badly mapping output trajectory to input trajectory

common/autoware_interpolation/README.md

@@ -91,7 +91,7 @@ $$
 
 ### Tridiagonal Matrix Algorithm
 
-We solve tridiagonal linear equation according to [this article](https://www.iist.ac.in/sites/default/files/people/tdma.pdf) where variables of linear equation are expressed as follows in the implementation.
+We solve tridiagonal linear equation according to [this article](https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm) where variables of linear equation are expressed as follows in the implementation.
 
 $$
 \begin{align}

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp

@@ -2149,6 +2149,38 @@ size_t findFirstNearestIndexWithSoftConstraints(
     }
   }
 
+  {  // with yaw 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 yaw =
+        autoware_utils::calc_yaw_deviation(autoware_utils::get_pose(points.at(i)), pose);
+
+      if (yaw_threshold < std::abs(yaw)) {
+        if (is_within_constraints) {
+          break;
+        }
+        continue;
+      }
+
+      const auto squared_dist =
+        autoware_utils::calc_squared_distance2d(points.at(i), pose.position);
+      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);
 }

common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -4872,11 +4872,11 @@ TEST(trajectory, findFirstNearestIndexWithSoftConstraints)
       EXPECT_EQ(
         findFirstNearestIndexWithSoftConstraints(
           poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4),
-        1U);
+        5U);
       EXPECT_EQ(
         findFirstNearestSegmentIndexWithSoftConstraints(
           poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4),
-        0U);
+        5U);
 
       // Yaw is out of range
       EXPECT_EQ(