refactor(behavior_path_planner): change shift point to shift line (#2043

)

feat(behavior_path_planner): change shift point to shift line

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp

@@ -35,7 +35,7 @@ namespace marker_utils
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using behavior_path_planner::ShiftPointArray;
+using behavior_path_planner::ShiftLineArray;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
@@ -89,9 +89,9 @@ MarkerArray createPathMarkerArray(
   const PathWithLaneId & path, std::string && ns, const int64_t & lane_id, const float & r,
   const float & g, const float & b);
 
-MarkerArray createShiftPointMarkerArray(
-  const ShiftPointArray & shift_points, const double & base_shift, std::string && ns,
-  const float & r, const float & g, const float & b, const float & w);
+MarkerArray createShiftLineMarkerArray(
+  const ShiftLineArray & shift_lines, const double & base_shift, std::string && ns, const float & r,
+  const float & g, const float & b, const float & w);
 
 MarkerArray createShiftLengthMarkerArray(
   const std::vector<double> & shift_distance,

planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp

@@ -56,7 +56,7 @@ void clipPathLength(
 
 std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,
-  const ShiftPoint & shift_point, const Pose & pose, const double & velocity,
+  const ShiftLine & shift_line, const Pose & pose, const double & velocity,
   const BehaviorPathPlannerParameters & common_parameter);
 
 }  // namespace behavior_path_planner::util

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

@@ -68,13 +68,13 @@ class AvoidanceModule : public SceneModuleInterface
   }
 
 private:
-  struct RegisteredShiftPoint
+  struct RegisteredShiftLine
   {
     UUID uuid;
     Pose start_pose;
     Pose finish_pose;
   };
-  using RegisteredShiftPointArray = std::vector<RegisteredShiftPoint>;
+  using RegisteredShiftLineArray = std::vector<RegisteredShiftLine>;
 
   std::shared_ptr<AvoidanceParameters> parameters_;
 
@@ -85,8 +85,8 @@ class AvoidanceModule : public SceneModuleInterface
   RTCInterface rtc_interface_left_;
   RTCInterface rtc_interface_right_;
 
-  RegisteredShiftPointArray left_shift_array_;
-  RegisteredShiftPointArray right_shift_array_;
+  RegisteredShiftLineArray left_shift_array_;
+  RegisteredShiftLineArray right_shift_array_;
   UUID candidate_uuid_;
   UUID uuid_left_;
   UUID uuid_right_;
@@ -181,11 +181,11 @@ class AvoidanceModule : public SceneModuleInterface
   ShiftedPath prev_linear_shift_path_;  // used for shift point check
   PathWithLaneId prev_reference_;
 
-  // for raw_shift_point registration
-  AvoidPointArray registered_raw_shift_points_;
-  AvoidPointArray current_raw_shift_points_;
-  void registerRawShiftPoints(const AvoidPointArray & future_registered);
-  void updateRegisteredRawShiftPoints();
+  // for raw_shift_line registration
+  AvoidLineArray registered_raw_shift_lines_;
+  AvoidLineArray current_raw_shift_lines_;
+  void registerRawShiftLines(const AvoidLineArray & future_registered);
+  void updateRegisteredRawShiftLines();
 
   // -- for state management --
   bool isAvoidancePlanRunning() const;
@@ -202,55 +202,53 @@ class AvoidanceModule : public SceneModuleInterface
   void CompensateDetectionLost(ObjectDataArray & objects) const;
 
   // -- for shift point generation --
-  AvoidPointArray calcShiftPoints(
-    AvoidPointArray & current_raw_shift_points, DebugData & debug) const;
+  AvoidLineArray calcShiftLines(AvoidLineArray & current_raw_shift_lines, DebugData & debug) const;
 
   // shift point generation: generator
   double getShiftLength(
     const ObjectData & object, const bool & is_object_on_right, const double & avoid_margin) const;
-  AvoidPointArray calcRawShiftPointsFromObjects(const ObjectDataArray & objects) const;
+  AvoidLineArray calcRawShiftLinesFromObjects(const ObjectDataArray & objects) const;
   double getRightShiftBound() const;
   double getLeftShiftBound() const;
 
   // shift point generation: combiner
-  AvoidPointArray combineRawShiftPointsWithUniqueCheck(
-    const AvoidPointArray & base_points, const AvoidPointArray & added_points) const;
+  AvoidLineArray combineRawShiftLinesWithUniqueCheck(
+    const AvoidLineArray & base_points, const AvoidLineArray & added_points) const;
 
   // shift point generation: merger
-  AvoidPointArray mergeShiftPoints(
-    const AvoidPointArray & raw_shift_points, DebugData & debug) const;
+  AvoidLineArray mergeShiftLines(const AvoidLineArray & raw_shift_lines, DebugData & debug) const;
   void generateTotalShiftLine(
-    const AvoidPointArray & avoid_points, ShiftLineData & shift_line_data) const;
-  AvoidPointArray extractShiftPointsFromLine(ShiftLineData & shift_line_data) const;
+    const AvoidLineArray & avoid_points, ShiftLineData & shift_line_data) const;
+  AvoidLineArray extractShiftLinesFromLine(ShiftLineData & shift_line_data) const;
   std::vector<size_t> calcParentIds(
-    const AvoidPointArray & parent_candidates, const AvoidPoint & child) const;
+    const AvoidLineArray & parent_candidates, const AvoidLine & child) const;
 
   // shift point generation: trimmers
-  AvoidPointArray trimShiftPoint(const AvoidPointArray & shift_points, DebugData & debug) const;
-  void quantizeShiftPoint(AvoidPointArray & shift_points, const double interval) const;
-  void trimSmallShiftPoint(AvoidPointArray & shift_points, const double shift_diff_thres) const;
-  void trimSimilarGradShiftPoint(AvoidPointArray & shift_points, const double threshold) const;
-  void trimMomentaryReturn(AvoidPointArray & shift_points) const;
-  void trimTooSharpShift(AvoidPointArray & shift_points) const;
-  void trimSharpReturn(AvoidPointArray & shift_points) const;
+  AvoidLineArray trimShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const;
+  void quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const;
+  void trimSmallShiftLine(AvoidLineArray & shift_lines, const double shift_diff_thres) const;
+  void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const;
+  void trimMomentaryReturn(AvoidLineArray & shift_lines) const;
+  void trimTooSharpShift(AvoidLineArray & shift_lines) const;
+  void trimSharpReturn(AvoidLineArray & shift_lines) const;
 
   // shift point generation: return-shift generator
-  void addReturnShiftPointFromEgo(
-    AvoidPointArray & sp_candidates, AvoidPointArray & current_raw_shift_points) const;
+  void addReturnShiftLineFromEgo(
+    AvoidLineArray & sl_candidates, AvoidLineArray & current_raw_shift_lines) const;
 
   // -- for shift point operations --
-  void alignShiftPointsOrder(
-    AvoidPointArray & shift_points, const bool recalculate_start_length = true) const;
-  AvoidPointArray fillAdditionalInfo(const AvoidPointArray & shift_points) const;
-  AvoidPoint fillAdditionalInfo(const AvoidPoint & shift_point) const;
-  void fillAdditionalInfoFromPoint(AvoidPointArray & shift_points) const;
-  void fillAdditionalInfoFromLongitudinal(AvoidPointArray & shift_points) const;
+  void alignShiftLinesOrder(
+    AvoidLineArray & shift_lines, const bool recalculate_start_length = true) const;
+  AvoidLineArray fillAdditionalInfo(const AvoidLineArray & shift_lines) const;
+  AvoidLine fillAdditionalInfo(const AvoidLine & shift_line) const;
+  void fillAdditionalInfoFromPoint(AvoidLineArray & shift_lines) const;
+  void fillAdditionalInfoFromLongitudinal(AvoidLineArray & shift_lines) const;
 
   // -- for new shift point approval --
-  boost::optional<AvoidPointArray> findNewShiftPoint(
-    const AvoidPointArray & shift_points, const PathShifter & shifter) const;
-  void addShiftPointIfApproved(const AvoidPointArray & point);
-  void addNewShiftPoints(PathShifter & path_shifter, const AvoidPointArray & shift_points) const;
+  boost::optional<AvoidLineArray> findNewShiftLine(
+    const AvoidLineArray & shift_lines, const PathShifter & shifter) const;
+  void addShiftLineIfApproved(const AvoidLineArray & point);
+  void addNewShiftLines(PathShifter & path_shifter, const AvoidLineArray & shift_lines) const;
 
   // -- path generation --
   ShiftedPath generateAvoidancePath(PathShifter & shifter) const;
@@ -267,16 +265,16 @@ class AvoidanceModule : public SceneModuleInterface
   TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const;
 
   // intersection (old)
-  boost::optional<AvoidPoint> calcIntersectionShiftPoint(const AvoidancePlanningData & data) const;
+  boost::optional<AvoidLine> calcIntersectionShiftLine(const AvoidancePlanningData & data) const;
 
   bool isTargetObjectType(const PredictedObject & object) const;
 
   // debug
   mutable DebugData debug_data_;
   void setDebugData(const PathShifter & shifter, const DebugData & debug);
   void updateAvoidanceDebugData(std::vector<AvoidanceDebugMsg> & avoidance_debug_msg_array) const;
-  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_point_;
-  mutable rclcpp::Time debug_avoidance_initializer_for_shift_point_time_;
+  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_line_;
+  mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_;
   // =====================================
   // ========= helper functions ==========
   // =====================================
@@ -287,7 +285,7 @@ class AvoidanceModule : public SceneModuleInterface
   // TODO(Horibe): think later.
   // for unique ID
   mutable uint64_t original_unique_id = 0;  // TODO(Horibe) remove mutable
-  uint64_t getOriginalShiftPointUniqueId() const { return original_unique_id++; }
+  uint64_t getOriginalShiftLineUniqueId() const { return original_unique_id++; }
 
   double getNominalAvoidanceDistance(const double shift_length) const;
   double getNominalPrepareDistance() const;

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

@@ -201,7 +201,7 @@ using ObjectDataArray = std::vector<ObjectData>;
 /*
  * Shift point with additional info for avoidance planning
  */
-struct AvoidPoint : public ShiftPoint
+struct AvoidLine : public ShiftLine
 {
   // relative shift length from start to end point
   double start_length = 0.0;
@@ -221,13 +221,13 @@ struct AvoidPoint : public ShiftPoint
   // corresponding object
   ObjectData object{};
 
-  double getRelativeLength() const { return length - start_length; }
+  double getRelativeLength() const { return end_shift_length - start_length; }
 
   double getRelativeLongitudinal() const { return end_longitudinal - start_longitudinal; }
 
   double getGradient() const { return getRelativeLength() / getRelativeLongitudinal(); }
 };
-using AvoidPointArray = std::vector<AvoidPoint>;
+using AvoidLineArray = std::vector<AvoidLine>;
 
 /*
  * Common data for avoidance planning
@@ -287,20 +287,20 @@ struct DebugData
   std::shared_ptr<lanelet::ConstLanelets> current_lanelets;
   std::shared_ptr<lanelet::ConstLineStrings3d> farthest_linestring_from_overhang;
 
-  AvoidPointArray current_shift_points;  // in path shifter
-  AvoidPointArray new_shift_points;      // in path shifter
+  AvoidLineArray current_shift_lines;  // in path shifter
+  AvoidLineArray new_shift_lines;      // in path shifter
 
-  AvoidPointArray registered_raw_shift;
-  AvoidPointArray current_raw_shift;
-  AvoidPointArray extra_return_shift;
+  AvoidLineArray registered_raw_shift;
+  AvoidLineArray current_raw_shift;
+  AvoidLineArray extra_return_shift;
 
-  AvoidPointArray merged;
-  AvoidPointArray trim_similar_grad_shift;
-  AvoidPointArray quantized;
-  AvoidPointArray trim_small_shift;
-  AvoidPointArray trim_similar_grad_shift_second;
-  AvoidPointArray trim_momentary_return;
-  AvoidPointArray trim_too_sharp_shift;
+  AvoidLineArray merged;
+  AvoidLineArray trim_similar_grad_shift;
+  AvoidLineArray quantized;
+  AvoidLineArray trim_small_shift;
+  AvoidLineArray trim_similar_grad_shift_second;
+  AvoidLineArray trim_momentary_return;
+  AvoidLineArray trim_too_sharp_shift;
   std::vector<double> pos_shift;
   std::vector<double> neg_shift;
   std::vector<double> total_shift;

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

@@ -37,14 +37,14 @@ size_t findPathIndexFromArclength(
 
 ShiftedPath toShiftedPath(const PathWithLaneId & path);
 
-ShiftPointArray toShiftPointArray(const AvoidPointArray & avoid_points);
+ShiftLineArray toShiftLineArray(const AvoidLineArray & avoid_points);
 
 std::vector<size_t> concatParentIds(
   const std::vector<size_t> & ids1, const std::vector<size_t> & ids2);
 
-double lerpShiftLengthOnArc(double arc, const AvoidPoint & ap);
+double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
 
-void clipByMinStartIdx(const AvoidPointArray & shift_points, PathWithLaneId & path);
+void clipByMinStartIdx(const AvoidLineArray & shift_lines, PathWithLaneId & path);
 
 void fillLongitudinalAndLengthByClosestFootprint(
   const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos,
@@ -54,11 +54,11 @@ double calcOverhangDistance(
   const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose);
 
 void setEndData(
-  AvoidPoint & ap, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,
+  AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,
   const double end_dist);
 
 void setStartData(
-  AvoidPoint & ap, const double start_length, const geometry_msgs::msg::Pose & start,
+  AvoidLine & al, const double start_length, const geometry_msgs::msg::Pose & start,
   const size_t start_idx, const double start_dist);
 
 std::string getUuidStr(const ObjectData & obj);

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

@@ -36,15 +36,15 @@ namespace marker_utils::avoidance_marker
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using behavior_path_planner::AvoidPointArray;
-using behavior_path_planner::ShiftPointArray;
+using behavior_path_planner::AvoidLineArray;
+using behavior_path_planner::ShiftLineArray;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
 using visualization_msgs::msg::MarkerArray;
 
-MarkerArray createAvoidPointMarkerArray(
-  const AvoidPointArray & shift_points, std::string && ns, const float & r, const float & g,
+MarkerArray createAvoidLineMarkerArray(
+  const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g,
   const float & b, const double & w);
 
 MarkerArray createAvoidanceObjectsMarkerArray(
@@ -59,12 +59,12 @@ MarkerArray createOverhangFurthestLineStringMarkerArray(
 
 }  // namespace marker_utils::avoidance_marker
 
-std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr);
+std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sp_arr);
 
-std::string toStrInfo(const behavior_path_planner::AvoidPointArray & ap_arr);
+std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr);
 
-std::string toStrInfo(const behavior_path_planner::ShiftPoint & sp);
+std::string toStrInfo(const behavior_path_planner::ShiftLine & sp);
 
-std::string toStrInfo(const behavior_path_planner::AvoidPoint & ap);
+std::string toStrInfo(const behavior_path_planner::AvoidLine & ap);
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__DEBUG_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp

@@ -30,7 +30,7 @@ struct LaneChangePath
 {
   PathWithLaneId path;
   ShiftedPath shifted_path;
-  ShiftPoint shift_point;
+  ShiftLine shift_line;
   double acceleration{0.0};
   double preparation_length{0.0};
   double lane_change_length{0.0};

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -59,7 +59,7 @@ double getDistanceWhenDecelerate(
 
 std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
-  const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point,
+  const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
   const double & acceleration, const double & prepare_distance, const double & prepare_duration,
   const double & prepare_speed, const double & minimum_prepare_distance,
@@ -103,7 +103,7 @@ bool hasEnoughDistance(
   const bool isInGoalRouteSection, const Pose & goal_pose,
   const lanelet::routing::RoutingGraphContainer & overall_graphs);
 
-ShiftPoint getLaneChangeShiftPoint(
+ShiftLine getLaneChangeShiftLine(
   const PathWithLaneId & path1, const PathWithLaneId & path2,
   const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path);
 
@@ -130,7 +130,7 @@ PathWithLaneId getLaneChangePathLaneChangingSegment(
 
 TurnSignalInfo calc_turn_signal_info(
   const PathWithLaneId & prepare_path, const double prepare_velocity,
-  const double min_prepare_distance, const double prepare_duration, const ShiftPoint & shift_points,
+  const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line,
   const ShiftedPath & lane_changing_path);
 
 void get_turn_signal_info(

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp

@@ -32,7 +32,7 @@ struct PullOverPath
   Pose start_pose{};
   Pose end_pose{};
   ShiftedPath shifted_path{};
-  ShiftPoint shift_point{};
+  ShiftLine shift_line{};
   double acceleration{0.0};
   double preparation_length{0.0};
   double pull_over_length{0.0};

planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp

@@ -77,9 +77,9 @@ class SideShiftModule : public SceneModuleInterface
   // non-const methods
   void adjustDrivableArea(ShiftedPath * path) const;
 
-  ShiftPoint calcShiftPoint() const;
+  ShiftLine calcShiftLine() const;
 
-  bool addShiftPoint();
+  bool addShiftLine();
 
   // const methods
   void publishPath(const PathWithLaneId & path) const;
@@ -104,7 +104,7 @@ class SideShiftModule : public SceneModuleInterface
   PathShifter path_shifter_;
 
   ShiftedPath prev_output_;
-  ShiftPoint prev_shift_point_;
+  ShiftLine prev_shift_line_;
 
   // NOTE: this function is ported from avoidance.
   PoseStamped getUnshiftedEgoPose(const ShiftedPath & prev_path) const;