feat(behavior_path_planner): add dynamic obstacle avoidance module

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -21,6 +21,7 @@
   <!-- behavior path planner -->
   <arg name="side_shift_param_path"/>
   <arg name="avoidance_param_path"/>
+  <arg name="dynamic_avoidance_param_path"/>
   <arg name="lane_change_param_path"/>
   <arg name="goal_planner_param_path"/>
   <arg name="pull_out_param_path"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -49,6 +49,8 @@ def launch_setup(context, *args, **kwargs):
         avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(LaunchConfiguration("avoidance_by_lc_param_path").perform(context), "r") as f:
         avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    with open(LaunchConfiguration("dynamic_avoidance_param_path").perform(context), "r") as f:
+        dynamic_avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f:
         lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f:
@@ -90,6 +92,7 @@ def launch_setup(context, *args, **kwargs):
             side_shift_param,
             avoidance_param,
             avoidance_by_lc_param,
+            dynamic_avoidance_param,
             lane_change_param,
             goal_planner_param,
             pull_out_param,

planning/behavior_path_planner/CMakeLists.txt

@@ -15,6 +15,7 @@ set(common_src
   src/scene_module/scene_module_visitor.cpp
   src/scene_module/avoidance/avoidance_module.cpp
   src/scene_module/avoidance_by_lc/module.cpp
+  src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
   src/scene_module/pull_out/pull_out_module.cpp
   src/scene_module/goal_planner/goal_planner_module.cpp
   src/scene_module/side_shift/side_shift_module.cpp
@@ -68,6 +69,7 @@ else()
     src/planner_manager.cpp
     src/scene_module/avoidance/manager.cpp
     src/scene_module/avoidance_by_lc/manager.cpp
+    src/scene_module/dynamic_avoidance/manager.cpp
     src/scene_module/pull_out/manager.cpp
     src/scene_module/goal_planner/manager.cpp
     src/scene_module/side_shift/manager.cpp

planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml

@@ -0,0 +1,20 @@
+/**:
+  ros__parameters:
+    dynamic_avoidance:
+      # avoidance is performed for the object type with true
+      target_object:
+        car: true
+        truck: true
+        bus: true
+        trailer: true
+        unknown: false
+        bicycle: false
+        motorcycle: true
+        pedestrian: false
+
+        min_obstacle_vel: 1.0 # [m/s]
+
+      drivable_area_generation:
+        lat_offset_from_obstacle: 0.8 # [m]
+        time_to_avoid: 5.0 # [s]
+        max_lat_offset_to_avoid: 1.0 # [m]

planning/behavior_path_planner/config/scene_module_manager.param.yaml

@@ -65,3 +65,10 @@
       enable_simultaneous_execution_as_candidate_module: false
       priority: 3
       max_module_size: 1
+
+    dynamic_avoidance:
+      enable_module: false
+      enable_simultaneous_execution_as_approved_module: true
+      enable_simultaneous_execution_as_candidate_module: true
+      priority: 7
+      max_module_size: 1

planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md

@@ -0,0 +1,22 @@
+# Dynamic avoidance design
+
+## Purpose / Role
+
+This is a module designed for avoiding obstacles which are running.
+Static obstacles such as parked vehicles are dealt with by the avoidance module.
+
+This module is under development.
+In the current implementation, the dynamic obstacles to avoid is extracted from the drivable area.
+Then the motion planner, in detail obstacle_avoidance_planner, will generate an avoiding trajectory.
+
+### Parameters
+
+| Name                                              | Unit  | Type   | Description                             | Default value |
+| :------------------------------------------------ | :---- | :----- | :-------------------------------------- | :------------ |
+| target_object.car                                 | [-]   | bool   | The flag whether to avoid cars or not   | true          |
+| target_object.truck                               | [-]   | bool   | The flag whether to avoid trucks or not | true          |
+| ...                                               | [-]   | bool   | ...                                     | ...           |
+| target_object.min_obstacle_vel                    | [m/s] | double | Minimum obstacle velocity to avoid      | 1.0           |
+| drivable_area_generation.lat_offset_from_obstacle | [m]   | double | Lateral offset to avoid from obstacles  | 0.8           |
+| drivable_area_generation.time_to_avoid            | [s]   | double | Elapsed time for avoiding an obstacle   | 5.0           |
+| drivable_area_generation.max_lat_offset_to_avoid  | [m]   | double | Maximum lateral offset to avoid         | 0.5           |

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -21,6 +21,7 @@
 #ifdef USE_OLD_ARCHITECTURE
 #include "behavior_path_planner/behavior_tree_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
+#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp"
 #include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
 #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
@@ -30,6 +31,7 @@
 #include "behavior_path_planner/planner_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp"
+#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp"
 #include "behavior_path_planner/scene_module/goal_planner/manager.hpp"
 #include "behavior_path_planner/scene_module/lane_change/manager.hpp"
 #include "behavior_path_planner/scene_module/pull_out/manager.hpp"
@@ -75,6 +77,7 @@ namespace behavior_path_planner
 {
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::Path;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
@@ -149,6 +152,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // parameters
   std::shared_ptr<AvoidanceParameters> avoidance_param_ptr_;
   std::shared_ptr<AvoidanceByLCParameters> avoidance_by_lc_param_ptr_;
+  std::shared_ptr<DynamicAvoidanceParameters> dynamic_avoidance_param_ptr_;
   std::shared_ptr<SideShiftParameters> side_shift_param_ptr_;
   std::shared_ptr<LaneChangeParameters> lane_change_param_ptr_;
   std::shared_ptr<PullOutParameters> pull_out_param_ptr_;
@@ -161,6 +165,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 #endif
 
   AvoidanceParameters getAvoidanceParam();
+  DynamicAvoidanceParameters getDynamicAvoidanceParam();
   LaneChangeParameters getLaneChangeParam();
   SideShiftParameters getSideShiftParam();
   GoalPlannerParameters getGoalPlannerParam();

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -43,6 +43,7 @@
 namespace behavior_path_planner
 {
 using autoware_adapi_v1_msgs::msg::OperationModeState;
+using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -35,6 +35,7 @@ struct BehaviorPathPlannerParameters
 
   ModuleConfigParameters config_avoidance;
   ModuleConfigParameters config_avoidance_by_lc;
+  ModuleConfigParameters config_dynamic_avoidance;
   ModuleConfigParameters config_pull_out;
   ModuleConfigParameters config_goal_planner;
   ModuleConfigParameters config_side_shift;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -0,0 +1,116 @@
+// Copyright 2023 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_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_
+
+#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
+
+#include <algorithm>
+#include <memory>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+namespace behavior_path_planner
+{
+struct DynamicAvoidanceParameters
+{
+  // obstacle types to avoid
+  bool avoid_car{true};
+  bool avoid_truck{true};
+  bool avoid_bus{true};
+  bool avoid_trailer{true};
+  bool avoid_unknown{false};
+  bool avoid_bicycle{false};
+  bool avoid_motorcycle{false};
+  bool avoid_pedestrian{false};
+  double min_obstacle_vel{0.0};
+
+  // drivable area generation
+  double lat_offset_from_obstacle{0.0};
+  double time_to_avoid{0.0};
+  double max_lat_offset_to_avoid{0.0};
+};
+
+class DynamicAvoidanceModule : public SceneModuleInterface
+{
+public:
+  struct DynamicAvoidanceObject
+  {
+    explicit DynamicAvoidanceObject(
+      const PredictedObject & predicted_object, const double arg_path_projected_vel)
+    : pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
+      path_projected_vel(arg_path_projected_vel),
+      shape(predicted_object.shape)
+    {
+    }
+
+    geometry_msgs::msg::Pose pose;
+    double path_projected_vel;
+    autoware_auto_perception_msgs::msg::Shape shape;
+  };
+
+#ifdef USE_OLD_ARCHITECTURE
+  DynamicAvoidanceModule(
+    const std::string & name, rclcpp::Node & node,
+    std::shared_ptr<DynamicAvoidanceParameters> parameters);
+#else
+  DynamicAvoidanceModule(
+    const std::string & name, rclcpp::Node & node,
+    std::shared_ptr<DynamicAvoidanceParameters> parameters,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
+
+  void updateModuleParams(const std::shared_ptr<DynamicAvoidanceParameters> & parameters)
+  {
+    parameters_ = parameters;
+  }
+#endif
+
+  bool isExecutionRequested() const override;
+  bool isExecutionReady() const override;
+  ModuleStatus updateState() override;
+  ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; }
+  BehaviorModuleOutput plan() override;
+  CandidateOutput planCandidate() const override;
+  BehaviorModuleOutput planWaitingApproval() override;
+  void updateData() override;
+  void acceptVisitor(
+    [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
+  {
+  }
+
+private:
+  std::vector<DynamicAvoidanceObject> calcTargetObjects() const;
+  lanelet::ConstLanelets getAdjacentLanes(
+    const double forward_distance, const double backward_distance) const;
+  std::optional<tier4_autoware_utils::Polygon2d> calcDynamicObstaclePolygon(
+    const PathWithLaneId & path, const DynamicAvoidanceObject & object) const;
+
+  std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject> target_objects_;
+  std::shared_ptr<DynamicAvoidanceParameters> parameters_;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp

@@ -0,0 +1,53 @@
+// Copyright 2023 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_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_
+
+#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp"
+#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface
+{
+public:
+  DynamicAvoidanceModuleManager(
+    rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
+    const std::shared_ptr<DynamicAvoidanceParameters> & parameters);
+
+  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+  {
+    return std::make_shared<DynamicAvoidanceModule>(
+      name_, *node_, parameters_, rtc_interface_ptr_map_);
+  }
+
+  void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
+
+private:
+  std::shared_ptr<DynamicAvoidanceParameters> parameters_;
+  std::vector<std::shared_ptr<DynamicAvoidanceModule>> registered_modules_;
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -121,6 +121,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   // set parameters
   {
     avoidance_param_ptr_ = std::make_shared<AvoidanceParameters>(getAvoidanceParam());
+    dynamic_avoidance_param_ptr_ =
+      std::make_shared<DynamicAvoidanceParameters>(getDynamicAvoidanceParam());
     lane_change_param_ptr_ = std::make_shared<LaneChangeParameters>(getLaneChangeParam());
     pull_out_param_ptr_ = std::make_shared<PullOutParameters>(getPullOutParam());
     goal_planner_param_ptr_ = std::make_shared<GoalPlannerParameters>(getGoalPlannerParam());
@@ -294,6 +296,12 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
         "avoidance_by_lane_change",
         create_publisher<Path>(path_reference_name_space + "avoidance_by_lane_change", 1));
     }
+
+    if (p.config_dynamic_avoidance.enable_module) {
+      auto manager = std::make_shared<DynamicAvoidanceModuleManager>(
+        this, "dynamic_avoidance", p.config_dynamic_avoidance, dynamic_avoidance_param_ptr_);
+      planner_manager_->registerSceneModuleManager(manager);
+    }
   }
 #endif
 
@@ -349,6 +357,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
     get_scene_module_manager_param("external_request_lane_change_left.");
   p.config_avoidance = get_scene_module_manager_param("avoidance.");
   p.config_avoidance_by_lc = get_scene_module_manager_param("avoidance_by_lc.");
+  p.config_dynamic_avoidance = get_scene_module_manager_param("dynamic_avoidance.");
 
   // vehicle info
   const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
@@ -647,6 +656,33 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   return p;
 }
 
+DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
+{
+  DynamicAvoidanceParameters p{};
+
+  {  // target object
+    std::string ns = "dynamic_avoidance.target_object.";
+    p.avoid_car = declare_parameter<bool>(ns + "car");
+    p.avoid_truck = declare_parameter<bool>(ns + "truck");
+    p.avoid_bus = declare_parameter<bool>(ns + "bus");
+    p.avoid_trailer = declare_parameter<bool>(ns + "trailer");
+    p.avoid_unknown = declare_parameter<bool>(ns + "unknown");
+    p.avoid_bicycle = declare_parameter<bool>(ns + "bicycle");
+    p.avoid_motorcycle = declare_parameter<bool>(ns + "motorcycle");
+    p.avoid_pedestrian = declare_parameter<bool>(ns + "pedestrian");
+    p.min_obstacle_vel = declare_parameter<double>(ns + "min_obstacle_vel");
+  }
+
+  {  // drivable_area_generation
+    std::string ns = "dynamic_avoidance.drivable_area_generation.";
+    p.lat_offset_from_obstacle = declare_parameter<double>(ns + "lat_offset_from_obstacle");
+    p.time_to_avoid = declare_parameter<double>(ns + "time_to_avoid");
+    p.max_lat_offset_to_avoid = declare_parameter<double>(ns + "max_lat_offset_to_avoid");
+  }
+
+  return p;
+}
+
 LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
 {
   LaneChangeParameters p{};