[pull] main from autowarefoundation:main

map/map_loader/README.md

@@ -14,6 +14,66 @@ Currently, it supports the following two types:
 - Send partial pointcloud map loading via ROS 2 service
 - Send differential pointcloud map loading via ROS 2 service
 
+### Prerequisites
+
+#### Prerequisites on pointcloud map file(s)
+
+You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules:
+
+1. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines.
+2. **The division size along each axis should be equal.**
+3. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation).
+4. **All the split maps should not overlap with each other.**
+5. **Metadata file should also be provided.** The metadata structure description is provided below.
+
+Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023).
+
+#### Metadata structure
+
+The metadata should look like this:
+
+```yaml
+x_resolution: 20.0
+y_resolution: 20.0
+A.pcd: [1200, 2500] # -> 1200 < x < 1220, 2500 < y < 2520
+B.pcd: [1220, 2500] # -> 1220 < x < 1240, 2500 < y < 2520
+C.pcd: [1200, 2520] # -> 1200 < x < 1220, 2520 < y < 2540
+D.pcd: [1240, 2520] # -> 1240 < x < 1260, 2520 < y < 2540
+```
+
+where,
+
+- `x_resolution` and `y_resolution`
+- `A.pcd`, `B.pcd`, etc, are the names of PCD files.
+- List such as `[1200, 2500]` are the values indicate that for this PCD file, x coordinates are between 1200 and 1220 (`x_resolution` + `x_coordinate`) and y coordinates are between 2500 and 2520 (`y_resolution` + `y_coordinate`).
+
+You may use [pointcloud_divider](https://github.com/MapIV/pointcloud_divider) from MAP IV for dividing pointcloud map as well as generating the compatible metadata.yaml.
+
+#### Directory structure of these files
+
+If you only have one pointcloud map, Autoware will assume the following directory structure by default.
+
+```bash
+sample-map-rosbag
+├── lanelet2_map.osm
+├── pointcloud_map.pcd
+```
+
+If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files.
+
+```bash
+sample-map-rosbag
+├── lanelet2_map.osm
+├── pointcloud_map.pcd
+│ ├── A.pcd
+│ ├── B.pcd
+│ ├── C.pcd
+│ └── ...
+└── pointcloud_map_metadata.yaml
+```
+
+### Specific features
+
 #### Publish raw pointcloud map (ROS 2 topic)
 
 The node publishes the raw pointcloud map loaded from the `.pcd` file(s).
@@ -71,46 +131,6 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co
 - pointcloud map file(s) (.pcd)
 - metadata of pointcloud map(s) (.yaml)
 
-### Metadata
-
-You must provide metadata in YAML format as well as pointcloud map files. Pointcloud map should be divided into one or more files with x-y grid.
-
-Metadata should look like this:
-
-```yaml
-x_resolution: 100.0
-y_resolution: 150.0
-A.pcd: [1200, 2500] # -> 1200 < x < 1300, 2500 < y < 2650
-B.pcd: [1300, 2500] # -> 1300 < x < 1400, 2500 < y < 2650
-C.pcd: [1200, 2650] # -> 1200 < x < 1300, 2650 < y < 2800
-D.pcd: [1400, 2650] # -> 1400 < x < 1500, 2650 < y < 2800
-```
-
-You may use [pointcloud_divider](https://github.com/MapIV/pointcloud_divider) from MAP IV for dividing pointcloud map as well as generating the compatible metadata.yaml.
-
-### How to store map-related files
-
-If you only have one pointcloud map, Autoware will assume the following directory structure by default.
-
-```bash
-sample-map-rosbag
-├── lanelet2_map.osm
-├── pointcloud_map.pcd
-```
-
-If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files.
-
-```bash
-sample-map-rosbag
-├── lanelet2_map.osm
-├── pointcloud_map.pcd
-│ ├── A.pcd
-│ ├── B.pcd
-│ ├── C.pcd
-│ └── ...
-└── pointcloud_map_metadata.yaml
-```
-
 ---
 
 ## lanelet2_map_loader

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_