feat(obstacle_avoidance_planner): fix optimization at end of trajecto…

…ry (#309) * feat(behavior_path_planner, obstacle_avoidance_planner): add new drivable area (autowarefoundation#2472) * update Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update obstacle avoidance planner Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * clean code Signed-off-by: yutaka <purewater0901@gmail.com> * uddate Signed-off-by: yutaka <purewater0901@gmail.com> * clean code Signed-off-by: yutaka <purewater0901@gmail.com> * remove resample Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * add orientation Signed-off-by: yutaka <purewater0901@gmail.com> * change color Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * remove drivable area Signed-off-by: yutaka <purewater0901@gmail.com> * add flag Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update color Signed-off-by: yutaka <purewater0901@gmail.com> * fix some codes Signed-off-by: yutaka <purewater0901@gmail.com> * change to makerker array Signed-off-by: yutaka <purewater0901@gmail.com> * change avoidance utils Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: yutaka <purewater0901@gmail.com> * feat(motion_utils): add points resample function (autowarefoundation#2465) Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: yutaka <purewater0901@gmail.com> * fix(motion_utils): rename sampling function (autowarefoundation#2469) Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: yutaka <purewater0901@gmail.com> * fix(behavior_path_planner): fix a merge failure Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * feat(interpolation): add curvature calculation (autowarefoundation#2801) * feat(interpolation): add curvature calculation Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add test Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update obstacle avoidance planner, static centerline optimizer, tier4_planning_launch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update velocity on joint and correct trajectory z Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * ci(pre-commit): autofix * fix(behavior_path_planner): backport drivable area for loop route (#310) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Mar 14, 2023 · ba6ff2d · ba6ff2d
1 parent 556a812
commit ba6ff2d
Show file tree

Hide file tree

Showing 94 changed files with 8,204 additions and 8,899 deletions.
diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp
@@ -70,9 +70,11 @@ class SplineInterpolation
 {
 public:
   SplineInterpolation() = default;
-
-  void calcSplineCoefficients(
-    const std::vector<double> & base_keys, const std::vector<double> & base_values);
+  SplineInterpolation(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values)
+  {
+    calcSplineCoefficients(base_keys, base_values);
+  }
 
   //!< @brief get values of spline interpolation on designated sampling points.
   //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
@@ -86,9 +88,21 @@ class SplineInterpolation
   //            return value will be dx/dt(t) vector
   std::vector<double> getSplineInterpolatedDiffValues(const std::vector<double> & query_keys) const;
 
+  //!< @brief get 2nd differential values of spline interpolation on designated sampling points.
+  //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
+  //            meaning that spline interpolation was applied to x(t),
+  //            return value will be d^2/dt^2(t) vector
+  std::vector<double> getSplineInterpolatedQuadDiffValues(
+    const std::vector<double> & query_keys) const;
+
+  size_t getSize() const { return base_keys_.size(); }
+
 private:
   std::vector<double> base_keys_;
   interpolation::MultiSplineCoef multi_spline_coef_;
+
+  void calcSplineCoefficients(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values);
 };
 
 #endif  // INTERPOLATION__SPLINE_INTERPOLATION_HPP_
diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp
@@ -37,20 +37,19 @@ std::vector<double> splineYawFromPoints(const std::vector<T> & points);
 // SplineInterpolationPoints2d spline;
 // // memorize pre-interpolation result internally
 // spline.calcSplineCoefficients(base_keys, base_values);
-// const auto interpolation_result1 = spline.getSplineInterpolatedPoints(
+// const auto interpolation_result1 = spline.getSplineInterpolatedPoint(
 //   base_keys, query_keys1);
-// const auto interpolation_result2 = spline.getSplineInterpolatedPoints(
+// const auto interpolation_result2 = spline.getSplineInterpolatedPoint(
 //   base_keys, query_keys2);
-// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaws(
+// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaw(
 //   base_keys, query_keys1);
 // ```
 class SplineInterpolationPoints2d
 {
 public:
   SplineInterpolationPoints2d() = default;
-
   template <typename T>
-  void calcSplineCoefficients(const std::vector<T> & points)
+  explicit SplineInterpolationPoints2d(const std::vector<T> & points)
   {
     std::vector<geometry_msgs::msg::Point> points_inner;
     for (const auto & p : points) {
@@ -63,15 +62,29 @@ class SplineInterpolationPoints2d
   // std::vector<geometry_msgs::msg::Point> getSplineInterpolatedPoints(const double width);
   // std::vector<geometry_msgs::msg::Pose> getSplineInterpolatedPoses(const double width);
 
+  // pose (= getSplineInterpolatedPoint + getSplineInterpolatedYaw)
+  geometry_msgs::msg::Pose getSplineInterpolatedPose(const size_t idx, const double s) const;
+
+  // point
   geometry_msgs::msg::Point getSplineInterpolatedPoint(const size_t idx, const double s) const;
+
+  // yaw
   double getSplineInterpolatedYaw(const size_t idx, const double s) const;
+  std::vector<double> getSplineInterpolatedYaws() const;
+
+  // curvature
+  double getSplineInterpolatedCurvature(const size_t idx, const double s) const;
+  std::vector<double> getSplineInterpolatedCurvatures() const;
 
+  size_t getSize() const { return base_s_vec_.size(); }
+  size_t getOffsetIndex(const size_t idx, const double offset) const;
   double getAccumulatedLength(const size_t idx) const;
 
 private:
   void calcSplineCoefficientsInner(const std::vector<geometry_msgs::msg::Point> & points);
   SplineInterpolation spline_x_;
   SplineInterpolation spline_y_;
+  SplineInterpolation spline_z_;
 
   std::vector<double> base_s_vec_;
 };

diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp
@@ -85,10 +85,8 @@ std::vector<double> spline(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys)
 {
-  SplineInterpolation interpolator;
-
   // calculate spline coefficients
-  interpolator.calcSplineCoefficients(base_keys, base_values);
+  SplineInterpolation interpolator(base_keys, base_values);
 
   // interpolate base_keys at query_keys
   return interpolator.getSplineInterpolatedValues(query_keys);
@@ -266,3 +264,26 @@ std::vector<double> SplineInterpolation::getSplineInterpolatedDiffValues(
 
   return res;
 }
+
+std::vector<double> SplineInterpolation::getSplineInterpolatedQuadDiffValues(
+  const std::vector<double> & query_keys) const
+{
+  // throw exceptions for invalid arguments
+  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys);
+
+  const auto & a = multi_spline_coef_.a;
+  const auto & b = multi_spline_coef_.b;
+
+  std::vector<double> res;
+  size_t j = 0;
+  for (const auto & query_key : validated_query_keys) {
+    while (base_keys_.at(j + 1) < query_key) {
+      ++j;
+    }
+
+    const double ds = query_key - base_keys_.at(j);
+    res.push_back(2.0 * b.at(j) + 6.0 * a.at(j) * ds);
+  }
+
+  return res;
+}
diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp
@@ -35,12 +35,13 @@ std::vector<double> calcEuclidDist(const std::vector<double> & x, const std::vec
   return dist_v;
 }
 
-std::array<std::vector<double>, 3> getBaseValues(
+std::array<std::vector<double>, 4> getBaseValues(
   const std::vector<geometry_msgs::msg::Point> & points)
 {
   // calculate x, y
   std::vector<double> base_x;
   std::vector<double> base_y;
+  std::vector<double> base_z;
   for (size_t i = 0; i < points.size(); i++) {
     const auto & current_pos = points.at(i);
     if (i > 0) {
@@ -53,16 +54,17 @@ std::array<std::vector<double>, 3> getBaseValues(
     }
     base_x.push_back(current_pos.x);
     base_y.push_back(current_pos.y);
+    base_z.push_back(current_pos.z);
   }
 
   // calculate base_keys, base_values
-  if (base_x.size() < 2 || base_y.size() < 2) {
+  if (base_x.size() < 2 || base_y.size() < 2 || base_z.size() < 2) {
     throw std::logic_error("The numbef of unique points is not enough.");
   }
 
   const std::vector<double> base_s = calcEuclidDist(base_x, base_y);
 
-  return {base_s, base_x, base_y};
+  return {base_s, base_x, base_y, base_z};
 }
 }  // namespace
 
@@ -73,14 +75,14 @@ std::array<std::vector<double>, 3> slerp2dFromXY(
   const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
   const std::vector<double> & base_y_values, const std::vector<double> & query_keys)
 {
-  SplineInterpolation interpolator_x, interpolator_y;
-  std::vector<double> yaw_vec;
-
   // calculate spline coefficients
-  interpolator_x.calcSplineCoefficients(base_keys, base_x_values);
-  interpolator_y.calcSplineCoefficients(base_keys, base_y_values);
+  SplineInterpolation interpolator_x(base_keys, base_x_values);
+  SplineInterpolation interpolator_y(base_keys, base_y_values);
   const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys);
   const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys);
+
+  // calculate yaw
+  std::vector<double> yaw_vec;
   for (size_t i = 0; i < diff_x.size(); i++) {
     double yaw = std::atan2(diff_y[i], diff_x[i]);
     yaw_vec.push_back(yaw);
@@ -94,10 +96,8 @@ std::array<std::vector<double>, 3> slerp2dFromXY(
 template <typename T>
 std::vector<double> splineYawFromPoints(const std::vector<T> & points)
 {
-  SplineInterpolationPoints2d interpolator;
-
   // calculate spline coefficients
-  interpolator.calcSplineCoefficients(points);
+  SplineInterpolationPoints2d interpolator(points);
 
   // interpolate base_keys at query_keys
   std::vector<double> yaw_vec;
@@ -112,6 +112,16 @@ template std::vector<double> splineYawFromPoints(
 
 }  // namespace interpolation
 
+geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose(
+  const size_t idx, const double s) const
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position = getSplineInterpolatedPoint(idx, s);
+  pose.orientation =
+    tier4_autoware_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s));
+  return pose;
+}
+
 geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint(
   const size_t idx, const double s) const
 {
@@ -129,10 +139,12 @@ geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoin
 
   const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0);
   const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0);
+  const double z = spline_z_.getSplineInterpolatedValues({whole_s}).at(0);
 
   geometry_msgs::msg::Point geom_point;
   geom_point.x = x;
   geom_point.y = y;
+  geom_point.z = z;
   return geom_point;
 }
 
@@ -142,18 +154,64 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c
     throw std::out_of_range("idx is out of range.");
   }
 
-  double whole_s = base_s_vec_.at(idx) + s;
-  if (whole_s < base_s_vec_.front()) {
-    whole_s = base_s_vec_.front();
+  const double whole_s =
+    std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back());
+
+  const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+  const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+
+  return std::atan2(diff_y, diff_x);
+}
+
+std::vector<double> SplineInterpolationPoints2d::getSplineInterpolatedYaws() const
+{
+  std::vector<double> yaw_vec;
+  for (size_t i = 0; i < spline_x_.getSize(); ++i) {
+    const double yaw = getSplineInterpolatedYaw(i, 0.0);
+    yaw_vec.push_back(yaw);
   }
-  if (whole_s > base_s_vec_.back()) {
-    whole_s = base_s_vec_.back();
+  return yaw_vec;
+}
+
+double SplineInterpolationPoints2d::getSplineInterpolatedCurvature(
+  const size_t idx, const double s) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
   }
 
+  const double whole_s =
+    std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back());
+
   const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0);
   const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0);
 
-  return std::atan2(diff_y, diff_x);
+  const double quad_diff_x = spline_x_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0);
+  const double quad_diff_y = spline_y_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0);
+
+  return (diff_x * quad_diff_y - quad_diff_x * diff_y) /
+         std::pow(std::pow(diff_x, 2) + std::pow(diff_y, 2), 1.5);
+}
+
+std::vector<double> SplineInterpolationPoints2d::getSplineInterpolatedCurvatures() const
+{
+  std::vector<double> curvature_vec;
+  for (size_t i = 0; i < spline_x_.getSize(); ++i) {
+    const double curvature = getSplineInterpolatedCurvature(i, 0.0);
+    curvature_vec.push_back(curvature);
+  }
+  return curvature_vec;
+}
+
+size_t SplineInterpolationPoints2d::getOffsetIndex(const size_t idx, const double offset) const
+{
+  const double whole_s = base_s_vec_.at(idx) + offset;
+  for (size_t s_idx = 0; s_idx < base_s_vec_.size(); ++s_idx) {
+    if (whole_s < base_s_vec_.at(s_idx)) {
+      return s_idx;
+    }
+  }
+  return base_s_vec_.size() - 1;
 }
 
 double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const
@@ -172,8 +230,10 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner(
   base_s_vec_ = base.at(0);
   const auto & base_x_vec = base.at(1);
   const auto & base_y_vec = base.at(2);
+  const auto & base_z_vec = base.at(3);
 
   // calculate spline coefficients
-  spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec);
-  spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec);
+  spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec);
+  spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec);
+  spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec);
 }