fix(behavior_velocity_planner): use common function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_velocity_planner/config/stop_line.param.yaml

@@ -2,8 +2,6 @@
   ros__parameters:
     stop_line:
       stop_margin: 0.0
-      stop_check_dist: 2.0
       stop_duration_sec: 1.0
+      hold_stop_margin_distance: 2.0
       use_initialization_stop_line_state: true
-      debug:
-        show_stopline_collision_check: false            # [-] whether to show stopline collision

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

@@ -27,7 +27,6 @@
 #include <lanelet2_extension/utility/query.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/scene_module_interface.hpp>
-#include <utilization/boost_geometry_helper.hpp>
 #include <utilization/util.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
@@ -42,30 +41,21 @@ using tier4_planning_msgs::msg::StopReason;
 
 class StopLineModule : public SceneModuleInterface
 {
-  using StopLineWithLaneId = std::pair<lanelet::ConstLineString3d, int64_t>;
-
 public:
   enum class State { APPROACH, STOPPED, START };
 
-  struct SegmentIndexWithPoint2d
-  {
-    size_t index;
-    Point2d point;
-  };
-
   struct DebugData
   {
     double base_link2front;
     boost::optional<geometry_msgs::msg::Pose> stop_pose;
-    std::vector<LineString2d> search_segments;
     LineString2d search_stopline;
   };
 
   struct PlannerParam
   {
     double stop_margin;
-    double stop_check_dist;
     double stop_duration_sec;
+    double hold_stop_margin_distance;
     bool use_initialization_stop_line_state;
   };
 
@@ -83,10 +73,6 @@ class StopLineModule : public SceneModuleInterface
 private:
   int64_t module_id_;
 
-  boost::optional<StopLineModule::SegmentIndexWithPoint2d> findCollision(
-    const PathWithLaneId & path, const LineString2d & stop_line,
-    const SearchRangeIndex & search_index);
-
   void insertStopPoint(const geometry_msgs::msg::Point & stop_point, PathWithLaneId & path) const;
 
   std::shared_ptr<const rclcpp::Time> stopped_time_;

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

@@ -39,18 +39,6 @@ MarkerArray StopLineModule::createDebugMarkerArray()
   MarkerArray msg;
 
   const auto now = this->clock_->now();
-  // Search Segments
-  {
-    auto marker = createDefaultMarker(
-      "map", now, "search_segments", module_id_, Marker::SPHERE_LIST,
-      createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 0.0, 1.0, 0.999));
-
-    for (const auto & e : debug_data_.search_segments) {
-      marker.points.push_back(createPoint(e.front().x(), e.front().y(), 0.0));
-      marker.points.push_back(createPoint(e.back().x(), e.back().y(), 0.0));
-    }
-    msg.markers.push_back(marker);
-  }
 
   // Search stopline
   {
@@ -59,8 +47,11 @@ MarkerArray StopLineModule::createDebugMarkerArray()
       createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 0.0, 1.0, 0.999));
 
     const auto line = debug_data_.search_stopline;
-    marker.points.push_back(createPoint(line.front().x(), line.front().y(), 0.0));
-    marker.points.push_back(createPoint(line.back().x(), line.back().y(), 0.0));
+    if (!line.empty()) {
+      marker.points.push_back(createPoint(line.front().x(), line.front().y(), 0.0));
+      marker.points.push_back(createPoint(line.back().x(), line.back().y(), 0.0));
+    }
+
     msg.markers.push_back(marker);
   }
 

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

@@ -29,8 +29,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node)
   const std::string ns(getModuleName());
   auto & p = planner_param_;
   p.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0);
-  p.stop_check_dist = node.declare_parameter(ns + ".stop_check_dist", 2.0);
   p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0);
+  p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance", 2.0);
   p.use_initialization_stop_line_state =
     node.declare_parameter(ns + ".use_initialization_stop_line_state", false);
 }

planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp

@@ -23,6 +23,8 @@ namespace behavior_velocity_planner
 {
 
 namespace bg = boost::geometry;
+using Point = bg::model::d2::point_xy<double>;
+using Line = bg::model::linestring<Point>;
 using motion_utils::calcLongitudinalOffsetPoint;
 using motion_utils::calcLongitudinalOffsetPose;
 using motion_utils::calcSignedArcLength;
@@ -31,6 +33,53 @@ using tier4_autoware_utils::createPoint;
 using tier4_autoware_utils::getPoint;
 using tier4_autoware_utils::getPose;
 
+namespace
+{
+std::vector<Point> getLinestringIntersects(
+  const PathWithLaneId & ego_path, const Line & linestring,
+  const geometry_msgs::msg::Point & ego_pos,
+  const size_t max_num = std::numeric_limits<size_t>::max())
+{
+  std::vector<Point> intersects{};
+
+  bool found_max_num = false;
+  for (size_t i = 0; i < ego_path.points.size() - 1; ++i) {
+    const auto & p_back = ego_path.points.at(i).point.pose.position;
+    const auto & p_front = ego_path.points.at(i + 1).point.pose.position;
+    const Line segment{{p_back.x, p_back.y}, {p_front.x, p_front.y}};
+
+    std::vector<Point> tmp_intersects{};
+    bg::intersection(segment, linestring, tmp_intersects);
+
+    for (const auto & p : tmp_intersects) {
+      intersects.push_back(p);
+      if (intersects.size() == max_num) {
+        found_max_num = true;
+        break;
+      }
+    }
+
+    if (found_max_num) {
+      break;
+    }
+  }
+
+  const auto compare = [&](const Point & p1, const Point & p2) {
+    const auto dist_l1 =
+      calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z));
+
+    const auto dist_l2 =
+      calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z));
+
+    return dist_l1 < dist_l2;
+  };
+
+  std::sort(intersects.begin(), intersects.end(), compare);
+
+  return intersects;
+}
+}  // namespace
+
 StopLineModule::StopLineModule(
   const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
   const PlannerParam & planner_param, const rclcpp::Logger logger,
@@ -44,45 +93,6 @@ StopLineModule::StopLineModule(
   planner_param_ = planner_param;
 }
 
-boost::optional<StopLineModule::SegmentIndexWithPoint2d> StopLineModule::findCollision(
-  const PathWithLaneId & path, const LineString2d & stop_line,
-  const SearchRangeIndex & search_index)
-{
-  const size_t min_search_index = std::max(static_cast<size_t>(0), search_index.min_idx);
-  const size_t max_search_index = std::min(search_index.max_idx, path.points.size() - 1);
-
-  // for creating debug marker
-  debug_data_.search_stopline = stop_line;
-  for (size_t i = min_search_index; i < max_search_index; ++i) {
-    const auto & p_front = path.points.at(i).point.pose.position;
-    const auto & p_back = path.points.at(i + 1).point.pose.position;
-    const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}};
-    debug_data_.search_segments.push_back(path_segment);
-  }
-
-  for (size_t i = min_search_index; i < max_search_index; ++i) {
-    const auto & p_front = path.points.at(i).point.pose.position;
-    const auto & p_back = path.points.at(i + 1).point.pose.position;
-
-    // Find intersection
-    const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}};
-    std::vector<Point2d> collision_points;
-    bg::intersection(stop_line, path_segment, collision_points);
-
-    // Ignore if no collision found
-    if (collision_points.empty()) {
-      continue;
-    }
-
-    // Select first collision
-    const auto & collision_point = collision_points.at(0);
-
-    return StopLineModule::SegmentIndexWithPoint2d{i, collision_point};
-  }
-
-  return {};
-}
-
 bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
 {
   const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
@@ -93,26 +103,18 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
   const auto ego_path = *path;
   const auto & ego_pos = planner_data_->current_pose.pose.position;
 
-  const LineString2d stop_line = planning_utils::extendLine(
+  const auto stop_line = planning_utils::extendLine(
     stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length);
-  SearchRangeIndex dst_search_range =
-    planning_utils::getPathIndexRangeIncludeLaneId(ego_path, lane_id_);
 
-  // extend following and previous search range to avoid no collision
-  if (dst_search_range.max_idx < ego_path.points.size() - 1) dst_search_range.max_idx++;
-  if (dst_search_range.min_idx > 0) dst_search_range.min_idx--;
+  debug_data_.search_stopline = stop_line;
 
-  // Find collision
-  const auto collision = findCollision(ego_path, stop_line, dst_search_range);
+  const auto intersects = getLinestringIntersects(ego_path, stop_line, ego_pos, 1);
 
-  // If no collision found, do nothing
-  if (!collision) {
-    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision");
-    return true;
+  if (intersects.empty()) {
+    return false;
   }
 
-  const auto p_stop_line =
-    createPoint(collision.get().point.x(), collision.get().point.y(), ego_pos.z);
+  const auto p_stop_line = createPoint(intersects.front().x(), intersects.front().y(), ego_pos.z);
   const auto margin = planner_param_.stop_margin + base_link2front;
   const auto stop_pose = calcLongitudinalOffsetPose(ego_path.points, p_stop_line, -margin);
 
@@ -141,14 +143,14 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
       debug_data_.stop_pose = stop_pose.get();
 
       if (
-        signed_arc_dist_to_stop_point < planner_param_.stop_check_dist &&
+        signed_arc_dist_to_stop_point < planner_param_.hold_stop_margin_distance &&
         planner_data_->isVehicleStopped()) {
         RCLCPP_INFO(logger_, "APPROACH -> STOPPED");
 
         state_ = State::STOPPED;
         stopped_time_ = std::make_shared<const rclcpp::Time>(clock_->now());
 
-        if (signed_arc_dist_to_stop_point < -planner_param_.stop_check_dist) {
+        if (signed_arc_dist_to_stop_point < -planner_param_.hold_stop_margin_distance) {
           RCLCPP_ERROR(
             logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED");
         }
@@ -182,7 +184,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
     case State::START: {
       // Initialize if vehicle is far from stop_line
       if (planner_param_.use_initialization_stop_line_state) {
-        if (signed_arc_dist_to_stop_point > planner_param_.stop_check_dist) {
+        if (signed_arc_dist_to_stop_point > planner_param_.hold_stop_margin_distance) {
           RCLCPP_INFO(logger_, "START -> APPROACH");
           state_ = State::APPROACH;
         }