feat(behavior_velocity_planner): keep stopping in hold stop margin distance (backport #1433)

planning/behavior_velocity_planner/config/stop_line.param.yaml

@@ -2,6 +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

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

@@ -26,6 +26,9 @@
 
 namespace behavior_velocity_planner
 {
+
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+
 class StopLineModuleManager : public SceneModuleManagerInterface
 {
 public:
@@ -35,10 +38,11 @@ class StopLineModuleManager : public SceneModuleManagerInterface
 
 private:
   StopLineModule::PlannerParam planner_param_;
-  void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
+
+  void launchNewModules(const PathWithLaneId & path) override;
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
+    const PathWithLaneId & path) override;
 };
 }  // namespace behavior_velocity_planner
 

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

@@ -15,6 +15,7 @@
 #ifndef SCENE_MODULE__STOP_LINE__SCENE_HPP_
 #define SCENE_MODULE__STOP_LINE__SCENE_HPP_
 
+#include <memory>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -26,50 +27,35 @@
 #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;
+    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;
   };
 
@@ -79,36 +65,22 @@ class StopLineModule : public SceneModuleInterface
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
-  bool modifyPathVelocity(
-    autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+  bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
 
 private:
   int64_t module_id_;
 
-  geometry_msgs::msg::Point getCenterOfStopLine(const 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);
+  void insertStopPoint(const geometry_msgs::msg::Point & stop_point, PathWithLaneId & path) const;
 
-  boost::optional<StopLineModule::SegmentIndexWithOffset> findOffsetSegment(
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-    const StopLineModule::SegmentIndexWithPoint2d & collision);
-
-  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);
+  std::shared_ptr<const rclcpp::Time> stopped_time_;
 
   lanelet::ConstLineString3d stop_line_;
+
   int64_t lane_id_;
+
+  // State machine
   State state_;
 
   // Parameter

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

@@ -13,85 +13,64 @@
 // limitations under the License.
 
 #include <scene_module/stop_line/scene.hpp>
-#include <utilization/marker_helper.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <utilization/util.hpp>
 
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
 
 namespace behavior_velocity_planner
 {
-namespace
-{
-using DebugData = StopLineModule::DebugData;
 
-visualization_msgs::msg::MarkerArray createMarkers(
-  const DebugData & debug_data, const int64_t module_id)
+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 tier4_autoware_utils::createStopVirtualWallMarker;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+
+MarkerArray StopLineModule::createDebugMarkerArray()
 {
-  visualization_msgs::msg::MarkerArray msg;
-  tf2::Transform tf_base_link2front(
-    tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(debug_data.base_link2front, 0.0, 0.0));
+  MarkerArray msg;
 
-  // Stop VirtualWall
-  if (debug_data.stop_pose) {
-    visualization_msgs::msg::Marker marker;
-    marker.header.frame_id = "map";
-    marker.ns = "stop_virtual_wall";
-    marker.id = module_id;
-    marker.lifetime = rclcpp::Duration::from_seconds(0.5);
-    marker.type = visualization_msgs::msg::Marker::CUBE;
-    marker.action = visualization_msgs::msg::Marker::ADD;
-    tf2::Transform tf_map2base_link;
-    tf2::fromMsg(*debug_data.stop_pose, tf_map2base_link);
-    tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front;
-    tf2::toMsg(tf_map2front, marker.pose);
-    marker.pose.position.z += 1.0;
-    marker.scale.x = 0.1;
-    marker.scale.y = 5.0;
-    marker.scale.z = 2.0;
-    marker.color.a = 0.5;  // Don't forget to set the alpha!
-    marker.color.r = 1.0;
-    marker.color.g = 0.0;
-    marker.color.b = 0.0;
-    msg.markers.push_back(marker);
-  }
+  const auto now = this->clock_->now();
+
+  // Search stopline
+  {
+    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));
+    }
 
-  // Factor Text
-  if (debug_data.stop_pose) {
-    visualization_msgs::msg::Marker marker;
-    marker.header.frame_id = "map";
-    marker.ns = "factor_text";
-    marker.id = module_id;
-    marker.lifetime = rclcpp::Duration::from_seconds(0.5);
-    marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-    marker.action = visualization_msgs::msg::Marker::ADD;
-    tf2::Transform tf_map2base_link;
-    tf2::fromMsg(*debug_data.stop_pose, tf_map2base_link);
-    tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front;
-    tf2::toMsg(tf_map2front, marker.pose);
-    marker.pose.position.z += 2.0;
-    marker.scale.x = 0.0;
-    marker.scale.y = 0.0;
-    marker.scale.z = 1.0;
-    marker.color.a = 0.999;  // Don't forget to set the alpha!
-    marker.color.r = 1.0;
-    marker.color.g = 1.0;
-    marker.color.b = 1.0;
-    marker.text = "stop line";
     msg.markers.push_back(marker);
   }
 
   return msg;
 }
 
-}  // namespace
-
-visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray()
+MarkerArray StopLineModule::createVirtualWallMarkerArray()
 {
-  visualization_msgs::msg::MarkerArray debug_marker_array;
+  MarkerArray wall_marker;
+
+  if (!debug_data_.stop_pose) {
+    return wall_marker;
+  }
+
+  const auto now = this->clock_->now();
 
-  appendMarkerArray(
-    createMarkers(debug_data_, module_id_), this->clock_->now(), &debug_marker_array);
+  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);
 
-  return debug_marker_array;
+  return wall_marker;
 }
 }  // namespace behavior_velocity_planner

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

@@ -48,8 +48,7 @@ TrafficSignsWithLaneId getTrafficSignRegElemsOnPath(
 }
 
 std::vector<StopLineWithLaneId> getStopLinesWithLaneIdOnPath(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const lanelet::LaneletMapPtr lanelet_map)
+  const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map)
 {
   std::vector<StopLineWithLaneId> stop_lines_with_lane_id;
 
@@ -71,8 +70,7 @@ std::vector<StopLineWithLaneId> getStopLinesWithLaneIdOnPath(
 }
 
 std::set<int64_t> getStopLineIdSetOnPath(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const lanelet::LaneletMapPtr lanelet_map)
+  const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map)
 {
   std::set<int64_t> stop_line_id_set;
 
@@ -91,14 +89,13 @@ 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.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance", 2.0);
   p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0);
   p.use_initialization_stop_line_state =
     node.declare_parameter(ns + ".use_initialization_stop_line_state", false);
 }
 
-void StopLineModuleManager::launchNewModules(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+void StopLineModuleManager::launchNewModules(const PathWithLaneId & path)
 {
   for (const auto & stop_line_with_lane_id :
        getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
@@ -113,8 +110,7 @@ void StopLineModuleManager::launchNewModules(
 }
 
 std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
-StopLineModuleManager::getModuleExpiredFunction(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+StopLineModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
 {
   const auto stop_line_id_set =
     getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr());