feat(motion_utils): add resampling function for path with lane id (ti…

…er4#1502)

* feat(motion_utils): add resampling function for path with lane id

Signed-off-by: yutaka <purewater0901@gmail.com>

* add test

Signed-off-by: yutaka <purewater0901@gmail.com>

* update test

Signed-off-by: yutaka <purewater0901@gmail.com>

* add stop point flag

Signed-off-by: yutaka <purewater0901@gmail.com>

common/interpolation/CMakeLists.txt

@@ -8,7 +8,6 @@ ament_auto_add_library(interpolation SHARED
   src/linear_interpolation.cpp
   src/spline_interpolation.cpp
   src/spline_interpolation_points_2d.cpp
-  src/zero_order_hold.cpp
 )
 
 if(BUILD_TESTING)

common/interpolation/include/interpolation/zero_order_hold.hpp

@@ -21,9 +21,53 @@
 
 namespace interpolation
 {
-std::vector<double> zero_order_hold(
-  const std::vector<double> & base_keys, const std::vector<double> & base_values,
-  const std::vector<double> & query_keys, const double overlap_threshold = 1e-3);
+template <class T>
+std::vector<T> zero_order_hold(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values,
+  const std::vector<double> & query_keys, const double overlap_threshold = 1e-3)
+{
+  // throw exception for invalid arguments
+  interpolation_utils::validateKeys(base_keys, query_keys);
+
+  // when vectors are empty
+  if (base_keys.empty() || base_values.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  // when size of vectors are less than 2
+  if (base_keys.size() < 2 || base_values.size() < 2) {
+    throw std::invalid_argument(
+      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) +
+      ", base_values.size() = " + std::to_string(base_values.size()));
+  }
+
+  // when sizes of indices and values are not same
+  if (base_keys.size() != base_values.size()) {
+    throw std::invalid_argument("The size of base_keys and base_values are not the same.");
+  }
+
+  std::vector<T> query_values;
+  size_t closest_segment_idx = 0;
+  for (size_t i = 0; i < query_keys.size(); ++i) {
+    // Check if query_key is closes to the terminal point of the base keys
+    if (base_keys.back() - overlap_threshold < query_keys.at(i)) {
+      closest_segment_idx = base_keys.size() - 1;
+    } else {
+      for (size_t j = closest_segment_idx; j < base_keys.size() - 1; ++j) {
+        if (
+          base_keys.at(j) - overlap_threshold < query_keys.at(i) &&
+          query_keys.at(i) < base_keys.at(j + 1)) {
+          // find closest segment in base keys
+          closest_segment_idx = j;
+        }
+      }
+    }
+
+    query_values.push_back(base_values.at(closest_segment_idx));
+  }
+
+  return query_values;
+}
 }  // namespace interpolation
 
 #endif  // INTERPOLATION__ZERO_ORDER_HOLD_HPP_

common/interpolation/test/src/test_zero_order_hold.cpp

@@ -95,3 +95,54 @@ TEST(zero_order_hold_interpolation, vector_interpolation)
     }
   }
 }
+
+TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation)
+{
+  {  // straight: query_keys is same as base_keys
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<bool> base_values{true, true, false, true, true};
+    const std::vector<double> query_keys = base_keys;
+    const auto ans = base_values;
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_EQ(query_values.at(i), ans.at(i));
+    }
+  }
+
+  {  // straight: query_keys is random
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{true, false, false, true, false};
+    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
+    const std::vector<bool> ans = {true, true, false, false};
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_EQ(query_values.at(i), ans.at(i));
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<bool> base_values{true, true, false, true, false};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001};
+    const std::vector<double> ans = {true, true, false, true, true};
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{true, false, true, true, false};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001};
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}

common/motion_utils/include/motion_utils/resample/resample.hpp

@@ -27,6 +27,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_auto_planning_msgs/msg/path.hpp"
+#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 
 #include <algorithm>
@@ -55,6 +56,54 @@ std::vector<geometry_msgs::msg::Pose> resamplePath(
   const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
   const bool use_lerp_for_z = true);
 
+/**
+ * @brief A resampling function for a path with lane id. Note that in a default setting, position xy
+ * are resampled by spline interpolation, position z are resampled by linear interpolation,
+ * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is resampled
+ * by linear interpolation. Orientation of the resampled path are calculated by a forward difference
+ * method based on the interpolated position x and y. Moreover, lane_ids and is_final are also
+ * interpolated by zero order hold
+ * @param input_path input path to resample
+ * @param resampled_arclength arclength that contains length of each resampling points from initial
+ * point
+ * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
+ * y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ * Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample
+ * longitudinal and lateral velocity. Otherwise, it uses linear interpolation
+ * @return resampled path
+ */
+autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true);
+
+/**
+ * @brief A resampling function for a path with lane id. Note that in a default setting, position xy
+ * are resampled by spline interpolation, position z are resampled by linear interpolation,
+ * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is resampled
+ * by linear interpolation. Orientation of the resampled path are calculated by a forward difference
+ * method based on the interpolated position x and y. Moreover, lane_ids and is_final are also
+ * interpolated by zero order hold
+ * @param input_path input path to resample
+ * @param resampled_interval resampling interval
+ * point
+ * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
+ * y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ * Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample
+ * longitudinal and lateral velocity. Otherwise, it uses linear interpolation
+ * @param resample_input_path_stop_point If true, resample closest stop point in input path
+ * @return resampled path
+ */
+autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  const double resample_interval, const bool use_lerp_for_xy = false,
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true,
+  const bool resample_input_path_stop_point = true);
+
 /**
  * @brief A resampling function for a path. Note that in a default setting, position xy are
  * resampled by spline interpolation, position z are resampled by linear interpolation, longitudinal
@@ -116,12 +165,15 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
  * Otherwise, it uses spline interpolation
  * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample
  * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation
+ * @param resample_input_trajectory_stop_point If true, resample closest stop point in input
+ * trajectory
  * @return resampled trajectory
  */
 autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory,
   const double resample_interval, const bool use_lerp_for_xy = false,
-  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true);
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true,
+  const bool resample_input_trajectory_stop_point = true);
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_