refactor(path_shifter): make member functions const as far as possible

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp

@@ -77,26 +77,40 @@ class PathShifter
    */
   void setShiftPoints(const std::vector<ShiftPoint> & points);
 
+  /**
+   * @brief  Get shift points.
+   */
   std::vector<ShiftPoint> getShiftPoints() const { return shift_points_; }
-  PathWithLaneId getReferencePath() const { return reference_path_; }
+
+  /**
+   * @brief  Get shift points size.
+   */
   size_t getShiftPointsSize() const { return shift_points_.size(); }
 
+  /**
+   * @brief  Get base offset.
+   */
   double getBaseOffset() const { return base_offset_; }
 
+  /**
+   * @brief  Get reference path.
+   */
+  PathWithLaneId getReferencePath() const { return reference_path_; }
+
   /**
    * @brief  Generate a shifted path according to the given reference path and shift points.
    * @return False if the path is empty or shift points have conflicts.
    */
   bool generate(
-    ShiftedPath * shift_path, const bool offset_back = true,
-    const SHIFT_TYPE type = SHIFT_TYPE::SPLINE);
+    ShiftedPath * shifted_path, const bool offset_back = true,
+    const SHIFT_TYPE type = SHIFT_TYPE::SPLINE) const;
 
   /**
    * @brief Remove behind shift points and add the removed offset to the base_offset_.
    * @details The previous offset information is stored in the base_offset_.
    *          This should be called after generate().
    */
-  void removeBehindShiftPointAndSetBaseOffset(const Pose & pose, const size_t nearest_idx);
+  void removeBehindShiftPointAndSetBaseOffset(const size_t nearest_idx);
 
   ////////////////////////////////////////
   // Utility Functions
@@ -139,10 +153,6 @@ class PathShifter
       return base_offset_;
     }
 
-    // TODO(Horibe) enable this with const
-    // if (!is_index_aligned_) {
-    //   updateShiftPointIndices();
-    // }
     const auto furthest = std::max_element(
       shift_points_.begin(), shift_points_.end(),
       [](auto & a, auto & b) { return a.end_idx < b.end_idx; });
@@ -167,7 +177,7 @@ class PathShifter
    * @brief  Calculate the theoretical lateral jerk by spline shifting for current shift_points_.
    * @return Jerk array. THe size is same as the shift points.
    */
-  std::vector<double> calcLateralJerk();
+  std::vector<double> calcLateralJerk() const;
 
 private:
   // The reference path along which the shift will be performed.
@@ -179,34 +189,35 @@ class PathShifter
   // The amount of shift length to the entire path.
   double base_offset_{0.0};
 
-  // Flag to check the path index is aligned. (cleared when new path or shift points are received)
-  bool is_index_aligned_{false};
+  // Logger
+  mutable rclcpp::Logger logger_{
+    rclcpp::get_logger("behavior_path_planner").get_child("path_shifter")};
 
-  rclcpp::Logger logger_{rclcpp::get_logger("behavior_path_planner").get_child("path_shifter")};
-  rclcpp::Clock clock_{RCL_ROS_TIME};
+  // Clock
+  mutable rclcpp::Clock clock_{RCL_ROS_TIME};
 
   /**
    * @brief Calculate path index for shift_points and set is_index_aligned_ to true.
    */
-  void updateShiftPointIndices();
+  void updateShiftPointIndices(ShiftPointArray & shift_points) const;
 
   /**
    * @brief Sort the points in order from the front of the path.
    */
-  bool sortShiftPointsAlongPath(const PathWithLaneId & path);
+  void sortShiftPointsAlongPath(ShiftPointArray & shift_points) const;
 
   /**
    * @brief Generate shifted path from reference_path_ and shift_points_ with linear shifting.
    */
-  void applyLinearShifter(ShiftedPath * shifted_path);
+  void applyLinearShifter(ShiftedPath * shifted_path) const;
 
   /**
    * @brief Generate shifted path from reference_path_ and shift_points_ with spline_based shifting.
    * @details Calculate the shift so that the horizontal jerk remains constant. This is achieved by
    *          dividing the shift interval into four parts and apply a cubic spline to them.
    *          The resultant shifting shape is closed to the Clothoid curve.
    */
-  void applySplineShifter(ShiftedPath * shifted_path, const bool offset_back);
+  void applySplineShifter(ShiftedPath * shifted_path, const bool offset_back) const;
 
   ////////////////////////////////////////
   // Helper Functions
@@ -217,9 +228,9 @@ class PathShifter
    */
   bool checkShiftPointsAlignment(const ShiftPointArray & shift_points) const;
 
-  void addLateralOffsetOnIndexPoint(ShiftedPath * point, double offset, size_t index) const;
+  void addLateralOffsetOnIndexPoint(ShiftedPath * path, double offset, size_t index) const;
 
-  void shiftBaseLength(ShiftedPath * point, double offset) const;
+  void shiftBaseLength(ShiftedPath * path, double offset) const;
 
   void setBaseOffset(const double val)
   {

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2341,7 +2341,7 @@ ShiftedPath AvoidanceModule::generateAvoidancePath(PathShifter & path_shifter) c
 void AvoidanceModule::postProcess(PathShifter & path_shifter) const
 {
   const size_t nearest_idx = findEgoIndex(path_shifter.getReferencePath().points);
-  path_shifter.removeBehindShiftPointAndSetBaseOffset(getEgoPose().pose, nearest_idx);
+  path_shifter.removeBehindShiftPointAndSetBaseOffset(nearest_idx);
 }
 
 void AvoidanceModule::updateData()

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -176,7 +176,7 @@ void SideShiftModule::updateData()
     current_lane, reference_pose.pose, p.backward_path_length, p.forward_path_length);
 
   const size_t nearest_idx = findEgoIndex(path_shifter_.getReferencePath().points);
-  path_shifter_.removeBehindShiftPointAndSetBaseOffset(planner_data_->self_pose->pose, nearest_idx);
+  path_shifter_.removeBehindShiftPointAndSetBaseOffset(nearest_idx);
 }
 
 bool SideShiftModule::addShiftPoint()

planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp

@@ -43,25 +43,35 @@ std::string toStr(const behavior_path_planner::ShiftPoint & p)
 
 namespace behavior_path_planner
 {
+
+using motion_utils::findNearestIndex;
+using motion_utils::insertOrientation;
+
 void PathShifter::setPath(const PathWithLaneId & path)
 {
   reference_path_ = path;
-  is_index_aligned_ = false;  // shift_point index has to be updated for new path.
+  //
+  updateShiftPointIndices(shift_points_);
+  sortShiftPointsAlongPath(shift_points_);
 }
 void PathShifter::addShiftPoint(const ShiftPoint & point)
 {
   shift_points_.push_back(point);
-  is_index_aligned_ = false;  // shift_point index has to be updated for new shift points.
+
+  updateShiftPointIndices(shift_points_);
+  sortShiftPointsAlongPath(shift_points_);
 }
 
 void PathShifter::setShiftPoints(const std::vector<ShiftPoint> & points)
 {
   shift_points_ = points;
-  is_index_aligned_ = false;  // shift_point index has to be updated for new shift points.
+
+  updateShiftPointIndices(shift_points_);
+  sortShiftPointsAlongPath(shift_points_);
 }
 
 bool PathShifter::generate(
-  ShiftedPath * shifted_path, const bool offset_back, const SHIFT_TYPE type)
+  ShiftedPath * shifted_path, const bool offset_back, const SHIFT_TYPE type) const
 {
   RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "PathShifter::generate start!");
 
@@ -81,23 +91,20 @@ bool PathShifter::generate(
     return true;
   }
 
-  if (!is_index_aligned_) {
-    updateShiftPointIndices();
-    for (const auto & shift_point : shift_points_) {
-      int idx_gap = shift_point.end_idx - shift_point.start_idx;
-      if (idx_gap <= 1) {
-        RCLCPP_WARN_STREAM_THROTTLE(
-          logger_, clock_, 3000,
-          "shift start point and end point can't be adjoining "
-          "Maybe shift length is too short?");
-        return false;
-      }
+  for (const auto & shift_point : shift_points_) {
+    int idx_gap = shift_point.end_idx - shift_point.start_idx;
+    if (idx_gap <= 1) {
+      RCLCPP_WARN_STREAM_THROTTLE(
+        logger_, clock_, 3000,
+        "shift start point and end point can't be adjoining "
+        "Maybe shift length is too short?");
+      return false;
     }
   }
 
-  // Sort shift points since applyShift function only supports sorted points
-  if (!sortShiftPointsAlongPath(reference_path_)) {
-    RCLCPP_ERROR_STREAM_THROTTLE(logger_, clock_, 3000, "has duplicated points. Failed!");
+  // Check if the shift points are sorted correctly
+  if (!checkShiftPointsAlignment(shift_points_)) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to sort shift points..!!");
     return false;
   }
 
@@ -116,7 +123,7 @@ bool PathShifter::generate(
                              : applyLinearShifter(shifted_path);
 
   const bool is_driving_forward = true;
-  motion_utils::insertOrientation(shifted_path->path.points, is_driving_forward);
+  insertOrientation(shifted_path->path.points, is_driving_forward);
 
   // DEBUG
   RCLCPP_DEBUG_STREAM_THROTTLE(
@@ -126,7 +133,7 @@ bool PathShifter::generate(
   return true;
 }
 
-void PathShifter::applyLinearShifter(ShiftedPath * shifted_path)
+void PathShifter::applyLinearShifter(ShiftedPath * shifted_path) const
 {
   const auto arclength_arr = util::calcPathArcLengthArray(reference_path_);
 
@@ -160,7 +167,7 @@ void PathShifter::applyLinearShifter(ShiftedPath * shifted_path)
   }
 }
 
-void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offset_back)
+void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offset_back) const
 {
   const auto arclength_arr = util::calcPathArcLengthArray(reference_path_);
 
@@ -229,13 +236,13 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs
   }
 }
 
-std::vector<double> PathShifter::calcLateralJerk()
+std::vector<double> PathShifter::calcLateralJerk() const
 {
   const auto arclength_arr = util::calcPathArcLengthArray(reference_path_);
 
   constexpr double epsilon = 1.0e-8;  // to avoid 0 division
 
-  std::vector<double> lateral_jerk;
+  std::vector<double> lateral_jerk{};
 
   // TODO(Watanabe) write docs.
   double current_shift = base_offset_;
@@ -250,19 +257,16 @@ std::vector<double> PathShifter::calcLateralJerk()
   return lateral_jerk;
 }
 
-void PathShifter::updateShiftPointIndices()
+void PathShifter::updateShiftPointIndices(ShiftPointArray & shift_points) const
 {
-  for (auto & p : shift_points_) {
-    p.start_idx = motion_utils::findNearestIndex(
-      reference_path_.points,
-      p.start.position);  // TODO(murooka) remove findNearestIndex for except
-                          // lane_following to support u-turn & crossing path
-    p.end_idx = motion_utils::findNearestIndex(
-      reference_path_.points,
-      p.end.position);  // TODO(murooka) remove findNearestIndex for except
-                        // lane_following to support u-turn & crossing path
+  for (auto & p : shift_points) {
+    // TODO(murooka) remove findNearestIndex for except
+    // lane_following to support u-turn & crossing path
+    p.start_idx = findNearestIndex(reference_path_.points, p.start.position);
+    // TODO(murooka) remove findNearestIndex for except
+    // lane_following to support u-turn & crossing path
+    p.end_idx = findNearestIndex(reference_path_.points, p.end.position);
   }
-  is_index_aligned_ = true;
 }
 
 bool PathShifter::checkShiftPointsAlignment(const ShiftPointArray & shift_points) const
@@ -281,20 +285,20 @@ bool PathShifter::checkShiftPointsAlignment(const ShiftPointArray & shift_points
   return true;
 }
 
-bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId & path)
+void PathShifter::sortShiftPointsAlongPath(ShiftPointArray & shift_points) const
 {
-  if (shift_points_.empty()) {
-    RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "shift_points_ is empty. do nothing.");
-    return true;
+  if (shift_points.empty()) {
+    RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "shift_points is empty. do nothing.");
+    return;
   }
 
-  const auto & unsorted_shift_points = shift_points_;
+  const auto unsorted_shift_points = shift_points;
 
   // Calc indices sorted by "shift start point index" order
   std::vector<size_t> sorted_indices(unsorted_shift_points.size());
   std::iota(sorted_indices.begin(), sorted_indices.end(), 0);
-  std::sort(sorted_indices.begin(), sorted_indices.end(), [&](size_t i1, size_t i2) {
-    return unsorted_shift_points[i1].start_idx < unsorted_shift_points[i2].start_idx;
+  std::sort(sorted_indices.begin(), sorted_indices.end(), [&](size_t i, size_t j) {
+    return unsorted_shift_points.at(i).start_idx < unsorted_shift_points.at(j).start_idx;
   });
 
   // Set shift points and index by sorted_indices
@@ -303,14 +307,7 @@ bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId
     sorted_shift_points.push_back(unsorted_shift_points.at(sorted_i));
   }
 
-  // Check if the shift points are sorted correctly
-  if (!checkShiftPointsAlignment(sorted_shift_points)) {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to sort shift points..!!");
-    return false;
-  }
-
-  // set to member
-  setShiftPoints(sorted_shift_points);
+  shift_points = sorted_shift_points;
 
   // Debug
   for (const auto & p : unsorted_shift_points) {
@@ -324,12 +321,9 @@ bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId
       logger_, clock_, 3000, "sorted_shift_points: in order: " << toStr(p));
   }
   RCLCPP_DEBUG(logger_, "PathShifter::sortShiftPointsAlongPath end.");
-
-  return true;
 }
 
-void PathShifter::removeBehindShiftPointAndSetBaseOffset(
-  [[maybe_unused]] const Pose & pose, const size_t nearest_idx)
+void PathShifter::removeBehindShiftPointAndSetBaseOffset(const size_t nearest_idx)
 {
   // If shift_point.end is behind the ego_pose, remove the shift_point and
   // set its shift_length to the base_offset.