refactor(autoware_behavior_velocity_stop_line_module): refactor and test (#9424)

* refactor(autoware_behavior_velocity_stop_line_module): refactor and test

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* modify

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* small changes

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* update

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* fix test error

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* update

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

---------

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Author: yhisaki

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt

@@ -11,4 +11,15 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/scene.cpp
 )
 
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_scene.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    gtest_main
+    ${PROJECT_NAME}
+  )
+  target_include_directories(test_${PROJECT_NAME} PRIVATE src)
+endif()
+
 ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml

@@ -3,8 +3,4 @@
     stop_line:
       stop_margin: 0.0
       stop_duration_sec: 1.0
-      use_initialization_stop_line_state: true
       hold_stop_margin_distance: 2.0
-
-      debug:
-        show_stop_line_collision_check: false            # [-] whether to show stopline collision

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml

@@ -33,6 +33,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp

@@ -12,89 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "autoware/behavior_velocity_planner_common/utilization/util.hpp"
 #include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
-#include "autoware/universe_utils/ros/marker_helper.hpp"
 #include "scene.hpp"
 
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
-
 namespace autoware::behavior_velocity_planner
 {
-using autoware::universe_utils::appendMarkerArray;
-using autoware::universe_utils::createMarkerColor;
-using autoware::universe_utils::createMarkerScale;
-
-namespace
-{
-using DebugData = StopLineModule::DebugData;
-
-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()
-{
-  visualization_msgs::msg::MarkerArray debug_marker_array;
-  if (planner_param_.show_stop_line_collision_check) {
-    appendMarkerArray(
-      createStopLineCollisionCheck(debug_data_, module_id_), &debug_marker_array,
-      this->clock_->now());
-  }
-  return debug_marker_array;
-}
 
 autoware::motion_utils::VirtualWalls StopLineModule::createVirtualWalls()
 {

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp

@@ -15,7 +15,8 @@
 #include "manager.hpp"
 
 #include "autoware/universe_utils/ros/parameter.hpp"
-#include "autoware_lanelet2_extension/utility/query.hpp"
+
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
 #include <memory>
 #include <set>
@@ -28,19 +29,14 @@ using autoware::universe_utils::getOrDeclareParameter;
 using lanelet::TrafficSign;
 
 StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node)
-: SceneModuleManagerInterface(node, getModuleName())
+: SceneModuleManagerInterface(node, getModuleName()), planner_param_()
 {
   const std::string ns(StopLineModuleManager::getModuleName());
   auto & p = planner_param_;
   p.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
   p.hold_stop_margin_distance =
     getOrDeclareParameter<double>(node, ns + ".hold_stop_margin_distance");
   p.stop_duration_sec = getOrDeclareParameter<double>(node, ns + ".stop_duration_sec");
-  p.use_initialization_stop_line_state =
-    getOrDeclareParameter<bool>(node, ns + ".use_initialization_stop_line_state");
-  // debug
-  p.show_stop_line_collision_check =
-    getOrDeclareParameter<bool>(node, ns + ".debug.show_stop_line_collision_check");
 }
 
 std::vector<StopLineWithLaneId> StopLineModuleManager::getStopLinesWithLaneIdOnPath(
@@ -82,10 +78,9 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat
   for (const auto & stop_line_with_lane_id :
        getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
     const auto module_id = stop_line_with_lane_id.first.id();
-    const auto lane_id = stop_line_with_lane_id.second;
     if (!isModuleRegistered(module_id)) {
       registerModule(std::make_shared<StopLineModule>(
-        module_id, lane_id, stop_line_with_lane_id.first, planner_param_,
+        module_id, stop_line_with_lane_id.first, planner_param_,
         logger_.get_child("stop_line_module"), clock_));
     }
   }

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp

@@ -15,7 +15,6 @@
 #ifndef MANAGER_HPP_
 #define MANAGER_HPP_
 
-#include "autoware/behavior_velocity_planner_common/plugin_interface.hpp"
 #include "autoware/behavior_velocity_planner_common/plugin_wrapper.hpp"
 #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp"
 #include "scene.hpp"