feat(autoware_trajectory): improve get_underlying_base performance

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp

@@ -16,36 +16,12 @@
 #define AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_
 
 #include <cstddef>
-#include <set>
 #include <vector>
 
 namespace autoware::trajectory::detail
 {
 inline namespace helpers
 {
-/**
- * @brief Merge multiple vectors into one, keeping only unique elements.
- * @tparam Vectors Variadic template parameter for vector types.
- * @param vectors Vectors to be merged.
- * @return std::vector<double> Merged vector with unique elements.
- */
-template <typename... Vectors>
-std::vector<double> merge_vectors(const Vectors &... vectors)
-{
-  std::set<double> unique_elements;
-
-  // Helper function to insert elements into the set
-  auto insert_elements = [&unique_elements](const auto & vec) {
-    unique_elements.insert(vec.begin(), vec.end());
-  };
-
-  // Expand the parameter pack and insert elements from each vector
-  (insert_elements(vectors), ...);
-
-  // Convert the set to std::vector<double>
-  return {unique_elements.begin(), unique_elements.end()};
-}
-
 /**
  * @brief Ensures the output vector has at least a specified number of points by linearly
  * interpolating values between each input intervals

common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp

@@ -18,7 +18,6 @@
 #include "autoware/trajectory/detail/logging.hpp"
 #include "autoware/trajectory/interpolator/interpolator.hpp"
 
-#include <Eigen/Core>
 #include <rclcpp/logging.hpp>
 
 #include <algorithm>
@@ -42,6 +41,7 @@ class InterpolatedArray
   std::vector<double> bases_;
   std::vector<T> values_;
   std::shared_ptr<interpolator::InterpolatorInterface<T>> interpolator_;
+  std::function<void(const double s)> base_addition_callback_slot_{nullptr};
 
 public:
   /**
@@ -97,13 +97,22 @@ class InterpolatedArray
       bases_ = other.bases_;
       values_ = other.values_;
       interpolator_ = other.interpolator_->clone();
+      base_addition_callback_slot_ = other.base_addition_callback_slot_;
     }
     return *this;
   }
 
   // Destructor
   ~InterpolatedArray() = default;
 
+  /**
+   * @brief add the callback function to be executed when a new base is added to this class
+   */
+  void connect_base_addition_callback(std::function<void(const double s)> && signal)
+  {
+    base_addition_callback_slot_ = std::move(signal);
+  }
+
   /**
    * @brief Get the start value of the base.
    * @return The start value.
@@ -143,6 +152,12 @@ class InterpolatedArray
           return index;
         }  // Insert into bases
         bases.insert(it, val);
+
+        // execute the callback to notify that a new base has been added
+        if (parent_.base_addition_callback_slot_) {
+          std::invoke(parent_.base_addition_callback_slot_, value);
+        }
+
         // Insert into values at the corresponding position
         values.insert(values.begin() + index, value);
         return index;
@@ -211,12 +226,6 @@ class InterpolatedArray
    * @return The interpolated value.
    */
   T compute(const double x) const { return interpolator_->compute(x); }
-
-  /**
-   * @brief Get the underlying data of the array.
-   * @return A pair containing the axis and values.
-   */
-  std::pair<std::vector<double>, std::vector<T>> get_data() const { return {bases_, values_}; }
 };
 
 }  // namespace autoware::trajectory::detail

common/autoware_trajectory/include/autoware/trajectory/path_point.hpp

@@ -42,6 +42,15 @@ class Trajectory<autoware_planning_msgs::msg::PathPoint>
   std::shared_ptr<detail::InterpolatedArray<double>> heading_rate_rps_{
     nullptr};  //!< Heading rate in rad/s};
 
+  /**
+   * @brief add the event function to
+   * longitudinal_velocity_mps/lateral_velocity_mps/heading_rate_mps interpolator using observer
+   * pattern
+   * @note when a new base is added to longitudinal_velocity_mps for example, the addition is also
+   * notified and update_base() is triggered.
+   */
+  virtual void add_base_addition_callback();
+
 public:
   Trajectory();
   ~Trajectory() override = default;

common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp

@@ -36,6 +36,13 @@ class Trajectory<autoware_internal_planning_msgs::msg::PathPointWithLaneId>
 protected:
   std::shared_ptr<detail::InterpolatedArray<LaneIdType>> lane_ids_{nullptr};  //!< Lane ID
 
+  /**
+   * @brief add the event function to lane_ids additionally
+   * @note when a new base is added to lane_ids for example, the addition is also
+   * notified and update_base() is triggered.
+   */
+  void add_base_addition_callback() override;
+
 public:
   Trajectory();
   ~Trajectory() override = default;

common/autoware_trajectory/include/autoware/trajectory/point.hpp

@@ -49,6 +49,11 @@ class Trajectory<geometry_msgs::msg::Point>
 
   double start_{0.0}, end_{0.0};  //!< Start and end of the arc length of the trajectory
 
+  /**
+   * @brief add the input s if it is not contained in bases_
+   */
+  void update_bases(const double s);
+
   /**
    * @brief Validate the arc length is within the trajectory
    * @param s Arc length

common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp

@@ -47,6 +47,15 @@ class Trajectory<autoware_planning_msgs::msg::TrajectoryPoint>
   std::shared_ptr<detail::InterpolatedArray<double>> rear_wheel_angle_rad_{
     nullptr};  //!< Rear wheel angle in rad} Warning, this is not used
 
+  /**
+   * @brief add the event function to
+   * longitudinal_velocity_mps/lateral_velocity_mps/heading_rate_mps/acceleration_mps2/front_wheel_angle_rad/rear_wheel_angle_rad
+   * interpolator
+   * @note when a new base is added to longitudinal_velocity_mps for example, the addition is also
+   * notified and update_base() is triggered.
+   */
+  virtual void add_base_addition_callback();
+
 public:
   Trajectory();
   ~Trajectory() override = default;

common/autoware_trajectory/src/path_point.cpp

@@ -29,9 +29,20 @@ namespace autoware::trajectory
 
 using PointType = autoware_planning_msgs::msg::PathPoint;
 
+void Trajectory<PointType>::add_base_addition_callback()
+{
+  longitudinal_velocity_mps_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+  lateral_velocity_mps_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+  heading_rate_rps_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+}
+
 Trajectory<PointType>::Trajectory()
 {
   Builder::defaults(this);
+  add_base_addition_callback();
 }
 
 Trajectory<PointType>::Trajectory(const Trajectory & rhs)
@@ -42,6 +53,7 @@ Trajectory<PointType>::Trajectory(const Trajectory & rhs)
     std::make_shared<detail::InterpolatedArray<double>>(*rhs.lateral_velocity_mps_)),
   heading_rate_rps_(std::make_shared<detail::InterpolatedArray<double>>(*rhs.heading_rate_rps_))
 {
+  add_base_addition_callback();
 }
 
 Trajectory<PointType> & Trajectory<PointType>::operator=(const Trajectory & rhs)
@@ -52,6 +64,7 @@ Trajectory<PointType> & Trajectory<PointType>::operator=(const Trajectory & rhs)
     *lateral_velocity_mps_ = *rhs.lateral_velocity_mps_;
     *heading_rate_rps_ = *rhs.heading_rate_rps_;
   }
+  add_base_addition_callback();
   return *this;
 }
 
@@ -98,16 +111,7 @@ interpolator::InterpolationResult Trajectory<PointType>::build(
 
 std::vector<double> Trajectory<PointType>::get_underlying_bases() const
 {
-  auto get_bases = [](const auto & interpolated_array) {
-    auto [bases, values] = interpolated_array.get_data();
-    return bases;
-  };
-
-  auto bases = detail::merge_vectors(
-    bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()),
-    get_bases(this->heading_rate_rps()));
-
-  bases = detail::crop_bases(bases, start_, end_);
+  auto bases = detail::crop_bases(bases_, start_, end_);
   std::transform(
     bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; });
   return bases;

common/autoware_trajectory/src/path_point_with_lane_id.cpp

@@ -26,9 +26,16 @@ namespace autoware::trajectory
 
 using PointType = autoware_internal_planning_msgs::msg::PathPointWithLaneId;
 
+void Trajectory<PointType>::add_base_addition_callback()
+{
+  BaseClass::add_base_addition_callback();
+  lane_ids_->connect_base_addition_callback([&](const double s) { return this->update_bases(s); });
+}
+
 Trajectory<PointType>::Trajectory()
 {
   Builder::defaults(this);
+  add_base_addition_callback();
 }
 
 Trajectory<PointType> & Trajectory<PointType>::operator=(const Trajectory & rhs)
@@ -37,6 +44,7 @@ Trajectory<PointType> & Trajectory<PointType>::operator=(const Trajectory & rhs)
     BaseClass::operator=(rhs);
     lane_ids_ = std::make_shared<detail::InterpolatedArray<LaneIdType>>(this->lane_ids());
   }
+  add_base_addition_callback();
   return *this;
 }
 
@@ -66,16 +74,7 @@ interpolator::InterpolationResult Trajectory<PointType>::build(
 
 std::vector<double> Trajectory<PointType>::get_underlying_bases() const
 {
-  auto get_bases = [](const auto & interpolated_array) {
-    auto [bases, values] = interpolated_array.get_data();
-    return bases;
-  };
-
-  auto bases = detail::merge_vectors(
-    bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()),
-    get_bases(this->heading_rate_rps()), get_bases(this->lane_ids()));
-
-  bases = detail::crop_bases(bases, start_, end_);
+  auto bases = detail::crop_bases(bases_, start_, end_);
 
   std::transform(
     bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; });

common/autoware_trajectory/src/point.cpp

@@ -124,6 +124,20 @@ std::vector<double> Trajectory<PointType>::get_underlying_bases() const
   return bases;
 }
 
+void Trajectory<PointType>::update_bases(const double s)
+{
+  const auto it = std::lower_bound(bases_.begin(), bases_.end(), s);
+  if (it == bases_.end()) {
+    // NOTE(soblin): the extension of base(or extrapolation) will be supported by other API.
+    return;
+  }
+  if (*it == s) {
+    // already inserted
+    return;
+  }
+  bases_.insert(it, s);
+}
+
 double Trajectory<PointType>::length() const
 {
   return end_ - start_;

common/autoware_trajectory/src/pose.cpp

@@ -88,6 +88,7 @@ PointType Trajectory<PointType>::compute(const double s) const
   PointType result;
   result.position = BaseClass::compute(s);
   const auto s_clamp = clamp(s);
+  // NOTE(soblin): Ideally azimuth() should be used? But okay if the interpolation is not rough
   result.orientation = orientation_interpolator_->compute(s_clamp);
   return result;
 }

common/autoware_trajectory/src/trajectory_point.cpp

@@ -31,9 +31,26 @@ namespace autoware::trajectory
 
 using PointType = autoware_planning_msgs::msg::TrajectoryPoint;
 
+void Trajectory<PointType>::add_base_addition_callback()
+{
+  longitudinal_velocity_mps_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+  lateral_velocity_mps_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+  heading_rate_rps_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+  acceleration_mps2_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+  front_wheel_angle_rad_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+  rear_wheel_angle_rad_->connect_base_addition_callback(
+    [&](const double s) { return this->update_bases(s); });
+}
+
 Trajectory<PointType>::Trajectory()
 {
   Builder::defaults(this);
+  add_base_addition_callback();
 }
 
 Trajectory<PointType>::Trajectory(const Trajectory & rhs)
@@ -49,6 +66,7 @@ Trajectory<PointType>::Trajectory(const Trajectory & rhs)
   rear_wheel_angle_rad_(
     std::make_shared<detail::InterpolatedArray<double>>(*rhs.rear_wheel_angle_rad_))
 {
+  add_base_addition_callback();
 }
 
 Trajectory<PointType> & Trajectory<PointType>::operator=(const Trajectory & rhs)
@@ -62,6 +80,7 @@ Trajectory<PointType> & Trajectory<PointType>::operator=(const Trajectory & rhs)
     *front_wheel_angle_rad_ = *rhs.front_wheel_angle_rad_;
     *rear_wheel_angle_rad_ = *rhs.rear_wheel_angle_rad_;
   }
+  add_base_addition_callback();
   return *this;
 }
 
@@ -141,17 +160,7 @@ interpolator::InterpolationResult Trajectory<PointType>::build(
 
 std::vector<double> Trajectory<PointType>::get_underlying_bases() const
 {
-  auto get_bases = [](const auto & interpolated_array) {
-    auto [bases, values] = interpolated_array.get_data();
-    return bases;
-  };
-
-  auto bases = detail::merge_vectors(
-    bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()),
-    get_bases(this->heading_rate_rps()), get_bases(this->acceleration_mps2()),
-    get_bases(this->front_wheel_angle_rad()), get_bases(this->rear_wheel_angle_rad()));
-
-  bases = detail::crop_bases(bases, start_, end_);
+  auto bases = detail::crop_bases(bases_, start_, end_);
   std::transform(
     bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; });
   return bases;