feat(behavior_velocity): add mandatory detection area for run out mod…

…ule (#2864)

* feat: add mandatory detection area

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* change the topic order to subscribe compare map filtered points

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* define function for transform pointcloud

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* add missing mutex lock

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* fix subscribing topic for points

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* remove unnecessary comments

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* add debug publisher for pointcloud

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* fix warning for empty frame id

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* add comments

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* add parameter whether to use mandatory detection area

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* use the same stop margin for the two kind of detection area

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* remove unused parameter

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* change max queue size

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* change the marker color of mandatory detection area

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* fix publishing debug pointcloud

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* create function to concatenate clouds

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* use current_odometory instead of current_pose

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* add param for mandatory area

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

@@ -164,6 +164,10 @@ def launch_setup(context, *args, **kwargs):
                 "~/input/compare_map_filtered_pointcloud",
                 "compare_map_filtered/pointcloud",
             ),
+            (
+                "~/input/vector_map_inside_area_filtered_pointcloud",
+                "vector_map_inside_area_filtered/pointcloud",
+            ),
             (
                 "~/input/traffic_signals",
                 "/perception/traffic_light_recognition/traffic_signals",

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

@@ -45,7 +45,7 @@ def add_launch_arg(name: str, default_value=None):
             plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent",
             name="voxel_distance_based_compare_map_filter_node",
             remappings=[
-                ("input", "vector_map_inside_area_filtered/pointcloud"),
+                ("input", "/perception/obstacle_segmentation/pointcloud"),
                 ("map", "/map/pointcloud_map"),
                 ("output", "compare_map_filtered/pointcloud"),
             ],

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

@@ -45,7 +45,7 @@ def add_launch_arg(name: str, default_value=None):
             plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent",
             name="vector_map_inside_area_filter_node",
             remappings=[
-                ("input", "/perception/obstacle_segmentation/pointcloud"),
+                ("input", "compare_map_filtered/pointcloud"),
                 ("input/vector_map", "/map/vector_map"),
                 ("output", "vector_map_inside_area_filtered/pointcloud"),
             ],

planning/behavior_velocity_planner/CMakeLists.txt

@@ -51,6 +51,7 @@ ament_target_dependencies(behavior_velocity_planner
   Boost
   Eigen3
   PCL
+  message_filters
 )
 
 target_link_libraries(behavior_velocity_planner ${PCL_LIBRARIES})

planning/behavior_velocity_planner/config/run_out.param.yaml

@@ -15,15 +15,20 @@
         margin_behind: 0.5 # [m] ahead margin for detection area length
         margin_ahead: 1.0  # [m] behind margin for detection area length
 
+      # This area uses points not filtered by vector map to ensure safety
+      mandatory_area:
+        decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area
+
       # parameter to create abstracted dynamic obstacles
       dynamic_obstacle:
-        min_vel_kmph: 0.0         # [km/h] minimum velocity for dynamic obstacles
-        max_vel_kmph: 5.0         # [km/h] maximum velocity for dynamic obstacles
-        diameter: 0.1             # [m] diameter of obstacles. used for creating dynamic obstacles from points
-        height: 2.0               # [m] height of obstacles. used for creating dynamic obstacles from points
-        max_prediction_time: 10.0 # [sec] create predicted path until this time
-        time_step: 0.5            # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
-        points_interval: 0.1      # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
+        use_mandatory_area: false  # [-] whether to use mandatory detection area
+        min_vel_kmph: 0.0          # [km/h] minimum velocity for dynamic obstacles
+        max_vel_kmph: 5.0          # [km/h] maximum velocity for dynamic obstacles
+        diameter: 0.1              # [m] diameter of obstacles. used for creating dynamic obstacles from points
+        height: 2.0                # [m] height of obstacles. used for creating dynamic obstacles from points
+        max_prediction_time: 10.0  # [sec] create predicted path until this time
+        time_step: 0.5             # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
+        points_interval: 0.1       # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
 
       # approach if ego has stopped in the front of the obstacle for a certain amount of time
       approaching:

planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp

@@ -14,14 +14,15 @@
 #ifndef SCENE_MODULE__RUN_OUT__DEBUG_HPP_
 #define SCENE_MODULE__RUN_OUT__DEBUG_HPP_
 
-#include "scene_module/run_out/dynamic_obstacle.hpp"
+#include "scene_module/run_out/utils.hpp"
 
 #include <memory>
 #include <string>
 #include <vector>
 namespace behavior_velocity_planner
 {
 using autoware_auto_planning_msgs::msg::Trajectory;
+using sensor_msgs::msg::PointCloud2;
 using tier4_debug_msgs::msg::Float32MultiArrayStamped;
 using tier4_debug_msgs::msg::Int32Stamped;
 
@@ -102,11 +103,16 @@ class RunOutDebug
   void pushPredictedObstaclePolygons(const std::vector<geometry_msgs::msg::Point> & polygon);
   void pushCollisionObstaclePolygons(const std::vector<geometry_msgs::msg::Point> & polygon);
   void pushDetectionAreaPolygons(const Polygon2d & debug_polygon);
+  void pushMandatoryDetectionAreaPolygons(const Polygon2d & debug_polygon);
   void pushTravelTimeTexts(
     const double travel_time, const geometry_msgs::msg::Pose pose, const float lateral_offset);
   void setAccelReason(const AccelReason & accel_reason);
   void publishDebugValue();
   void publishDebugTrajectory(const Trajectory & trajectory);
+  void publishFilteredPointCloud(const PointCloud2 & pointcloud);
+  void publishFilteredPointCloud(
+    const pcl::PointCloud<pcl::PointXYZ> & pointcloud, const std_msgs::msg::Header header);
+  void publishEmptyPointCloud();
   visualization_msgs::msg::MarkerArray createVisualizationMarkerArray();
   void setHeight(const double height);
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray();
@@ -120,13 +126,15 @@ class RunOutDebug
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr pub_debug_values_;
   rclcpp::Publisher<Int32Stamped>::SharedPtr pub_accel_reason_;
   rclcpp::Publisher<Trajectory>::SharedPtr pub_debug_trajectory_;
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_debug_pointcloud_;
   std::vector<geometry_msgs::msg::Point> collision_points_;
   std::vector<geometry_msgs::msg::Point> nearest_collision_point_;
   std::vector<geometry_msgs::msg::Pose> stop_pose_;
   std::vector<std::vector<geometry_msgs::msg::Point>> predicted_vehicle_polygons_;
   std::vector<std::vector<geometry_msgs::msg::Point>> predicted_obstacle_polygons_;
   std::vector<std::vector<geometry_msgs::msg::Point>> collision_obstacle_polygons_;
   std::vector<std::vector<geometry_msgs::msg::Point>> detection_area_polygons_;
+  std::vector<std::vector<geometry_msgs::msg::Point>> mandatory_detection_area_polygons_;
   std::vector<TextWithPosition> travel_time_texts_;
   DebugValues debug_values_;
   AccelReason accel_reason_;

planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp

@@ -16,13 +16,18 @@
 #define SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_
 
 #include "behavior_velocity_planner/planner_data.hpp"
+#include "scene_module/run_out/debug.hpp"
+#include "scene_module/run_out/utils.hpp"
 #include "utilization/path_utilization.hpp"
 #include "utilization/util.hpp"
 
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/synchronizer.h>
 #include <pcl/common/transforms.h>
 #include <pcl_conversions/pcl_conversions.h>
 
@@ -32,6 +37,7 @@
 #include <tf2_eigen/tf2_eigen.hpp>
 #endif
 
+#include <memory>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -41,80 +47,62 @@ using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::Shape;
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using run_out_utils::DynamicObstacle;
+using run_out_utils::DynamicObstacleData;
+using run_out_utils::DynamicObstacleParam;
+using run_out_utils::PlannerParam;
+using run_out_utils::PredictedPath;
 using PathPointsWithLaneId = std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>;
 
-struct DynamicObstacleParam
-{
-  float min_vel_kmph{0.0};
-  float max_vel_kmph{5.0};
-
-  // parameter to convert points to dynamic obstacle
-  float diameter{0.1};              // [m]
-  float height{2.0};                // [m]
-  float max_prediction_time{10.0};  // [sec]
-  float time_step{0.5};             // [sec]
-  float points_interval{0.1};       // [m]
-};
-
-struct PoseWithRange
-{
-  geometry_msgs::msg::Pose pose_min;
-  geometry_msgs::msg::Pose pose_max;
-};
-
-// since we use the minimum and maximum velocity,
-// define the PredictedPath without time_step
-struct PredictedPath
-{
-  std::vector<geometry_msgs::msg::Pose> path;
-  float confidence;
-};
-
-// abstracted obstacle information
-struct DynamicObstacle
-{
-  geometry_msgs::msg::Pose pose;
-  std::vector<geometry_msgs::msg::Point> collision_points;
-  geometry_msgs::msg::Point nearest_collision_point;
-  float min_velocity_mps;
-  float max_velocity_mps;
-  std::vector<ObjectClassification> classifications;
-  Shape shape;
-  std::vector<PredictedPath> predicted_paths;
-};
-
-struct DynamicObstacleData
-{
-  PredictedObjects predicted_objects;
-  pcl::PointCloud<pcl::PointXYZ> obstacle_points;
-  PathWithLaneId path;
-  Polygons2d detection_area_polygon;
-};
-
 /**
  * @brief base class for creating dynamic obstacles from multiple types of input
  */
 class DynamicObstacleCreator
 {
 public:
-  explicit DynamicObstacleCreator(rclcpp::Node & node) : node_(node) {}
+  using PointCloud2 = sensor_msgs::msg::PointCloud2;
+  using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime<PointCloud2, PointCloud2>;
+  using ExactTimeSynchronizer = message_filters::Synchronizer<ExactTimeSyncPolicy>;
+
+  explicit DynamicObstacleCreator(
+    rclcpp::Node & node, std::shared_ptr<RunOutDebug> & debug_ptr,
+    const DynamicObstacleParam & param)
+  : node_(node), debug_ptr_(debug_ptr), param_(param)
+  {
+  }
   virtual ~DynamicObstacleCreator() = default;
   virtual std::vector<DynamicObstacle> createDynamicObstacles() = 0;
-  void setParam(const DynamicObstacleParam & param) { param_ = param; }
   void setData(
-    const PlannerData & planner_data, const PathWithLaneId & path, const Polygons2d & poly)
+    const PlannerData & planner_data, const PlannerParam & planner_param,
+    const PathWithLaneId & path, const PathWithLaneId & smoothed_path)
   {
     std::lock_guard<std::mutex> lock(mutex_);
 
-    // compare map filtered points are subscribed in derived class that needs points
     dynamic_obstacle_data_.predicted_objects = *planner_data.predicted_objects;
     dynamic_obstacle_data_.path = path;
-    dynamic_obstacle_data_.detection_area_polygon = poly;
+
+    // detection area is used only when detection target is Points
+    if (planner_param.run_out.detection_method == "Points") {
+      dynamic_obstacle_data_.detection_area =
+        createDetectionAreaPolygon(smoothed_path, planner_data, planner_param);
+      for (const auto & poly : dynamic_obstacle_data_.detection_area) {
+        debug_ptr_->pushDetectionAreaPolygons(poly);
+      }
+
+      if (param_.use_mandatory_area) {
+        dynamic_obstacle_data_.mandatory_detection_area =
+          createMandatoryDetectionAreaPolygon(smoothed_path, planner_data, planner_param);
+        for (const auto & poly : dynamic_obstacle_data_.mandatory_detection_area) {
+          debug_ptr_->pushMandatoryDetectionAreaPolygons(poly);
+        }
+      }
+    }
   }
 
 protected:
-  DynamicObstacleParam param_;
   rclcpp::Node & node_;
+  std::shared_ptr<RunOutDebug> debug_ptr_;
+  DynamicObstacleParam param_;
   DynamicObstacleData dynamic_obstacle_data_;
 
   // mutex for dynamic_obstacle_data_
@@ -127,7 +115,9 @@ class DynamicObstacleCreator
 class DynamicObstacleCreatorForObject : public DynamicObstacleCreator
 {
 public:
-  explicit DynamicObstacleCreatorForObject(rclcpp::Node & node);
+  explicit DynamicObstacleCreatorForObject(
+    rclcpp::Node & node, std::shared_ptr<RunOutDebug> & debug_ptr,
+    const DynamicObstacleParam & param);
   std::vector<DynamicObstacle> createDynamicObstacles() override;
 };
 
@@ -138,7 +128,9 @@ class DynamicObstacleCreatorForObject : public DynamicObstacleCreator
 class DynamicObstacleCreatorForObjectWithoutPath : public DynamicObstacleCreator
 {
 public:
-  explicit DynamicObstacleCreatorForObjectWithoutPath(rclcpp::Node & node);
+  explicit DynamicObstacleCreatorForObjectWithoutPath(
+    rclcpp::Node & node, std::shared_ptr<RunOutDebug> & debug_ptr,
+    const DynamicObstacleParam & param);
   std::vector<DynamicObstacle> createDynamicObstacles() override;
 };
 
@@ -149,17 +141,32 @@ class DynamicObstacleCreatorForObjectWithoutPath : public DynamicObstacleCreator
 class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator
 {
 public:
-  explicit DynamicObstacleCreatorForPoints(rclcpp::Node & node);
+  explicit DynamicObstacleCreatorForPoints(
+    rclcpp::Node & node, std::shared_ptr<RunOutDebug> & debug_ptr,
+    const DynamicObstacleParam & param);
   std::vector<DynamicObstacle> createDynamicObstacles() override;
 
 private:
   void onCompareMapFilteredPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
+
+  void onSynchronizedPointCloud(
+    const PointCloud2::ConstSharedPtr compare_map_filtered_points,
+    const PointCloud2::ConstSharedPtr vector_map_filtered_points);
+
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
     sub_compare_map_filtered_pointcloud_;
 
+  // synchronized subscribers
+  message_filters::Subscriber<PointCloud2> sub_compare_map_filtered_pointcloud_sync_;
+  message_filters::Subscriber<PointCloud2> sub_vector_map_inside_area_filtered_pointcloud_sync_;
+  std::unique_ptr<ExactTimeSynchronizer> exact_time_synchronizer_;
+
   // tf
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
+
+  // obstacle points
+  pcl::PointCloud<pcl::PointXYZ> obstacle_points_map_filtered_;
 };
 
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp

@@ -39,7 +39,7 @@ class RunOutModuleManager : public SceneModuleManagerInterface
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
-  void setDynamicObstacleCreator(rclcpp::Node & node);
+  void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr<RunOutDebug> & debug_ptr);
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp

@@ -32,6 +32,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using run_out_utils::PlannerParam;
+using run_out_utils::PoseWithRange;
 using tier4_debug_msgs::msg::Float32Stamped;
 using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;