feat(behavior_path_sampling_module): add sampling based planner

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)"/>

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

planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp

@@ -0,0 +1,54 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_
+#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_
+
+#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+class SamplingPlannerModuleManager : public SceneModuleManagerInterface
+{
+public:
+  SamplingPlannerModuleManager() : SceneModuleManagerInterface{"sampling_planner"} {}
+  void init(rclcpp::Node * node) override;
+
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+  {
+    return std::make_unique<SamplingPlannerModule>(
+      name_, *node_, parameters_, rtc_interface_ptr_map_,
+      objects_of_interest_marker_interface_ptr_map_);
+  }
+
+  void updateModuleParams(
+    [[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) override;
+
+private:
+  std::shared_ptr<SamplingPlannerParameters> parameters_;
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_