fixup! refactor(avoidance): separate shift line generate process

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -394,15 +394,6 @@ class AvoidanceModule : public SceneModuleInterface
     path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx);
   }
 
-  // misc functions
-
-  double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); }
-
-  // TODO(Horibe): think later.
-  // for unique ID
-  mutable uint64_t original_unique_id = 0;  // TODO(Horibe) remove mutable
-  uint64_t getOriginalShiftLineUniqueId() const { return original_unique_id++; }
-
   struct RegisteredShiftLine
   {
     UUID uuid;

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -414,10 +414,10 @@ struct AvoidLine : public ShiftLine
   double end_longitudinal = 0.0;
 
   // for unique_id
-  uint64_t id = 0;
+  UUID id{};
 
   // for the case the point is created by merge other points
-  std::vector<uint64_t> parent_ids{};
+  std::vector<UUID> parent_ids{};
 
   // corresponding object
   ObjectData object{};

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp

@@ -22,12 +22,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-// #include <algorithm>
 #include <memory>
-// #include <string>
-// #include <unordered_map>
-// #include <utility>
-// #include <vector>
 
 namespace behavior_path_planner::utils::avoidance
 {
@@ -47,8 +42,6 @@ class ShiftLineGenerator
 
   void setData(const std::shared_ptr<const PlannerData> & data) { data_ = data; }
 
-  void setAvoidData(const AvoidancePlanningData & avoid_data) { avoid_data_ = avoid_data; }
-
   void setHelper(const std::shared_ptr<AvoidanceHelper> & helper) { helper_ = helper; }
 
   void setPathShifter(const PathShifter & path_shifter) { path_shifter_ = path_shifter; }
@@ -57,21 +50,19 @@ class ShiftLineGenerator
 
   void update(AvoidancePlanningData & data, DebugData & debug);
 
-  void generate(DebugData & debug);
-
   void reset()
   {
     outline_.clear();
     raw_.clear();
     raw_registered_.clear();
   }
 
+  AvoidLineArray generate(DebugData & debug) const;
+
   AvoidLineArray getRawShiftLine() const { return raw_; }
 
   AvoidLineArray getRawRegisteredShiftLine() const { return raw_registered_; }
 
-  AvoidLineArray getCandidateShiftLine() const { return candidate_; }
-
 private:
   /**
    * @brief Calculate the shift points (start/end point, shift length) from the object lateral
@@ -220,15 +211,8 @@ class ShiftLineGenerator
    */
   void updateRegisteredRawShiftLines(const AvoidancePlanningData & data);
 
-  // misc functions
-
   double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); }
 
-  // TODO(Horibe): think later.
-  // for unique ID
-  mutable uint64_t original_unique_id = 0;  // TODO(Horibe) remove mutable
-  uint64_t getOriginalShiftLineUniqueId() const { return original_unique_id++; }
-
   std::shared_ptr<const PlannerData> data_;
 
   std::shared_ptr<AvoidanceParameters> parameters_;
@@ -244,10 +228,6 @@ class ShiftLineGenerator
   AvoidLineArray raw_;
 
   AvoidLineArray raw_registered_;
-
-  // AvoidLineArray raw_current_;
-
-  AvoidLineArray candidate_;
 };
 
 }  // namespace behavior_path_planner::utils::avoidance

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -59,10 +59,9 @@ ShiftedPath toShiftedPath(const PathWithLaneId & path);
 
 ShiftLineArray toShiftLineArray(const AvoidLineArray & avoid_points);
 
-std::vector<size_t> concatParentIds(
-  const std::vector<size_t> & ids1, const std::vector<size_t> & ids2);
+std::vector<UUID> concatParentIds(const std::vector<UUID> & ids1, const std::vector<UUID> & ids2);
 
-std::vector<size_t> calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2);
+std::vector<UUID> calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2);
 
 double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
 

planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp

@@ -17,6 +17,7 @@
 #include "behavior_path_planner/utils/utils.hpp"
 
 #include <magic_enum.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <lanelet2_core/primitives/LineString.h>
 #include <tf2/utils.h>
@@ -509,14 +510,16 @@ std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr)
 }
 std::string toStrInfo(const behavior_path_planner::AvoidLine & ap)
 {
+  using tier4_autoware_utils::toHexString;
+
   std::stringstream pids;
   for (const auto pid : ap.parent_ids) {
-    pids << pid << ", ";
+    pids << toHexString(pid) << ", ";
   }
   const auto & ps = ap.start.position;
   const auto & pe = ap.end.position;
   std::stringstream ss;
-  ss << "id = " << ap.id << ", shift length: " << ap.end_shift_length
+  ss << "id = " << toHexString(ap.id) << ", shift length: " << ap.end_shift_length
      << ", start_idx: " << ap.start_idx << ", end_idx: " << ap.end_idx
      << ", start_dist = " << ap.start_longitudinal << ", end_dist = " << ap.end_longitudinal
      << ", start_shift_length: " << ap.start_shift_length << ", start: (" << ps.x << ", " << ps.y

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

@@ -440,31 +440,14 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
 {
   auto path_shifter = path_shifter_;
 
-  // generator_.generate(data, debug);
-
-  //   data.raw_shift_line = generator_.getRawShiftLine();
-
-  const auto processed_shift_lines = generator_.getCandidateShiftLine();
-
-  // /**
-  //  * STEP1: Generate avoid outlines.
-  //  * Basically, avoid outlines are generated per target objects.
-  //  */
-  // const auto outlines = generateAvoidOutline(data, debug);
-
-  // /**
-  //  * STEP2: Create rough shift lines.
-  //  */
-  // data.raw_shift_line = applyPreProcess(outlines, debug);
-
-  // /**
-  //  * STEP3: Create candidate shift lines.
-  //  * Merge rough shift lines, and extract new shift lines.
-  //  */
-  // const auto processed_shift_lines = generateCandidateShiftLine(data.raw_shift_line, debug);
+  /**
+   * STEP1: Create candidate shift lines.
+   * Merge rough shift lines, and extract new shift lines.
+   */
+  const auto processed_shift_lines = generator_.generate(debug);
 
   /**
-   * Step4: Validate new shift lines.
+   * Step2: Validate new shift lines.
    * Output new shift lines only when the avoidance path which is generated from them doesn't have
    * huge offset from ego.
    */
@@ -475,7 +458,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
   data.found_avoidance_path = found_new_sl || registered;
 
   /**
-   * STEP5: Set new shift lines.
+   * STEP3: Set new shift lines.
    * If there are new shift points, these shift points are registered in path_shifter in order to
    * generate candidate avoidance path.
    */
@@ -484,7 +467,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
   }
 
   /**
-   * STEP6: Generate avoidance path.
+   * STEP4: Generate avoidance path.
    */
   ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path);
   const auto success_spline_path_generation =
@@ -494,7 +477,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
                           : utils::avoidance::toShiftedPath(data.reference_path);
 
   /**
-   * STEP7: Check avoidance path safety.
+   * STEP5: Check avoidance path safety.
    * For each target objects and the objects in adjacent lanes,
    * check that there is a certain amount of margin in the lateral and longitudinal direction.
    */
@@ -1029,13 +1012,8 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines)
 
   addNewShiftLines(path_shifter_, shift_lines);
 
-  // generator_.setRawShiftLine(avoid_data_.raw_shift_line);
   generator_.setRawRegisteredShiftLine(shift_lines);
 
-  // current_raw_shift_lines_ = avoid_data_.raw_shift_line;
-
-  // registerRawShiftLines(shift_lines);
-
   const auto sl = helper_.getMainShiftLine(shift_lines);
   const auto sl_front = shift_lines.front();
   const auto sl_back = shift_lines.back();
@@ -1205,15 +1183,11 @@ void AvoidanceModule::updateData()
     return;
   }
 
-  // // update registered shift point for new reference path & remove past objects
-  // updateRegisteredRawShiftLines();
-
+  // update shift line generator.
   generator_.setData(planner_data_);
   generator_.setPathShifter(path_shifter_);
   generator_.setHelper(std::make_shared<helper::avoidance::AvoidanceHelper>(helper_));
   generator_.update(avoid_data_, debug_data_);
-  generator_.setAvoidData(avoid_data_);
-  generator_.generate(debug_data_);
 
   // update shift line and check path safety.
   fillShiftLine(avoid_data_, debug_data_);
@@ -1251,7 +1225,6 @@ void AvoidanceModule::initVariables()
   debug_marker_.markers.clear();
   resetPathCandidate();
   resetPathReference();
-  original_unique_id = 0;
   is_avoidance_maneuver_starts = false;
   arrived_path_end_ = false;
 }