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

autowarefoundation · Nov 20, 2023 · 0f31d68 · 0f31d68
1 parent 098cc22
commit 0f31d68
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 13 deletions.
diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -495,8 +495,8 @@ struct AvoidancePlanningData
   // nearest object that should be avoid
   boost::optional<ObjectData> stop_target_object{boost::none};
 
-  // raw shift point
-  AvoidLineArray raw_shift_line{};
+  // // raw shift point
+  // AvoidLineArray raw_shift_line{};
 
   // new shift point
   AvoidLineArray new_shift_line{};

diff --git a/...avior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp b/...avior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp
@@ -218,7 +218,7 @@ class ShiftLineGenerator
    * @brief update path index of the registered objects. remove old objects whose end point is
    * behind ego pose.
    */
-  void updateRegisteredRawShiftLines();
+  void updateRegisteredRawShiftLines(const AvoidancePlanningData & data);
 
   // misc functions
 

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -442,7 +442,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
 
   // generator_.generate(data, debug);
 
-  data.raw_shift_line = generator_.getRawShiftLine();
+  //   data.raw_shift_line = generator_.getRawShiftLine();
 
   const auto processed_shift_lines = generator_.getCandidateShiftLine();
 
@@ -1520,7 +1520,8 @@ void AvoidanceModule::updateDebugMarker(
   // registering process
   {
     addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3);
-    addAvoidLine(data.raw_shift_line, "step4_raw_shift_line", 1.0, 0.0, 0.0, 0.3);
+    addAvoidLine(generator_.getRawShiftLine(), "step4_raw_shift_line", 1.0, 0.0, 0.0, 0.3);
+    // addAvoidLine(data.raw_shift_line, "step4_raw_shift_line", 1.0, 0.0, 0.0, 0.3);
     addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3);
   }
 

diff --git a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp b/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp
@@ -104,7 +104,7 @@ void ShiftLineGenerator::update(AvoidancePlanningData & data, DebugData & debug)
    * STEP1: Generate avoid outlines.
    * Basically, avoid outlines are generated per target objects.
    */
-  updateRegisteredRawShiftLines();
+  updateRegisteredRawShiftLines(data);
 
   /**
    * STEP1: Generate avoid outlines.
@@ -302,8 +302,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       const auto offset =
         object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
       const auto to_shift_end = o.longitudinal - offset;
-      const auto path_front_to_ego =
-        avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index);
+      const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index);
 
       // start point (use previous linear shift length as start shift length.)
       al_avoid.start_longitudinal = [&]() {
@@ -322,8 +321,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       }();
 
       al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength(
-        avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego);
-      al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose;
+        data.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego);
+      al_avoid.start = data.reference_path.points.at(al_avoid.start_idx).point.pose;
       al_avoid.start_shift_length = helper_->getLinearShift(al_avoid.start.position);
 
       // end point
@@ -1257,10 +1256,8 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine(
   return {};
 }
 
-void ShiftLineGenerator::updateRegisteredRawShiftLines()
+void ShiftLineGenerator::updateRegisteredRawShiftLines(const AvoidancePlanningData & data)
 {
-  const auto & data = avoid_data_;
-
   utils::avoidance::fillAdditionalInfoFromPoint(data, raw_registered_);
 
   AvoidLineArray avoid_lines;
@@ -1298,6 +1295,8 @@ void ShiftLineGenerator::updateRegisteredRawShiftLines()
     avoid_lines.push_back(s);
   }
 
+  raw_registered_ = avoid_lines;
+
   // registered_raw_shift_lines_ = avoid_lines;
   // debug_data_.step1_registered_shift_line = registered_raw_shift_lines_;
 }