revert(behavior_velocity_planner): keep stopping in hold stop margin …

…distance (tier4#1433) (autowarefoundation#1645)

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

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

planning/behavior_velocity_planner/config/stop_line.param.yaml

@@ -2,6 +2,8 @@
   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/manager.hpp

@@ -29,8 +29,6 @@
 
 namespace behavior_velocity_planner
 {
-
-using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using StopLineWithLaneId = std::pair<lanelet::ConstLineString3d, int64_t>;
 
 class StopLineModuleManager : public SceneModuleManagerInterface
@@ -44,15 +42,17 @@ class StopLineModuleManager : public SceneModuleManagerInterface
   StopLineModule::PlannerParam planner_param_;
 
   std::vector<StopLineWithLaneId> getStopLinesWithLaneIdOnPath(
-    const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map);
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const lanelet::LaneletMapPtr lanelet_map);
 
   std::set<int64_t> getStopLineIdSetOnPath(
-    const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map);
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const lanelet::LaneletMapPtr lanelet_map);
 
-  void launchNewModules(const PathWithLaneId & path) override;
+  void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
-    const PathWithLaneId & path) override;
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 };
 }  // namespace behavior_velocity_planner
 

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

@@ -15,7 +15,6 @@
 #ifndef SCENE_MODULE__STOP_LINE__SCENE_HPP_
 #define SCENE_MODULE__STOP_LINE__SCENE_HPP_
 
-#include <memory>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -27,36 +26,54 @@
 #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>
 #include <lanelet2_routing/RoutingGraph.h>
 
 namespace behavior_velocity_planner
 {
-
-using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using tier4_planning_msgs::msg::StopFactor;
-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 SegmentIndexWithPose
+  {
+    size_t index;
+    geometry_msgs::msg::Pose pose;
+  };
+
+  struct SegmentIndexWithPoint2d
+  {
+    size_t index;
+    Point2d point;
+  };
+
+  struct SegmentIndexWithOffset
+  {
+    size_t index;
+    double offset;
+  };
+
   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;
+    bool show_stopline_collision_check;
   };
 
 public:
@@ -65,21 +82,37 @@ class StopLineModule : public SceneModuleInterface
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
-  bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
+  bool modifyPathVelocity(
+    autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+    tier4_planning_msgs::msg::StopReason * stop_reason) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
 
 private:
   int64_t module_id_;
 
-  std::shared_ptr<const rclcpp::Time> stopped_time_;
+  geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line);
 
-  lanelet::ConstLineString3d stop_line_;
+  boost::optional<StopLineModule::SegmentIndexWithPoint2d> findCollision(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
+    const SearchRangeIndex & search_index);
 
-  int64_t lane_id_;
+  boost::optional<StopLineModule::SegmentIndexWithOffset> findOffsetSegment(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const StopLineModule::SegmentIndexWithPoint2d & collision);
 
-  // State machine
+  boost::optional<StopLineModule::SegmentIndexWithPose> calcStopPose(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const boost::optional<StopLineModule::SegmentIndexWithOffset> & offset_segment);
+
+  autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const StopLineModule::SegmentIndexWithPose & insert_index_with_pose,
+    tier4_planning_msgs::msg::StopReason * stop_reason);
+
+  lanelet::ConstLineString3d stop_line_;
+  int64_t lane_id_;
   State state_;
 
   // Parameter

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

@@ -24,53 +24,90 @@
 
 namespace behavior_velocity_planner
 {
-
-using motion_utils::createStopVirtualWallMarker;
 using tier4_autoware_utils::appendMarkerArray;
-using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
-using tier4_autoware_utils::createPoint;
-using visualization_msgs::msg::Marker;
-using visualization_msgs::msg::MarkerArray;
 
-MarkerArray StopLineModule::createDebugMarkerArray()
+namespace
 {
-  MarkerArray msg;
+using DebugData = StopLineModule::DebugData;
 
-  const auto now = this->clock_->now();
+visualization_msgs::msg::MarkerArray createStopLineCollisionCheck(
+  const DebugData & debug_data, const int64_t module_id)
+{
+  visualization_msgs::msg::MarkerArray msg;
 
-  // Search stopline
+  // Search Segments
   {
-    auto marker = createDefaultMarker(
-      "map", now, "search_stopline", module_id_, Marker::LINE_STRIP,
-      createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 0.0, 1.0, 0.999));
-
-    const auto line = debug_data_.search_stopline;
-    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));
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = "map";
+    marker.ns = "search_segments";
+    marker.id = module_id;
+    marker.lifetime = rclcpp::Duration::from_seconds(0.5);
+    marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    for (const auto & e : debug_data.search_segments) {
+      marker.points.push_back(
+        geometry_msgs::build<geometry_msgs::msg::Point>().x(e.at(0).x()).y(e.at(0).y()).z(0.0));
+      marker.points.push_back(
+        geometry_msgs::build<geometry_msgs::msg::Point>().x(e.at(1).x()).y(e.at(1).y()).z(0.0));
     }
+    marker.scale = createMarkerScale(0.1, 0.1, 0.1);
+    marker.color = createMarkerColor(0.0, 0.0, 1.0, 0.999);
+    msg.markers.push_back(marker);
+  }
+
+  // Search stopline
+  {
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = "map";
+    marker.ns = "search_stopline";
+    marker.id = module_id;
+    marker.lifetime = rclcpp::Duration::from_seconds(0.5);
+    marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    const auto p0 = debug_data.search_stopline.at(0);
+    marker.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point>().x(p0.x()).y(p0.y()).z(0.0));
+    const auto p1 = debug_data.search_stopline.at(1);
+    marker.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point>().x(p1.x()).y(p1.y()).z(0.0));
 
+    marker.scale = createMarkerScale(0.1, 0.1, 0.1);
+    marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.999);
     msg.markers.push_back(marker);
   }
 
   return msg;
 }
 
-MarkerArray StopLineModule::createVirtualWallMarkerArray()
+}  // namespace
+
+visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray()
 {
-  MarkerArray wall_marker;
+  visualization_msgs::msg::MarkerArray debug_marker_array;
+  if (planner_param_.show_stopline_collision_check) {
+    appendMarkerArray(
+      createStopLineCollisionCheck(debug_data_, module_id_), &debug_marker_array,
+      this->clock_->now());
+  }
+  return debug_marker_array;
+}
 
+visualization_msgs::msg::MarkerArray StopLineModule::createVirtualWallMarkerArray()
+{
+  const auto now = this->clock_->now();
+  visualization_msgs::msg::MarkerArray wall_marker;
   if (!debug_data_.stop_pose) {
     return wall_marker;
   }
-
-  const auto now = this->clock_->now();
-
-  const auto p = calcOffsetPose(*debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0);
-  appendMarkerArray(createStopVirtualWallMarker(p, "stopline", now, module_id_), &wall_marker);
-
+  const auto p_front = tier4_autoware_utils::calcOffsetPose(
+    *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0);
+  if (state_ == State::APPROACH) {
+    appendMarkerArray(
+      motion_utils::createStopVirtualWallMarker(p_front, "stopline", now, module_id_), &wall_marker,
+      now);
+  }
   return wall_marker;
 }
 }  // namespace behavior_velocity_planner

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

@@ -29,14 +29,18 @@ 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);
+  // debug
+  p.show_stopline_collision_check =
+    node.declare_parameter(ns + ".debug.show_stopline_collision_check", false);
 }
 
 std::vector<StopLineWithLaneId> StopLineModuleManager::getStopLinesWithLaneIdOnPath(
-  const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map)
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map)
 {
   std::vector<StopLineWithLaneId> stop_lines_with_lane_id;
 
@@ -58,7 +62,8 @@ std::vector<StopLineWithLaneId> StopLineModuleManager::getStopLinesWithLaneIdOnP
 }
 
 std::set<int64_t> StopLineModuleManager::getStopLineIdSetOnPath(
-  const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map)
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map)
 {
   std::set<int64_t> stop_line_id_set;
 
@@ -69,7 +74,8 @@ std::set<int64_t> StopLineModuleManager::getStopLineIdSetOnPath(
   return stop_line_id_set;
 }
 
-void StopLineModuleManager::launchNewModules(const PathWithLaneId & path)
+void StopLineModuleManager::launchNewModules(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
   for (const auto & stop_line_with_lane_id :
        getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
@@ -84,7 +90,8 @@ void StopLineModuleManager::launchNewModules(const PathWithLaneId & path)
 }
 
 std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
-StopLineModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
+StopLineModuleManager::getModuleExpiredFunction(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
   const auto stop_line_id_set =
     getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr());