feat(goal_planne): check objects within the area between ego edge and…

… boudary of pull_over_lanes (autowarefoundation#6369)

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml

@@ -39,6 +39,8 @@
         object_recognition_collision_check_max_extra_stopping_margin: 1.0
         th_moving_object_velocity: 1.0
         detection_bound_offset: 15.0
+        outer_road_detection_offset: 1.0
+        inner_road_detection_offset: 0.0
 
       # pull over
       pull_over:

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -221,6 +221,7 @@ struct GoalPlannerDebugData
 {
   FreespacePlannerDebugData freespace_planner{};
   std::vector<Polygon2d> ego_polygons_expanded{};
+  lanelet::ConstLanelet expanded_pull_over_lane_between_ego{};
 };
 
 struct LastApprovalData

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp

@@ -75,6 +75,8 @@ struct GoalPlannerParameters
   double object_recognition_collision_check_max_extra_stopping_margin{0.0};
   double th_moving_object_velocity{0.0};
   double detection_bound_offset{0.0};
+  double outer_road_detection_offset{0.0};
+  double inner_road_detection_offset{0.0};
 
   // pull over general params
   double pull_over_minimum_request_length{0.0};

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp

@@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray;
 lanelet::ConstLanelets getPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance);
+
+/*
+ * @brief expand pull_over_lanes to the opposite side of drivable roads by bound_offset.
+ * bound_offset must be positive regardless of left_side is true/false
+ */
 lanelet::ConstLanelets generateExpandedPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
-  const double forward_distance, double bound_offset);
+  const double forward_distance, const double bound_offset);
+
+lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
+  const lanelet::ConstLanelets & pull_over_lanes, const bool left_side,
+  const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
+  const double outer_road_offset, const double inner_road_offset);
 PredictedObjects extractObjectsInExpandedPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance, double bound_offset, const PredictedObjects & objects);
@@ -85,6 +95,9 @@ MarkerArray createTextsMarkerArray(
   const std::vector<Pose> & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color);
 MarkerArray createGoalCandidatesMarkerArray(
   const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color);
+MarkerArray createLaneletPolygonMarkerArray(
+  const lanelet::CompoundPolygon3d & polygon, const std_msgs::msg::Header & header,
+  const std::string & ns, const std_msgs::msg::ColorRGBA & color);
 MarkerArray createNumObjectsToAvoidTextsMarkerArray(
   const GoalCandidates & goal_candidates, std::string && ns,
   const std_msgs::msg::ColorRGBA & color);

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1867,6 +1867,50 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData(
   goal_planner_data_.ego_predicted_path = ego_predicted_path;
 }
 
+static std::vector<utils::path_safety_checker::ExtendedPredictedObject> filterObjectsByWithinPolicy(
+  const std::shared_ptr<const PredictedObjects> & objects,
+  const lanelet::ConstLanelets & target_lanes,
+  const std::shared_ptr<behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams> &
+    params)
+{
+  // implanted part of behavior_path_planner::utils::path_safety_checker::filterObjects() and
+  // createTargetObjectsOnLane()
+
+  // Guard
+  if (objects->objects.empty()) {
+    return {};
+  }
+
+  const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold;
+  const auto & target_object_types = params->object_types_to_check;
+
+  PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity(
+    *objects, ignore_object_velocity_threshold, false);
+
+  utils::path_safety_checker::filterObjectsByClass(filtered_objects, target_object_types);
+
+  std::vector<PredictedObject> within_filtered_objects;
+  for (const auto & target_lane : target_lanes) {
+    const auto lane_poly = target_lane.polygon2d().basicPolygon();
+    for (const auto & filtered_object : filtered_objects.objects) {
+      const auto object_bbox = tier4_autoware_utils::toPolygon2d(filtered_object);
+      if (boost::geometry::within(object_bbox, lane_poly)) {
+        within_filtered_objects.push_back(filtered_object);
+      }
+    }
+  }
+
+  const double safety_check_time_horizon = params->safety_check_time_horizon;
+  const double safety_check_time_resolution = params->safety_check_time_resolution;
+
+  std::vector<utils::path_safety_checker::ExtendedPredictedObject> refined_filtered_objects;
+  for (const auto & within_filtered_object : within_filtered_objects) {
+    refined_filtered_objects.push_back(utils::path_safety_checker::transform(
+      within_filtered_object, safety_check_time_horizon, safety_check_time_resolution));
+  }
+  return refined_filtered_objects;
+}
+
 std::pair<bool, bool> GoalPlannerModule::isSafePath() const
 {
   if (!thread_safe_data_.get_pull_over_path()) {
@@ -1904,31 +1948,67 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath() const
       ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity,
       ego_seg_idx, is_object_front, limit_to_max_velocity);
 
-  // filtering objects with velocity, position and class
-  const auto filtered_objects = utils::path_safety_checker::filterObjects(
-    dynamic_object, route_handler, pull_over_lanes, current_pose.position,
-    objects_filtering_params_);
+  // ==========================================================================================
+  // if ego is before the entry of pull_over_lanes, the beginning of the safety check area
+  // should be from the entry of pull_over_lanes
+  // ==========================================================================================
+  const Pose ego_pose_for_expand = std::invoke([&]() {
+    // get first road lane in pull over lanes segment
+    const auto fist_road_lane = std::invoke([&]() {
+      const auto first_pull_over_lane = pull_over_lanes.front();
+      if (!route_handler->isShoulderLanelet(first_pull_over_lane)) {
+        return first_pull_over_lane;
+      }
+      const auto road_lane_opt = left_side_parking_
+                                   ? route_handler->getRightLanelet(first_pull_over_lane)
+                                   : route_handler->getLeftLanelet(first_pull_over_lane);
+      if (road_lane_opt) {
+        return road_lane_opt.value();
+      }
+      return first_pull_over_lane;
+    });
+    // generate first road lane pose
+    Pose first_road_pose{};
+    const auto first_road_point =
+      lanelet::utils::conversion::toGeomMsgPt(fist_road_lane.centerline().front());
+    const double lane_yaw = lanelet::utils::getLaneletAngle(fist_road_lane, first_road_point);
+    first_road_pose.position = first_road_point;
+    first_road_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw);
+    // if current ego pose is before pull over lanes segment, use first road lanelet center pose
+    if (
+      calcSignedArcLength(pull_over_path.points, first_road_pose.position, current_pose.position) <
+      0.0) {
+      return first_road_pose;
+    }
+    // if current ego pose is in pull over lanes segment, use current ego pose
+    return current_pose;
+  });
 
   // filtering objects based on the current position's lane
-  const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
-    pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_);
-
-  utils::parking_departure::updateSafetyCheckTargetObjectsData(
-    goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
+  const auto expanded_pull_over_lanes_between_ego =
+    goal_planner_utils::generateBetweenEgoAndExpandedPullOverLanes(
+      pull_over_lanes, left_side_parking_, ego_pose_for_expand,
+      planner_data_->parameters.vehicle_info, parameters_->outer_road_detection_offset,
+      parameters_->inner_road_detection_offset);
+  const auto merged_expanded_pull_over_lanes =
+    lanelet::utils::combineLaneletsShape(expanded_pull_over_lanes_between_ego);
+  debug_data_.expanded_pull_over_lane_between_ego = merged_expanded_pull_over_lanes;
+
+  const auto filtered_objects = filterObjectsByWithinPolicy(
+    dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params_);
 
   const double hysteresis_factor =
     prev_data_.safety_status.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate;
 
   const bool current_is_safe = std::invoke([&]() {
     if (parameters_->safety_check_params.method == "RSS") {
       return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
-        pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane,
-        goal_planner_data_.collision_check, planner_data_->parameters,
-        safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path,
-        hysteresis_factor);
+        pull_over_path, ego_predicted_path, filtered_objects, goal_planner_data_.collision_check,
+        planner_data_->parameters, safety_check_params_->rss_params,
+        objects_filtering_params_->use_all_predicted_path, hysteresis_factor);
     } else if (parameters_->safety_check_params.method == "integral_predicted_polygon") {
       return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon(
-        ego_predicted_path, vehicle_info_, target_objects_on_lane.on_current_lane,
+        ego_predicted_path, vehicle_info_, filtered_objects,
         objects_filtering_params_->check_all_predicted_path,
         parameters_->safety_check_params.integral_predicted_polygon_params,
         goal_planner_data_.collision_check);
@@ -2046,6 +2126,15 @@ void GoalPlannerModule::setDebugData()
     }
     debug_marker_.markers.push_back(marker);
 
+    if (parameters_->safety_check_params.enable_safety_check) {
+      tier4_autoware_utils::appendMarkerArray(
+        goal_planner_utils::createLaneletPolygonMarkerArray(
+          debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header,
+          "expanded_pull_over_lane_between_ego",
+          tier4_autoware_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)),
+        &debug_marker_);
+    }
+
     // Visualize debug poses
     const auto & debug_poses = thread_safe_data_.get_pull_over_path()->debug_poses;
     for (size_t i = 0; i < debug_poses.size(); ++i) {

planning/behavior_path_goal_planner_module/src/manager.cpp

@@ -102,6 +102,10 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
         ns + "object_recognition_collision_check_max_extra_stopping_margin");
     p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
     p.detection_bound_offset = node->declare_parameter<double>(ns + "detection_bound_offset");
+    p.outer_road_detection_offset =
+      node->declare_parameter<double>(ns + "outer_road_detection_offset");
+    p.inner_road_detection_offset =
+      node->declare_parameter<double>(ns + "inner_road_detection_offset");
   }
 
   // pull over general params

planning/behavior_path_goal_planner_module/src/util.cpp

@@ -17,6 +17,7 @@
 #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
+#include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -78,13 +79,62 @@ lanelet::ConstLanelets getPullOverLanes(
 
 lanelet::ConstLanelets generateExpandedPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
-  const double forward_distance, double bound_offset)
+  const double forward_distance, const double bound_offset)
 {
   const auto pull_over_lanes =
     getPullOverLanes(route_handler, left_side, backward_distance, forward_distance);
 
   return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0)
-                   : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset);
+                   : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, -bound_offset);
+}
+
+static double getOffsetToLanesBoundary(
+  const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose target_pose,
+  const bool left_side)
+{
+  lanelet::ConstLanelet closest_lanelet;
+  lanelet::utils::query::getClosestLanelet(lanelet_sequence, target_pose, &closest_lanelet);
+
+  // the boundary closer to ego. if left_side, take right boundary
+  const auto & boundary3d = left_side ? closest_lanelet.rightBound() : closest_lanelet.leftBound();
+  const auto boundary = lanelet::utils::to2D(boundary3d);
+  using lanelet::utils::conversion::toLaneletPoint;
+  const auto arc_coords = lanelet::geometry::toArcCoordinates(
+    boundary, lanelet::utils::to2D(toLaneletPoint(target_pose.position)).basicPoint());
+  return arc_coords.distance;
+}
+
+lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
+  const lanelet::ConstLanelets & pull_over_lanes, const bool left_side,
+  const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
+  const double outer_road_offset, const double inner_road_offset)
+{
+  const double front_overhang = vehicle_info.front_overhang_m,
+               wheel_base = vehicle_info.wheel_base_m, wheel_tread = vehicle_info.wheel_tread_m;
+  const double side_overhang =
+    left_side ? vehicle_info.left_overhang_m : vehicle_info.right_overhang_m;
+  const double ego_length_to_front = wheel_base + front_overhang;
+  const double ego_width_to_front =
+    !left_side ? (-wheel_tread / 2.0 - side_overhang) : (wheel_tread / 2.0 + side_overhang);
+  tier4_autoware_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front};
+  const auto front_edge_glob = tier4_autoware_utils::transformPoint(
+    front_edge_local, tier4_autoware_utils::pose2transform(ego_pose));
+  geometry_msgs::msg::Pose ego_front_pose;
+  ego_front_pose.position =
+    createPoint(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z);
+
+  // ==========================================================================================
+  // NOTE: the point which is on the right side of a directed line has negative distance
+  // getExpandedLanelet(1.0, -2.0) expands a lanelet by 1.0 to the left and by 2.0 to the right
+  // ==========================================================================================
+  const double ego_offset_to_closer_boundary =
+    getOffsetToLanesBoundary(pull_over_lanes, ego_front_pose, left_side);
+  return left_side ? lanelet::utils::getExpandedLanelets(
+                       pull_over_lanes, outer_road_offset,
+                       ego_offset_to_closer_boundary - inner_road_offset)
+                   : lanelet::utils::getExpandedLanelets(
+                       pull_over_lanes, ego_offset_to_closer_boundary + inner_road_offset,
+                       -outer_road_offset);
 }
 
 PredictedObjects extractObjectsInExpandedPullOverLanes(
@@ -233,6 +283,21 @@ MarkerArray createGoalCandidatesMarkerArray(
   return marker_array;
 }
 
+MarkerArray createLaneletPolygonMarkerArray(
+  const lanelet::CompoundPolygon3d & polygon, const std_msgs::msg::Header & header,
+  const std::string & ns, const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::MarkerArray marker_array{};
+  auto marker = createDefaultMarker(
+    header.frame_id, header.stamp, ns, 0, visualization_msgs::msg::Marker::LINE_STRIP,
+    createMarkerScale(0.1, 0.0, 0.0), color);
+  for (const auto & p : polygon) {
+    marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+  }
+  marker_array.markers.push_back(marker);
+  return marker_array;
+}
+
 double calcLateralDeviationBetweenPaths(
   const PathWithLaneId & reference_path, const PathWithLaneId & target_path)
 {