Merge pull request autowarefoundation#570 from tier4/sync-upstream

chore: sync upstream

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -68,7 +68,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
       std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, true);
   }
 
-  if (enable_partial_load || enable_differential_load) {
+  if (enable_partial_load || enable_differential_load || enable_selected_load) {
     std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
     try {
       pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths);

perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -28,7 +28,7 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio
   fusion_params_.passthrough_lower_bound_probability_threshold =
     declare_parameter<double>("passthrough_lower_bound_probability_threshold");
   fusion_params_.use_roi_probability = declare_parameter<bool>("use_roi_probability");
-  fusion_params_.roi_probability_threshold = declare_parameter<bool>("roi_probability_threshold");
+  fusion_params_.roi_probability_threshold = declare_parameter<double>("roi_probability_threshold");
   fusion_params_.min_iou_threshold = declare_parameter<double>("min_iou_threshold");
 }
 

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

@@ -57,8 +57,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   ModuleStatus updateState() override;
   ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; }
-  BehaviorModuleOutput plan() override;
   CandidateOutput planCandidate() const override;
+  BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
@@ -322,20 +322,6 @@ class AvoidanceModule : public SceneModuleInterface
 
   // shift line generation
 
-  /**
-   * @brief fill index and longitudinal.
-   * @param target shift line.
-   * @return processed shift line.
-   */
-  AvoidLine fillAdditionalInfo(const AvoidLine & shift_line) const;
-
-  /**
-   * @brief fill index and longitudinal.
-   * @param target shift lines.
-   * @return processed shift lines.
-   */
-  AvoidLineArray fillAdditionalInfo(const AvoidLineArray & shift_lines) const;
-
   /**
    * @brief Calculate the shift points (start/end point, shift length) from the object lateral
    * and longitudinal positions in the Frenet coordinate. The jerk limit is also considered here.
@@ -360,16 +346,6 @@ class AvoidanceModule : public SceneModuleInterface
   AvoidLineArray applyPreProcessToRawShiftLines(
     AvoidLineArray & current_raw_shift_points, DebugData & debug) const;
 
-  /*
-   * @brief Combine points A into B. If shift_line of A which has same object_id and
-   * similar shape is already in B, it will not be added into B.
-   * @param original shift lines.
-   * @param new shift lines.
-   * @return processed shift lines.
-   */
-  AvoidLineArray combineRawShiftLinesWithUniqueCheck(
-    const AvoidLineArray & base_lines, const AvoidLineArray & added_lines) const;
-
   /*
    * @brief fill gap between two shift lines.
    * @param original shift lines.
@@ -413,15 +389,6 @@ class AvoidanceModule : public SceneModuleInterface
    */
   AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines) const;
 
-  /*
-   * @brief calculate parent ids.
-   * @param parent shift lines.
-   * @param child shift lines.
-   * @return parent ids.
-   */
-  std::vector<size_t> calcParentIds(
-    const AvoidLineArray & parent_candidates, const AvoidLine & child) const;
-
   /*
    * @brief add return shift line from ego position.
    * @param shift lines which the return shift is added.
@@ -490,26 +457,6 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const;
 
-  /*
-   * @brief sort shift line order based on their end longitudinal distance.
-   * @param target shift lines.
-   * @param re-calculate shift line start length from previous one's. (optional)
-   */
-  void alignShiftLinesOrder(
-    AvoidLineArray & shift_lines, const bool recalculate_start_length = true) const;
-
-  /**
-   * @brief fill index and longitudinal.
-   * @param target shift line.
-   */
-  void fillAdditionalInfoFromPoint(AvoidLineArray & shift_lines) const;
-
-  /**
-   * @brief fill index and pose.
-   * @param target shift line.
-   */
-  void fillAdditionalInfoFromLongitudinal(AvoidLineArray & shift_lines) const;
-
   /**
    * @brief add new shift line to path shifter if the RTC status is activated.
    * @param new shift lines.

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

@@ -156,6 +156,26 @@ class AvoidanceHelper
                        : std::max(shift_length, getRightShiftBound());
   }
 
+  void alignShiftLinesOrder(AvoidLineArray & lines, const bool align_shift_length = true) const
+  {
+    if (lines.empty()) {
+      return;
+    }
+
+    std::sort(lines.begin(), lines.end(), [](auto a, auto b) {
+      return a.end_longitudinal < b.end_longitudinal;
+    });
+
+    if (!align_shift_length) {
+      return;
+    }
+
+    lines.front().start_shift_length = getEgoLinearShift();
+    for (size_t i = 1; i < lines.size(); ++i) {
+      lines.at(i).start_shift_length = lines.at(i - 1).end_shift_length;
+    }
+  }
+
   AvoidLine getMainShiftLine(const AvoidLineArray & lines) const
   {
     const auto itr =

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

@@ -48,6 +48,8 @@ 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<size_t> calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2);
+
 double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
 
 void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
@@ -110,6 +112,15 @@ double extendToRoadShoulderDistanceWithPolygon(
   const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance,
   const geometry_msgs::msg::Point & overhang_pos,
   const lanelet::BasicPoint3d & overhang_basic_pose);
+
+void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines);
+
+void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLineArray & lines);
+
+AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine & line);
+
+AvoidLineArray combineRawShiftLinesWithUniqueCheck(
+  const AvoidLineArray & base_lines, const AvoidLineArray & added_lines);
 }  // namespace behavior_path_planner::utils::avoidance
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_