Merge branch 'main' into refactor/pure_pursuit

Author: mitsudome-r

.github/CODEOWNERS

@@ -94,7 +94,7 @@ localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier
 localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp
 localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
+localization/autoware_pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp

.github/workflows/cppcheck-daily.yaml .github/workflows/cppcheck-weekly.yaml

@@ -1,12 +1,12 @@
-name: cppcheck-daily
+name: cppcheck-weekly
 
 on:
   schedule:
-    - cron: 0 0 * * *
+    - cron: 0 0 * * 1
   workflow_dispatch:
 
 jobs:
-  cppcheck-daily:
+  cppcheck-weekly:
     runs-on: ubuntu-latest
 
     steps:

common/autoware_motion_utils/README.md

@@ -97,19 +97,6 @@ const size_t traffic_obj_nearest_idx = findNearestIndexFromLaneId(path_with_lane
 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_seg_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);
-```
-
 ## For developers
 
 Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time.

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp

@@ -19,6 +19,7 @@
 
 #include <Eigen/Dense>
 
+#include <memory>
 #include <vector>
 
 namespace autoware::motion_utils::trajectory_container::interpolator
@@ -31,14 +32,9 @@ namespace autoware::motion_utils::trajectory_container::interpolator
  */
 class AkimaSpline : public Interpolator<double>
 {
-  template <typename InterpolatorType>
-  friend class InterpolatorCreator;
-
 private:
   Eigen::VectorXd a_, b_, c_, d_;  ///< Coefficients for the Akima spline.
 
-  AkimaSpline() = default;
-
   /**
    * @brief Compute the spline parameters.
    *
@@ -85,12 +81,21 @@ class AkimaSpline : public Interpolator<double>
   [[nodiscard]] double compute_second_derivative_impl(const double & s) const override;
 
 public:
+  AkimaSpline() = default;
+
   /**
    * @brief Get the minimum number of required points for the interpolator.
    *
    * @return The minimum number of required points.
    */
   [[nodiscard]] size_t minimum_required_points() const override { return 5; }
+
+  /**
+   * @brief Clone the interpolator.
+   *
+   * @return A shared pointer to a new instance of the interpolator.
+   */
+  [[nodiscard]] std::shared_ptr<Interpolator<double>> clone() const override;
 };
 
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp

@@ -31,15 +31,10 @@ namespace autoware::motion_utils::trajectory_container::interpolator
  */
 class CubicSpline : public Interpolator<double>
 {
-  template <typename InterpolatorType>
-  friend class InterpolatorCreator;
-
 private:
   Eigen::VectorXd a_, b_, c_, d_;  ///< Coefficients for the cubic spline.
   Eigen::VectorXd h_;              ///< Interval sizes between axis points.
 
-  CubicSpline() = default;
-
   /**
    * @brief Compute the spline parameters.
    *
@@ -87,12 +82,21 @@ class CubicSpline : public Interpolator<double>
   [[nodiscard]] double compute_second_derivative_impl(const double & s) const override;
 
 public:
+  CubicSpline() = default;
+
   /**
    * @brief Get the minimum number of required points for the interpolator.
    *
    * @return The minimum number of required points.
    */
   [[nodiscard]] size_t minimum_required_points() const override { return 4; }
+
+  /**
+   * @brief Clone the interpolator.
+   *
+   * @return A shared pointer to a new instance of the interpolator.
+   */
+  [[nodiscard]] std::shared_ptr<Interpolator<double>> clone() const override;
 };
 
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp

@@ -17,6 +17,8 @@
 
 #include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp"
 
+#include <memory>
+
 namespace autoware::motion_utils::trajectory_container::interpolator
 {
 /**
@@ -29,6 +31,8 @@ namespace autoware::motion_utils::trajectory_container::interpolator
 template <typename T>
 class Interpolator : public detail::InterpolatorCommonImpl<T>
 {
+public:
+  [[nodiscard]] virtual std::shared_ptr<Interpolator<T>> clone() const = 0;
 };
 
 /**
@@ -84,6 +88,8 @@ class Interpolator<double> : public detail::InterpolatorCommonImpl<double>
     this->validate_compute_input(s);
     return compute_second_derivative_impl(s);
   }
+
+  [[nodiscard]] virtual std::shared_ptr<Interpolator<double>> clone() const = 0;
 };
 
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp

@@ -19,6 +19,7 @@
 
 #include <Eigen/Dense>
 
+#include <memory>
 #include <vector>
 
 namespace autoware::motion_utils::trajectory_container::interpolator
@@ -31,17 +32,9 @@ namespace autoware::motion_utils::trajectory_container::interpolator
  */
 class Linear : public Interpolator<double>
 {
-  template <typename InterpolatorType>
-  friend class InterpolatorCreator;
-
 private:
   Eigen::VectorXd values_;  ///< Interpolation values.
 
-  /**
-   * @brief Default constructor.
-   */
-  Linear() = default;
-
   /**
    * @brief Build the interpolator with the given values.
    *
@@ -77,12 +70,24 @@ class Linear : public Interpolator<double>
   [[nodiscard]] double compute_second_derivative_impl(const double &) const override;
 
 public:
+  /**
+   * @brief Default constructor.
+   */
+  Linear() = default;
+
   /**
    * @brief Get the minimum number of required points for the interpolator.
    *
    * @return The minimum number of required points.
    */
   [[nodiscard]] size_t minimum_required_points() const override;
+
+  /**
+   * @brief Clone the interpolator.
+   *
+   * @return A shared pointer to the cloned interpolator.
+   */
+  [[nodiscard]] std::shared_ptr<Interpolator<double>> clone() const override;
 };
 
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp

@@ -17,6 +17,8 @@
 
 #include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp"
 
+#include <memory>
+
 namespace autoware::motion_utils::trajectory_container::interpolator
 {
 
@@ -40,11 +42,18 @@ class NearestNeighbor;
 template <typename T>
 class NearestNeighbor : public detail::NearestNeighborCommonImpl<T>
 {
-  template <typename InterpolatorType>
-  friend class InterpolatorCreator;
-
-private:
+public:
   NearestNeighbor() = default;
+
+  /**
+   * @brief Clone the interpolator.
+   *
+   * @return A shared pointer to a new instance of the interpolator.
+   */
+  [[nodiscard]] std::shared_ptr<Interpolator<double>> clone() const override
+  {
+    return std::make_shared<NearestNeighbor<T>>(*this);
+  }
 };
 
 /**
@@ -55,12 +64,7 @@ class NearestNeighbor : public detail::NearestNeighborCommonImpl<T>
 template <>
 class NearestNeighbor<double> : public detail::NearestNeighborCommonImpl<double>
 {
-  template <typename InterpolatorType>
-  friend class InterpolatorCreator;
-
 private:
-  NearestNeighbor() = default;
-
   /**
    * @brief Compute the first derivative at the given point.
    *
@@ -76,6 +80,19 @@ class NearestNeighbor<double> : public detail::NearestNeighborCommonImpl<double>
    * @return The second derivative.
    */
   [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; }
+
+public:
+  NearestNeighbor() = default;
+
+  /**
+   * @brief Clone the interpolator.
+   *
+   * @return A shared pointer to a new instance of the interpolator.
+   */
+  [[nodiscard]] std::shared_ptr<Interpolator<double>> clone() const override
+  {
+    return std::make_shared<NearestNeighbor<double>>(*this);
+  }
 };
 
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp

@@ -17,6 +17,8 @@
 
 #include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp"
 
+#include <memory>
+
 namespace autoware::motion_utils::trajectory_container::interpolator
 {
 
@@ -40,11 +42,18 @@ class Stairstep;
 template <typename T>
 class Stairstep : public detail::StairstepCommonImpl<T>
 {
-  template <typename InterpolatorType>
-  friend class InterpolatorCreator;
-
-private:
+public:
   Stairstep() = default;
+
+  /**
+   * @brief Clone the interpolator.
+   *
+   * @return A shared pointer to a new instance of the interpolator.
+   */
+  [[nodiscard]] std::shared_ptr<Interpolator<T>> clone() const override
+  {
+    return std::make_shared<Stairstep<T>>(*this);
+  }
 };
 
 /**
@@ -55,11 +64,7 @@ class Stairstep : public detail::StairstepCommonImpl<T>
 template <>
 class Stairstep<double> : public detail::StairstepCommonImpl<double>
 {
-  template <typename InterpolatorType>
-  friend class InterpolatorCreator;
-
 private:
-  Stairstep() = default;
   /**
    * @brief Compute the first derivative at the given point.
    *
@@ -75,6 +80,19 @@ class Stairstep<double> : public detail::StairstepCommonImpl<double>
    * @return The second derivative.
    */
   [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; }
+
+public:
+  Stairstep() = default;
+
+  /**
+   * @brief Clone the interpolator.
+   *
+   * @return A shared pointer to a new instance of the interpolator.
+   */
+  [[nodiscard]] std::shared_ptr<Interpolator<double>> clone() const override
+  {
+    return std::make_shared<Stairstep<double>>(*this);
+  }
 };
 
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp

@@ -17,6 +17,7 @@
 #include <Eigen/Dense>
 
 #include <cmath>
+#include <memory>
 #include <vector>
 
 namespace autoware::motion_utils::trajectory_container::interpolator
@@ -90,4 +91,9 @@ double AkimaSpline::compute_second_derivative_impl(const double & s) const
   return 2 * c_[i] + 6 * d_[i] * dx;
 }
 
+std::shared_ptr<Interpolator<double>> AkimaSpline::clone() const
+{
+  return std::make_shared<AkimaSpline>(*this);
+}
+
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp

@@ -91,4 +91,9 @@ double CubicSpline::compute_second_derivative_impl(const double & s) const
   return 2 * c_(i) + 6 * d_(i) * dx;
 }
 
+std::shared_ptr<Interpolator<double>> CubicSpline::clone() const
+{
+  return std::make_shared<CubicSpline>(*this);
+}
+
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp

@@ -16,6 +16,7 @@
 
 #include <Eigen/Dense>
 
+#include <memory>
 #include <vector>
 
 namespace autoware::motion_utils::trajectory_container::interpolator
@@ -59,4 +60,9 @@ size_t Linear::minimum_required_points() const
   return 2;
 }
 
+std::shared_ptr<Interpolator<double>> Linear::clone() const
+{
+  return std::make_shared<Linear>(*this);
+}
+
 }  // namespace autoware::motion_utils::trajectory_container::interpolator