Merge branch 'main' into chore/ndt_scan_matcher/rename_config_path

Author: YamatoAndo

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -15,6 +15,7 @@
   <arg name="launch_external_request_lane_change_left_module" default="true"/>
   <arg name="launch_goal_planner_module" default="true"/>
   <arg name="launch_start_planner_module" default="true"/>
+  <arg name="launch_sampling_planner_module" default="true"/>
   <arg name="launch_side_shift_module" default="true"/>
 
   <arg name="launch_crosswalk_module" default="true"/>
@@ -58,6 +59,11 @@
     value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::StartPlannerModuleManager, '&quot;)"
     if="$(var launch_start_planner_module)"
   />
+  <let
+    name="behavior_path_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::SamplingPlannerModuleManager, '&quot;)"
+    if="$(var launch_sampling_planner_module)"
+  />
   <let
     name="behavior_path_planner_launch_modules"
     value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::GoalPlannerModuleManager, '&quot;)"
@@ -204,6 +210,7 @@
       <param from="$(var behavior_path_planner_lane_change_module_param_path)"/>
       <param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
       <param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
+      <param from="$(var behavior_path_planner_sampling_planner_module_param_path)"/>
       <param from="$(var behavior_path_planner_drivable_area_expansion_param_path)"/>
       <param from="$(var behavior_path_planner_scene_module_manager_param_path)"/>
       <param from="$(var behavior_path_planner_common_param_path)"/>

map/map_height_fitter/CMakeLists.txt

@@ -14,4 +14,7 @@ ament_auto_add_executable(node
   src/node.cpp
 )
 
-ament_auto_package(INSTALL_TO_SHARE launch)
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config)

map/map_height_fitter/README.md

@@ -4,8 +4,18 @@ This library fits the given point with the ground of the point cloud map.
 The map loading operation is switched by the parameter `enable_partial_load` of the node specified by `map_loader_name`.
 The node using this library must use multi thread executor.
 
-| Interface    | Local Name         | Description                              |
-| ------------ | ------------------ | ---------------------------------------- |
-| Parameter    | map_loader_name    | The point cloud map loader name.         |
-| Subscription | ~/pointcloud_map   | The topic name to load the whole map     |
-| Client       | ~/partial_map_load | The service name to load the partial map |
+## Parameters
+
+{{ json_to_markdown("map/map_height_fitter/schema/map_height_fitter.schema.json") }}
+
+## Topic subscription
+
+| Topic Name       | Description                                                                                  |
+| ---------------- | -------------------------------------------------------------------------------------------- |
+| ~/pointcloud_map | The topic containing the whole pointcloud map (only used when `enable_partial_load = false`) |
+
+## Service client
+
+| Service Name       | Description                         |
+| ------------------ | ----------------------------------- |
+| ~/partial_map_load | The service to load the partial map |

map/map_height_fitter/config/map_height_fitter.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    map_loader_name: "/map/pointcloud_map_loader"

map/map_height_fitter/launch/map_height_fitter.launch.xml

@@ -1,8 +1,10 @@
 <launch>
+  <arg name="param_path" default="$(find-pkg-share map_height_fitter)/config/map_height_fitter.param.yaml"/>
+
   <group>
     <push-ros-namespace namespace="map"/>
     <node pkg="map_height_fitter" exec="node" name="map_height_fitter">
-      <param name="map_loader_name" value="/map/pointcloud_map_loader"/>
+      <param from="$(var param_path)"/>
       <remap from="~/pointcloud_map" to="/map/pointcloud_map"/>
       <remap from="~/partial_map_load" to="/map/get_partial_pointcloud_map"/>
     </node>

map/map_height_fitter/schema/map_height_fitter.schema.json

@@ -0,0 +1,33 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for map_height_fitter",
+  "type": "object",
+  "definitions": {
+    "map_height_fitter": {
+      "type": "object",
+      "properties": {
+        "map_loader_name": {
+          "type": "string",
+          "description": "Node name of the map loader from which this map_height_fitter will retrieve its parameters",
+          "default": "/map/pointcloud_map_loader"
+        }
+      },
+      "required": ["map_loader_name"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/map_height_fitter"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -37,6 +37,7 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 
+#include <atomic>
 #include <deque>
 #include <limits>
 #include <memory>
@@ -290,6 +291,35 @@ class GoalPlannerModule : public SceneModuleInterface
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map);
 
+  ~GoalPlannerModule()
+  {
+    if (lane_parking_timer_) {
+      lane_parking_timer_->cancel();
+    }
+    if (freespace_parking_timer_) {
+      freespace_parking_timer_->cancel();
+    }
+
+    while (is_lane_parking_cb_running_.load() || is_freespace_parking_cb_running_.load()) {
+      const std::string running_callbacks = std::invoke([&]() {
+        if (is_lane_parking_cb_running_ && is_freespace_parking_cb_running_) {
+          return "lane parking and freespace parking";
+        }
+        if (is_lane_parking_cb_running_) {
+          return "lane parking";
+        }
+        return "freespace parking";
+      });
+      RCLCPP_INFO_THROTTLE(
+        getLogger(), *clock_, 1000, "Waiting for %s callback to finish...",
+        running_callbacks.c_str());
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    RCLCPP_INFO_THROTTLE(
+      getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished");
+  }
+
   void updateModuleParams(const std::any & parameters) override
   {
     parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
@@ -330,6 +360,18 @@ class GoalPlannerModule : public SceneModuleInterface
   CandidateOutput planCandidate() const override { return CandidateOutput{}; }
 
 private:
+  // Flag class for managing whether a certain callback is running in multi-threading
+  class ScopedFlag
+  {
+  public:
+    explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }
+
+    ~ScopedFlag() { flag_.store(false); }
+
+  private:
+    std::atomic<bool> & flag_;
+  };
+
   /*
    * state transitions and plan function used in each state
    *
@@ -403,10 +445,12 @@ class GoalPlannerModule : public SceneModuleInterface
   // pre-generate lane parking paths in a separate thread
   rclcpp::TimerBase::SharedPtr lane_parking_timer_;
   rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
+  std::atomic<bool> is_lane_parking_cb_running_;
 
   // generate freespace parking paths in a separate thread
   rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
   rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
+  std::atomic<bool> is_freespace_parking_cb_running_;
 
   // debug
   mutable GoalPlannerDebugData debug_data_;

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -62,6 +62,8 @@ GoalPlannerModule::GoalPlannerModule(
   parameters_{parameters},
   vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
   thread_safe_data_{mutex_, clock_},
+  is_lane_parking_cb_running_{false},
+  is_freespace_parking_cb_running_{false},
   debug_stop_pose_with_info_{&stop_pose_}
 {
   LaneDepartureChecker lane_departure_checker{};
@@ -171,6 +173,8 @@ bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const
 // generate pull over candidate paths
 void GoalPlannerModule::onTimer()
 {
+  const ScopedFlag flag(is_lane_parking_cb_running_);
+
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }
@@ -287,6 +291,8 @@ void GoalPlannerModule::onTimer()
 
 void GoalPlannerModule::onFreespaceParkingTimer()
 {
+  const ScopedFlag flag(is_freespace_parking_cb_running_);
+
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }

planning/behavior_path_planner/config/scene_module_manager.param.yaml

@@ -82,3 +82,12 @@
       keep_last: false
       priority: 7
       max_module_size: 1
+
+    sampling_planner:
+      enable_module: true
+      enable_rtc: false
+      enable_simultaneous_execution_as_approved_module: false
+      enable_simultaneous_execution_as_candidate_module: false
+      keep_last: true
+      priority: 16
+      max_module_size: 1

planning/behavior_path_planner/launch/behavior_path_planner.launch.xml

@@ -19,6 +19,7 @@
   <arg name="behavior_path_planner_lane_change_module_param_path"/>
   <arg name="behavior_path_planner_goal_planner_module_param_path"/>
   <arg name="behavior_path_planner_start_planner_module_param_path"/>
+  <arg name="behavior_path_planner_sampling_planner_module_param_path"/>
   <arg name="behavior_path_planner_drivable_area_expansion_param_path"/>
 
   <node pkg="behavior_path_planner" exec="behavior_path_planner" name="behavior_path_planner" output="screen">
@@ -49,6 +50,7 @@
     <param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
     <param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
     <param from="$(var behavior_path_planner_dynamic_avoidance_module_param_path)"/>
+    <param from="$(var behavior_path_planner_sampling_planner_module_param_path)"/>
     <param from="$(var behavior_path_planner_lane_change_module_param_path)"/>
     <param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
     <param from="$(var behavior_path_planner_start_planner_module_param_path)"/>

planning/behavior_path_planner/package.xml

@@ -43,6 +43,7 @@
   <depend>autoware_perception_msgs</depend>
   <depend>behavior_path_planner_common</depend>
   <depend>freespace_planning_algorithms</depend>
+  <depend>frenet_planner</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>lane_departure_checker</depend>
@@ -52,6 +53,7 @@
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>object_recognition_utils</depend>
+  <depend>path_sampler</depend>
   <depend>planning_test_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

planning/behavior_path_sampling_planner_module/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.14)
+project(behavior_path_sampling_planner_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/sampling_planner_module.cpp
+  src/manager.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_path_sampling_planner_module/README.md

@@ -0,0 +1,78 @@
+# Behavior Path Sampling Based Planner
+
+WARNING: This module is experimental and has not been properly tested on a real vehicle, use only on simulations.
+
+## Purpose
+
+This package implements a node that uses sampling based planning to generate a drivable trajectory for the behavior path planner. It is heavily based off the [sampling_based_planner module](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/sampling_based_planner).
+
+## Features
+
+This package is able to:
+
+- create a smooth trajectory to avoid static obstacles.
+- guarantees the generated trajectory (if any) complies with customizable hard constraints.
+- transitions to a success state after the ego vehicle merges to its goal lane.
+- re-use previously generated outputs to re-sample new alternative paths
+
+Note that the velocity is just taken over from the input path.
+
+## Algorithm
+
+Sampling based planning is decomposed into 3 successive steps:
+
+1. Sampling: candidate trajectories are generated.
+2. Pruning: invalid candidates are discarded.
+3. Selection: the best remaining valid candidate is selected.
+
+### Sampling
+
+Candidate trajectories are generated based on the current ego state and some target state.
+1 sampling algorithms is currently implemented: polynomials in the frenet frame.
+
+### Pruning
+
+The validity of each candidate trajectory is checked using a set of hard constraints.
+
+- collision: ensure no collision with static obstacles;
+- curvature: ensure smooth curvature;
+- drivable area: ensure the trajectory stays within the drivable area.
+
+### Selection
+
+Among the valid candidate trajectories, the _best_ one is determined using a set of soft constraints (i.e., objective functions).
+
+- curvature: prefer smoother trajectories;
+- length: prefer trajectories with longer remaining path length;
+- lateral deviation: prefer trajectories close to the goal.
+
+Each soft constraint is associated with a weight to allow tuning of the preferences.
+
+## Limitations
+
+The quality of the candidates generated with polynomials in frenet frame greatly depend on the reference path.
+If the reference path is not smooth, the resulting candidates will probably be un-drivable.
+
+Failure to find a valid trajectory current results in a suddenly stopping trajectory.
+
+The module has troubles generating paths that converge rapidly to the goal lanelet. Basically, after overcoming all obstacles, the module should prioritize paths that rapidly make the ego vehicle converge back to its goal lane (ie. paths with low average and final lateral deviation). However, this does not function properly at the moment.
+
+Detection of proper merging can be rough: Sometimes, the module when detects that the ego has converged on the goal lanelet and that there are no more obstacles, the planner transitions to the goal planner, but the transition is not very smooth and could cause discomfort for the user.
+
+## Future works
+
+Some possible improvements for this module include:
+
+-Implementing a dynamic weight tuning algorithm: Dynamically change weights depending on the scenario (ie. to prioritize more the paths with low curvature and low avg. lat. deviation after all obstacles have been avoided).
+
+-Implementing multi-objective optimization to improve computing time and possibly make a more dynamic soft constraints weight tuning. [Related publication](https://ieeexplore.ieee.org/abstract/document/10180226).
+
+-Implement bezier curves as another method to obtain samples, see the [sampling_based_planner module](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/sampling_based_planner).
+
+-Explore the possibility to replace several or other behavior path modules with the sampling based behavior path module.
+
+-Perform real-life tests of this module once it has matured and some of its limitations have been solved.
+
+## Other possibilities
+
+The module is currently aimed at creating paths for static obstacle avoidance. However, the nature of sampling planner allows this module to be expanded or repurposed to other tasks such as lane changes, dynamic avoidance and general reaching of a goal. It is possible, with a good dynamic/scenario dependant weight tuning to use the sampling planning approach as a replacement for the other behavior path modules, assuming good candidate pruning and good soft constraints weight tuning.

planning/behavior_path_sampling_planner_module/config/sampling_planner.param.yaml

@@ -0,0 +1,20 @@
+/**:
+  ros__parameters:
+    start_planner:
+      max_curvature: 0.1
+      min_curvature: -0.1
+      lateral_deviation_weight: 1.0
+      length_weight: 1.0
+      curvature_weight: 1.0
+      enable_frenet: true
+      enable_bezier: false
+      resolution: 0.5
+      previous_path_reuse_points_nb: 2
+      nb_target_lateral_positions: {10.0, 20.0}
+      target_lengths: {-2.0, -1.0, 0.0, 1.0, 2.0}
+      target_lateral_positions: 2
+      target_lateral_velocities: {-0.1, 0.0, 0.1}
+      target_lateral_accelerations: {0.0}
+      force_zero_deviation: true
+      force_zero_heading: true
+      smooth_reference: false