Merge branch 'main' into feat/insert_ahead_stop_point

Signed-off-by: kyoichi-sugahara <[email protected]>

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -314,6 +314,7 @@
       <arg name="input/detected_objects" value="$(var lidar_detection_model)/roi_fusion/objects"/>
       <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
       <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+      <arg name="obstacle_pointcloud_based_validator_param_path" value="$(var object_recognition_detection_obstacle_pointcloud_based_validator_param_path)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -348,6 +348,7 @@
       <arg name="input/detected_objects" value="$(var lidar_detection_model)/roi_fusion/objects"/>
       <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
       <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+      <arg name="obstacle_pointcloud_based_validator_param_path" value="$(var object_recognition_detection_obstacle_pointcloud_based_validator_param_path)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -115,6 +115,7 @@
       <arg name="input/detected_objects" value="$(var lidar_detection_model)/objects"/>
       <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
       <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+      <arg name="obstacle_pointcloud_based_validator_param_path" value="$(var object_recognition_detection_obstacle_pointcloud_based_validator_param_path)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -17,6 +17,7 @@
   <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
+  <arg name="object_recognition_detection_obstacle_pointcloud_based_validator_param_path"/>
   <arg name="occupancy_grid_map_method"/>
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>

perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+    min_points_num:
+    #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+      [10,     10,  10,    10,     10,     10,      10,         10]
+
+    max_points_num:
+    #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+      [10,     10,  10,    10,     10,     10,      10,         10]
+
+    min_points_and_distance_ratio:
+    #UNKNOWN,  CAR,    TRUCK,     BUS,    TRAILER, MOTORBIKE, BICYCLE,      PEDESTRIAN
+      [800.0,  800.0,  800.0,    800.0,   800.0,      800.0,    800.0,         800.0]

perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp

@@ -32,14 +32,14 @@
 
 #include <memory>
 #include <optional>
-
+#include <vector>
 namespace obstacle_pointcloud_based_validator
 {
 struct PointsNumThresholdParam
 {
-  size_t min_points_num;
-  size_t max_points_num;
-  float min_points_and_distance_ratio;
+  std::vector<int64_t> min_points_num;
+  std::vector<int64_t> max_points_num;
+  std::vector<double> min_points_and_distance_ratio;
 };
 class ObstaclePointCloudBasedValidator : public rclcpp::Node
 {

perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml

@@ -3,11 +3,13 @@
   <arg name="input/detected_objects" default="/perception/object_recognition/detection/objects"/>
   <arg name="input/obstacle_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="output/objects" default="/perception/object_recognition/detection/validation/obstacle_pointcloud_based/objects"/>
+  <arg name="obstacle_pointcloud_based_validator_param_path" default="$(find-pkg-share detected_object_validation)/config/obstacle_pointcloud_based_validator.param.yaml"/>
 
   <node pkg="detected_object_validation" exec="obstacle_pointcloud_based_validator_node" name="obstacle_pointcloud_based_validator_node" output="screen">
     <remap from="~/input/detected_objects" to="$(var input/detected_objects)"/>
     <remap from="~/input/obstacle_pointcloud" to="$(var input/obstacle_pointcloud)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
     <param name="enable_debugger" value="false"/>
+    <param from="$(var obstacle_pointcloud_based_validator_param_path)"/>
   </node>
 </launch>

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -92,10 +92,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
   objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "~/output/objects", rclcpp::QoS{1});
 
-  points_num_threshold_param_.min_points_num = declare_parameter<int>("min_points_num", 10);
-  points_num_threshold_param_.max_points_num = declare_parameter<int>("max_points_num", 10);
+  points_num_threshold_param_.min_points_num =
+    declare_parameter<std::vector<int64_t>>("min_points_num");
+  points_num_threshold_param_.max_points_num =
+    declare_parameter<std::vector<int64_t>>("max_points_num");
   points_num_threshold_param_.min_points_and_distance_ratio =
-    declare_parameter<float>("min_points_and_distance_ratio", 800.0);
+    declare_parameter<std::vector<double>>("min_points_and_distance_ratio");
 
   const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
   if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
@@ -134,6 +136,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
 
   for (size_t i = 0; i < transformed_objects.objects.size(); ++i) {
     const auto & transformed_object = transformed_objects.objects.at(i);
+    const auto object_label_id = transformed_object.classification.front().label;
     const auto & object = input_objects->objects.at(i);
     const auto & transformed_object_position =
       transformed_object.kinematics.pose_with_covariance.pose.position;
@@ -155,13 +158,17 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
     if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud);
 
     // Filter object that have few pointcloud in them.
+    // TODO(badai-nguyen) add 3d validator option
     const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud);
     const auto object_distance =
       std::hypot(transformed_object_position.x, transformed_object_position.y);
     size_t min_pointcloud_num = std::clamp(
       static_cast<size_t>(
-        points_num_threshold_param_.min_points_and_distance_ratio / object_distance + 0.5f),
-      points_num_threshold_param_.min_points_num, points_num_threshold_param_.max_points_num);
+        points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) /
+          object_distance +
+        0.5f),
+      static_cast<size_t>(points_num_threshold_param_.min_points_num.at(object_label_id)),
+      static_cast<size_t>(points_num_threshold_param_.max_points_num.at(object_label_id)));
     if (num) {
       (min_pointcloud_num <= num.value()) ? output.objects.push_back(object)
                                           : removed_objects.objects.push_back(object);

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -139,12 +139,16 @@
         check_other_object: true                         # [-]
         # collision check parameters
         check_all_predicted_path: false                  # [-]
-        time_resolution: 0.5                             # [s]
-        time_horizon_for_front_object: 10.0              # [s]
-        time_horizon_for_rear_object: 10.0               # [s]
         safety_check_backward_distance: 100.0            # [m]
         hysteresis_factor_expand_rate: 2.0               # [-]
         hysteresis_factor_safe_count: 10                 # [-]
+        # predicted path parameters
+        min_velocity: 1.38                               # [m/s]
+        max_velocity: 50.0                               # [m/s]
+        time_resolution: 0.5                             # [s]
+        time_horizon_for_front_object: 10.0              # [s]
+        time_horizon_for_rear_object: 10.0               # [s]
+        delay_until_departure: 0.0                       # [s]
         # rss parameters
         expected_front_deceleration: -1.0                # [m/ss]
         expected_rear_deceleration: -1.0                 # [m/ss]

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -183,9 +183,6 @@ struct AvoidanceParameters
 
   // parameters for collision check.
   bool check_all_predicted_path{false};
-  double time_horizon_for_front_object{0.0};
-  double time_horizon_for_rear_object{0.0};
-  double safety_check_time_resolution{0.0};
 
   // find adjacent lane vehicles
   double safety_check_backward_distance{0.0};
@@ -295,6 +292,9 @@ struct AvoidanceParameters
   // parameters depend on object class
   std::unordered_map<uint8_t, ObjectParameter> object_parameters;
 
+  // ego predicted path params.
+  utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{};
+
   // rss parameters
   utils::path_safety_checker::RSSparams rss_params{};
 

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -87,11 +87,6 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
   const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
   const double vehicle_width);
 
-std::vector<PoseWithVelocityStamped> convertToPredictedPath(
-  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
-  const bool is_object_front, const bool limit_to_max_velocity,
-  const std::shared_ptr<AvoidanceParameters> & parameters);
-
 double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);
 
 bool isCentroidWithinLanelets(
@@ -162,9 +157,6 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine
 AvoidLineArray combineRawShiftLinesWithUniqueCheck(
   const AvoidLineArray & base_lines, const AvoidLineArray & added_lines);
 
-ExtendedPredictedObject transform(
-  const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);
-
 std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
   const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp

@@ -117,14 +117,16 @@ void filterObjectsByClass(
  * lanelet.
  */
 std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
+  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,
+  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet)> & condition);
 
 /**
  * @brief Separate the objects into two part based on whether the object is within lanelet.
  * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet.
  */
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
+  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,
+  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet)> & condition);
 
 /**
  * @brief Get the predicted path from an object.

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -336,6 +336,8 @@ std::optional<double> getSignedDistanceFromBoundary(
 
 // misc
 
+Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet);
+
 std::vector<Polygon2d> getTargetLaneletPolygons(
   const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length,
   const std::string & target_type);

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -1839,10 +1839,16 @@ bool AvoidanceModule::isSafePath(
   }
 
   const bool limit_to_max_velocity = false;
-  const auto ego_predicted_path_for_front_object = utils::avoidance::convertToPredictedPath(
-    shifted_path.path, planner_data_, true, limit_to_max_velocity, parameters_);
-  const auto ego_predicted_path_for_rear_object = utils::avoidance::convertToPredictedPath(
-    shifted_path.path, planner_data_, false, limit_to_max_velocity, parameters_);
+  const auto ego_predicted_path_params =
+    std::make_shared<utils::path_safety_checker::EgoPredictedPathParams>(
+      parameters_->ego_predicted_path_params);
+  const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points);
+  const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath(
+    ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
+    true, limit_to_max_velocity);
+  const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath(
+    ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
+    false, limit_to_max_velocity);
 
   const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points);
   const auto is_right_shift = [&]() -> std::optional<bool> {

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -142,11 +142,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.check_other_object = getOrDeclareParameter<bool>(*node, ns + "check_other_object");
     p.check_all_predicted_path =
       getOrDeclareParameter<bool>(*node, ns + "check_all_predicted_path");
-    p.time_horizon_for_front_object =
-      getOrDeclareParameter<double>(*node, ns + "time_horizon_for_front_object");
-    p.time_horizon_for_rear_object =
-      getOrDeclareParameter<double>(*node, ns + "time_horizon_for_rear_object");
-    p.safety_check_time_resolution = getOrDeclareParameter<double>(*node, ns + "time_resolution");
     p.safety_check_backward_distance =
       getOrDeclareParameter<double>(*node, ns + "safety_check_backward_distance");
     p.hysteresis_factor_expand_rate =
@@ -155,6 +150,23 @@ AvoidanceModuleManager::AvoidanceModuleManager(
       getOrDeclareParameter<int>(*node, ns + "hysteresis_factor_safe_count");
   }
 
+  // safety check predicted path params
+  {
+    std::string ns = "avoidance.safety_check.";
+    p.ego_predicted_path_params.min_velocity =
+      getOrDeclareParameter<double>(*node, ns + "min_velocity");
+    p.ego_predicted_path_params.max_velocity =
+      getOrDeclareParameter<double>(*node, ns + "max_velocity");
+    p.ego_predicted_path_params.acceleration =
+      getOrDeclareParameter<double>(*node, "avoidance.constraints.longitudinal.max_acceleration");
+    p.ego_predicted_path_params.time_horizon_for_rear_object =
+      getOrDeclareParameter<double>(*node, ns + "time_horizon_for_rear_object");
+    p.ego_predicted_path_params.time_resolution =
+      getOrDeclareParameter<double>(*node, ns + "time_resolution");
+    p.ego_predicted_path_params.delay_until_departure =
+      getOrDeclareParameter<double>(*node, ns + "delay_until_departure");
+  }
+
   // safety check rss params
   {
     std::string ns = "avoidance.safety_check.";

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -21,6 +21,7 @@
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
@@ -1300,12 +1301,17 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
     return false;
   }
 
+  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
+    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
+    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
+    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
+  };
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length);
   const auto [pull_over_lane_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *(planner_data_->dynamic_object), pull_over_lanes);
+      *(planner_data_->dynamic_object), pull_over_lanes, condition);
   const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     pull_over_lane_objects, parameters_->th_moving_object_velocity);
   const auto common_parameters = planner_data_->parameters;

planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp

@@ -17,6 +17,7 @@
 #include "behavior_path_planner/utils/avoidance/utils.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <tier4_autoware_utils/geometry/boost_geometry.hpp>
@@ -140,9 +141,14 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
 {
   const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);
 
+  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
+    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
+    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
+    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
+  };
   const auto [object_within_target_lane, object_outside_target_lane] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *planner_data_->dynamic_object, data.current_lanelets);
+      *planner_data_->dynamic_object, data.current_lanelets, condition);
 
   // Assume that the maximum allocation for data.other object is the sum of
   // objects_within_target_lane and object_outside_target_lane. The maximum allocation for

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -20,6 +20,7 @@
 #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp"
 #include "behavior_path_planner/utils/start_planner/util.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -770,9 +771,14 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
   std::vector<Pose> pull_out_start_pose{};
 
   // filter pull out lanes stop objects
+  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
+    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
+    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
+    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
+  };
   const auto [pull_out_lane_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *planner_data_->dynamic_object, status_.pull_out_lanes);
+      *planner_data_->dynamic_object, status_.pull_out_lanes, condition);
   const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     pull_out_lane_objects, parameters_->th_moving_object_velocity);