feat(intersection): add conflicting area with margin debug (#1021)

* add detection area margin debug

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* extention lanelet in intersection function

* feat: add conflicting area with margin

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* fix(intersection_module): remove unnecessary comment

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* fix check collision

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* fix(intersection_module): remove unnecessary diff

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* ci(pre-commit): autofix

* fix(intersection_module): fix expand lane only right bound

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* fix(intersection_module): remove calc of detection area to object distance

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* ci(pre-commit): autofix

* fix(intersection_module): split lane extentions

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* ci(pre-commit): autofix

* refactor: lanelet::utils::resamplePoints -> resamplePoints

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* feat: add right and left margin parameter

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml

@@ -10,6 +10,8 @@
       intersection_velocity: 2.778 # 2.778m/s = 10.0km/h
       intersection_max_accel: 0.5 # m/ss
       detection_area_margin: 0.5 # [m]
+      detection_area_right_margin: 0.5 # [m]
+      detection_area_left_margin: 0.5 # [m]
       detection_area_length: 200.0 # [m]
       min_predicted_path_confidence: 0.05
       collision_start_margin_time: 5.0 # [s]

map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp

@@ -35,6 +35,10 @@ lanelet::LineString3d generateFineCenterline(
   const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0);
 lanelet::ConstLineString3d getCenterlineWithOffset(
   const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0);
+lanelet::ConstLineString3d getRightBoundWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0);
+lanelet::ConstLineString3d getLeftBoundWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0);
 
 lanelet::ConstLanelet getExpandedLanelet(
   const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset);

map/lanelet2_extension/lib/utilities.cpp

@@ -282,7 +282,7 @@ lanelet::ConstLineString3d getCenterlineWithOffset(
   const int num_segments = std::max(static_cast<int>(ceil(longer_distance / resolution)), 1);
 
   // Resample points
-  const auto left_points = lanelet::utils::resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
   const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
 
   // Create centerline
@@ -303,6 +303,65 @@ lanelet::ConstLineString3d getCenterlineWithOffset(
   return static_cast<lanelet::ConstLineString3d>(centerline);
 }
 
+lanelet::ConstLineString3d getRightBoundWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution)
+{
+  // Get length of longer border
+  const double left_length = lanelet::geometry::length(lanelet_obj.leftBound());
+  const double right_length = lanelet::geometry::length(lanelet_obj.rightBound());
+  const double longer_distance = (left_length > right_length) ? left_length : right_length;
+  const int num_segments = std::max(static_cast<int>(ceil(longer_distance / resolution)), 1);
+
+  // Resample points
+  const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
+
+  // Create centerline
+  lanelet::LineString3d rightBound(lanelet::utils::getId());
+  for (int i = 0; i < num_segments + 1; i++) {
+    // Add ID for the average point of left and right
+    const auto vec_left_2_right = (right_points.at(i) - left_points.at(i)).normalized();
+
+    const auto offset_right_basic_point = right_points.at(i) + vec_left_2_right * offset;
+
+    const lanelet::Point3d rightBound_point(
+      lanelet::utils::getId(), offset_right_basic_point.x(), offset_right_basic_point.y(),
+      offset_right_basic_point.z());
+    rightBound.push_back(rightBound_point);
+  }
+  return static_cast<lanelet::ConstLineString3d>(rightBound);
+}
+
+lanelet::ConstLineString3d getLeftBoundWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution)
+{
+  // Get length of longer border
+  const double left_length = lanelet::geometry::length(lanelet_obj.leftBound());
+  const double right_length = lanelet::geometry::length(lanelet_obj.rightBound());
+  const double longer_distance = (left_length > right_length) ? left_length : right_length;
+  const int num_segments = std::max(static_cast<int>(ceil(longer_distance / resolution)), 1);
+
+  // Resample points
+  const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
+
+  // Create centerline
+  lanelet::LineString3d leftBound(lanelet::utils::getId());
+  for (int i = 0; i < num_segments + 1; i++) {
+    // Add ID for the average point of left and right
+
+    const auto vec_right_2_left = (left_points.at(i) - right_points.at(i)).normalized();
+
+    const auto offset_left_basic_point = left_points.at(i) + vec_right_2_left * offset;
+
+    const lanelet::Point3d leftBound_point(
+      lanelet::utils::getId(), offset_left_basic_point.x(), offset_left_basic_point.y(),
+      offset_left_basic_point.z());
+    leftBound.push_back(leftBound_point);
+  }
+  return static_cast<lanelet::ConstLineString3d>(leftBound);
+}
+
 lanelet::ConstLanelet getExpandedLanelet(
   const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset)
 {

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -10,6 +10,8 @@
       intersection_velocity: 2.778 # 2.778m/s = 10.0km/h
       intersection_max_accel: 0.5 # m/ss
       detection_area_margin: 0.5 # [m]
+      detection_area_right_margin: 0.5 # [m]
+      detection_area_left_margin: 0.5 # [m]
       detection_area_length: 200.0 # [m]
       min_predicted_path_confidence: 0.05
       collision_start_margin_time: 5.0 # [s]

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -89,6 +89,7 @@ class IntersectionModule : public SceneModuleInterface
     geometry_msgs::msg::Pose stop_point_pose;
     geometry_msgs::msg::Pose judge_point_pose;
     geometry_msgs::msg::Polygon ego_lane_polygon;
+    std::vector<lanelet::CompoundPolygon3d> detection_area_with_margin;
     geometry_msgs::msg::Polygon stuck_vehicle_detect_area;
     geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon;
     std::vector<geometry_msgs::msg::Polygon> candidate_collision_object_polygons;
@@ -111,8 +112,12 @@ class IntersectionModule : public SceneModuleInterface
     double intersection_velocity;  //! used for intersection passing time
     double intersection_max_acc;   //! used for calculating intersection velocity
     double detection_area_margin;  //! used for detecting objects in detection area
-    double detection_area_length;  //! used to create detection area polygon
-    double detection_area_angle_thr;  //! threshold in checking the angle of detecting objects
+    double detection_area_right_margin;  //! used for detecting objects in detection area only right
+                                         //! direction
+    double detection_area_left_margin;   //! used for detecting objects in detection area only left
+                                         //! direction
+    double detection_area_length;        //! used to create detection area polygon
+    double detection_area_angle_thr;     //! threshold in checking the angle of detecting objects
     double min_predicted_path_confidence;
     //! minimum confidence value of predicted path to use for collision detection
     double external_input_timeout;       //! used to disable external input
@@ -149,7 +154,6 @@ class IntersectionModule : public SceneModuleInterface
    * actual collision check algorithm inside this function)
    * @param lanelet_map_ptr  lanelet map
    * @param path             ego-car lane
-   * @param detection_areas  collision check is performed for vehicles that exist in this area
    * @param detection_area_lanelet_ids  angle check is performed for obstacles using this lanelet
    * ids
    * @param objects_ptr      target objects
@@ -159,7 +163,6 @@ class IntersectionModule : public SceneModuleInterface
   bool checkCollision(
     lanelet::LaneletMapConstPtr lanelet_map_ptr,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-    const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
     const std::vector<int> & detection_area_lanelet_ids,
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
     const int closest_idx);

planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp

@@ -101,6 +101,8 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
   {
     double decel_velocity;
     double detection_area_length;
+    double detection_area_right_margin;
+    double detection_area_left_margin;
     double stop_line_margin;
     double stop_duration_sec;
   };

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -56,9 +56,11 @@ bool hasDuplicatedPoint(
  */
 bool getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id, const double detection_area_length,
-  std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
-  std::vector<lanelet::ConstLanelets> * objective_lanelets_result, const rclcpp::Logger logger);
+  const int lane_id, const double detection_area_length, const double right_margin,
+  const double left_margin, std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
+  std::vector<lanelet::ConstLanelets> * objective_lanelets_result,
+  std::vector<lanelet::ConstLanelets> * objective_lanelets_with_margin_result,
+  const rclcpp::Logger logger);
 
 /**
  * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map,
@@ -107,6 +109,8 @@ double calcArcLengthFromPath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const size_t src_idx,
   const size_t dst_idx);
 
+lanelet::ConstLanelet generateOffsetLanelet(
+  const lanelet::ConstLanelet lanelet, double right_margin, double left_margin);
 }  // namespace util
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp

@@ -203,6 +203,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
     createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", lane_id_),
     current_time, &debug_marker_array);
 
+  appendMarkerArray(
+    createLaneletPolygonsMarkerArray(
+      debug_data_.detection_area_with_margin, "detection_area_with_margin", lane_id_),
+    current_time, &debug_marker_array);
+
   appendMarkerArray(
     createPolygonMarkerArray(debug_data_.ego_lane_polygon, "ego_lane", lane_id_, 0.0, 0.3, 0.7),
     current_time, &debug_marker_array);

planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp

@@ -75,6 +75,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.intersection_velocity = node.declare_parameter(ns + ".intersection_velocity", 10.0 / 3.6);
   ip.intersection_max_acc = node.declare_parameter(ns + ".intersection_max_accel", 0.5);
   ip.detection_area_margin = node.declare_parameter(ns + ".detection_area_margin", 0.5);
+  ip.detection_area_right_margin = node.declare_parameter(ns + ".detection_area_right_margin", 0.5);
+  ip.detection_area_left_margin = node.declare_parameter(ns + ".detection_area_left_margin", 0.5);
   ip.detection_area_length = node.declare_parameter(ns + ".detection_area_length", 200.0);
   ip.detection_area_angle_thr =
     node.declare_parameter(ns + ".detection_area_angle_threshold", M_PI / 4.0);

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -92,14 +92,20 @@ bool IntersectionModule::modifyPathVelocity(
   /* get detection area and conflicting area */
   std::vector<lanelet::ConstLanelets> detection_area_lanelets;
   std::vector<lanelet::ConstLanelets> conflicting_area_lanelets;
+  std::vector<lanelet::ConstLanelets> detection_area_lanelets_with_margin;
 
   util::getObjectiveLanelets(
     lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    &conflicting_area_lanelets, &detection_area_lanelets, logger_);
+    planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin,
+    &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin,
+    logger_);
   std::vector<lanelet::CompoundPolygon3d> conflicting_areas = util::getPolygon3dFromLaneletsVec(
     conflicting_area_lanelets, planner_param_.detection_area_length);
   std::vector<lanelet::CompoundPolygon3d> detection_areas = util::getPolygon3dFromLaneletsVec(
     detection_area_lanelets, planner_param_.detection_area_length);
+  std::vector<lanelet::CompoundPolygon3d> detection_areas_with_margin =
+    util::getPolygon3dFromLaneletsVec(
+      detection_area_lanelets_with_margin, planner_param_.detection_area_length);
   std::vector<int> conflicting_area_lanelet_ids =
     util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets);
   std::vector<int> detection_area_lanelet_ids =
@@ -110,6 +116,7 @@ bool IntersectionModule::modifyPathVelocity(
     return true;
   }
   debug_data_.detection_area = detection_areas;
+  debug_data_.detection_area_with_margin = detection_areas_with_margin;
 
   /* set stop-line and stop-judgement-line for base_link */
   int stop_line_idx = -1;
@@ -153,8 +160,8 @@ bool IntersectionModule::modifyPathVelocity(
   const auto objects_ptr = planner_data_->predicted_objects;
 
   /* calculate dynamic collision around detection area */
-  bool has_collision = checkCollision(
-    lanelet_map_ptr, *path, detection_areas, detection_area_lanelet_ids, objects_ptr, closest_idx);
+  bool has_collision =
+    checkCollision(lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx);
   bool is_stuck = checkStuckVehicleInIntersection(
     lanelet_map_ptr, *path, closest_idx, stop_line_idx, objects_ptr);
   bool is_entry_prohibited = (has_collision || is_stuck);
@@ -226,7 +233,6 @@ void IntersectionModule::cutPredictPathWithDuration(
 bool IntersectionModule::checkCollision(
   lanelet::LaneletMapConstPtr lanelet_map_ptr,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
   const std::vector<int> & detection_area_lanelet_ids,
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
   const int closest_idx)
@@ -256,24 +262,11 @@ bool IntersectionModule::checkCollision(
       continue;  // TODO(Kenji Miyake): check direction?
     }
 
-    // keep vehicle in detection_area
-    const Point2d obj_point(
-      object.kinematics.initial_pose_with_covariance.pose.position.x,
-      object.kinematics.initial_pose_with_covariance.pose.position.y);
-    for (const auto & detection_area : detection_areas) {
-      const auto detection_poly = lanelet::utils::to2D(detection_area).basicPolygon();
-      const double dist_to_detection_area =
-        boost::geometry::distance(obj_point, toBoostPoly(detection_poly));
-      if (dist_to_detection_area > planner_param_.detection_area_margin) {
-        // ignore the object far from detection area
-        continue;
-      }
-      // check direction of objects
-      const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics);
-      if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) {
-        target_objects.objects.push_back(object);
-        break;
-      }
+    // check direction of objects
+    const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics);
+    if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) {
+      target_objects.objects.push_back(object);
+      break;
     }
   }
 

planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp

@@ -67,10 +67,13 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   /* get detection area */
   std::vector<lanelet::ConstLanelets> detection_area_lanelets;
   std::vector<lanelet::ConstLanelets> conflicting_area_lanelets;
+  std::vector<lanelet::ConstLanelets> detection_area_lanelets_with_margin;
 
   util::getObjectiveLanelets(
     lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    &conflicting_area_lanelets, &detection_area_lanelets, logger_);
+    planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin,
+    &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin,
+    logger_);
   std::vector<lanelet::CompoundPolygon3d> conflicting_areas = util::getPolygon3dFromLaneletsVec(
     conflicting_area_lanelets, planner_param_.detection_area_length);
   if (conflicting_areas.empty()) {

planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp

@@ -322,9 +322,11 @@ bool getStopPoseIndexFromMap(
 
 bool getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id, const double detection_area_length,
+  const int lane_id, const double detection_area_length, double right_margin, double left_margin,
   std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
-  std::vector<lanelet::ConstLanelets> * objective_lanelets_result, const rclcpp::Logger logger)
+  std::vector<lanelet::ConstLanelets> * objective_lanelets_result,
+  std::vector<lanelet::ConstLanelets> * objective_lanelets_with_margin_result,
+  const rclcpp::Logger logger)
 {
   const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id);
 
@@ -365,6 +367,7 @@ bool getObjectiveLanelets(
   std::vector<lanelet::ConstLanelets>                      // conflicting lanes with "lane_id"
     conflicting_lanelets_ex_yield_ego;                     // excluding ego lanes and yield lanes
   std::vector<lanelet::ConstLanelets> objective_lanelets;  // final objective lanelets
+  std::vector<lanelet::ConstLanelets> objective_lanelets_with_margin;  // final objective lanelets
 
   // exclude yield lanelets and ego lanelets from objective_lanelets
   for (const auto & conflicting_lanelet : conflicting_lanelets) {
@@ -374,8 +377,11 @@ bool getObjectiveLanelets(
     if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) {
       continue;
     }
+    const auto objective_lanelet_with_margin =
+      generateOffsetLanelet(conflicting_lanelet, right_margin, left_margin);
     conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet});
     objective_lanelets.push_back({conflicting_lanelet});
+    objective_lanelets_with_margin.push_back({objective_lanelet_with_margin});
   }
 
   // get possible lanelet path that reaches conflicting_lane longer than given length
@@ -395,6 +401,7 @@ bool getObjectiveLanelets(
 
   *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego;
   *objective_lanelets_result = objective_lanelets_sequences;
+  *objective_lanelets_with_margin_result = objective_lanelets_with_margin;
 
   std::stringstream ss_c, ss_y, ss_e, ss_o, ss_os;
   for (const auto & l : conflicting_lanelets) {
@@ -462,5 +469,32 @@ double calcArcLengthFromPath(
   return length;
 }
 
+lanelet::ConstLanelet generateOffsetLanelet(
+  const lanelet::ConstLanelet lanelet, double right_margin, double left_margin)
+{
+  lanelet::Points3d lefts, rights;
+
+  const double right_offset = right_margin;
+  const double left_offset = left_margin;
+  const auto offset_rightBound = lanelet::utils::getRightBoundWithOffset(lanelet, right_offset);
+  const auto offset_leftBound = lanelet::utils::getLeftBoundWithOffset(lanelet, left_offset);
+
+  const auto original_left_bound = offset_leftBound;
+  const auto original_right_bound = offset_rightBound;
+
+  for (const auto & pt : original_left_bound) {
+    lefts.push_back(lanelet::Point3d(pt));
+  }
+  for (const auto & pt : original_right_bound) {
+    rights.push_back(lanelet::Point3d(pt));
+  }
+  const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts);
+  const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights);
+  auto lanelet_with_margin = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
+  const auto centerline = lanelet::utils::generateFineCenterline(lanelet_with_margin, 5.0);
+  lanelet_with_margin.setCenterline(centerline);
+  return std::move(lanelet_with_margin);
+}
+
 }  // namespace util
 }  // namespace behavior_velocity_planner