chore(behavior_velocity_planner.stopline): add debug marker for stopl…

…ine collision check (autowarefoundation#932)

* chore(behavior_velocity_planner.stopline): add debug marker for stopline collision check

* feat: use marker helper

Signed-off-by: h-ohta <hiroki.ota@tier4.jp>

planning/behavior_velocity_planner/config/stop_line.param.yaml

@@ -4,3 +4,5 @@
       stop_margin: 0.0
       stop_check_dist: 2.0
       stop_duration_sec: 1.0
+      debug:
+        show_stopline_collision_check: false            # [-] whether to show stopline collision

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

@@ -63,13 +63,16 @@ class StopLineModule : public SceneModuleInterface
   {
     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;
+    bool show_stopline_collision_check;
   };
 
 public:

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

@@ -87,6 +87,55 @@ visualization_msgs::msg::MarkerArray createMarkers(
   return msg;
 }
 
+visualization_msgs::msg::MarkerArray createStopLineCollisionCheck(
+  const DebugData & debug_data, const int64_t module_id)
+{
+  visualization_msgs::msg::MarkerArray msg;
+
+  // Search Segments
+  {
+    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;
+}
+
 }  // namespace
 
 visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray()
@@ -96,6 +145,12 @@ visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray()
   appendMarkerArray(
     createMarkers(debug_data_, module_id_), this->clock_->now(), &debug_marker_array);
 
+  if (planner_param_.show_stopline_collision_check) {
+    appendMarkerArray(
+      createStopLineCollisionCheck(debug_data_, module_id_), this->clock_->now(),
+      &debug_marker_array);
+  }
+
   return debug_marker_array;
 }
 }  // namespace behavior_velocity_planner

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

@@ -93,6 +93,9 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node)
   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);
+  // debug
+  p.show_stopline_collision_check =
+    node.declare_parameter(ns + ".debug.show_stopline_collision_check", false);
 }
 
 void StopLineModuleManager::launchNewModules(

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

@@ -109,6 +109,18 @@ boost::optional<StopLineModule::SegmentIndexWithPoint2d> StopLineModule::findCol
 {
   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
+  if (planner_param_.show_stopline_collision_check) {
+    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;