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

@@ -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);
+```

common/motion_utils/include/motion_utils/motion_utils.hpp

@@ -17,6 +17,7 @@
 
 #include "motion_utils/marker/marker_helper.hpp"
 #include "motion_utils/resample/resample.hpp"
+#include "motion_utils/trajectory/path_with_lane_id.hpp"
 #include "motion_utils/trajectory/tmp_conversion.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "motion_utils/vehicle/vehicle_state_checker.hpp"

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_

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -1056,6 +1056,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_