refactor(motion_utils): delete wall marker explicitly

common/motion_utils/include/motion_utils/marker/marker_helper.hpp

@@ -15,23 +15,28 @@
 #ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 
+#include "motion_utils/resample/resample_utils.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
+#include <functional>
 #include <string>
+#include <vector>
 
 namespace motion_utils
 {
+using geometry_msgs::msg::Pose;
+
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
-  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset = 0.0);
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0);
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
-  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset = 0.0);
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0);
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
-  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset = 0.0);
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0);
 
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);
@@ -41,6 +46,46 @@ visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
 
 visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);
+
+visualization_msgs::msg::MarkerArray createVirtualWallMarkerFromPreviousPoses(
+  const std::vector<Pose> & stop_poses, const std::vector<Pose> & previous_poses,
+  const rclcpp::Time & now, int32_t id);
+
+using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset)>;
+
+using delete_wall_function =
+  std::function<visualization_msgs::msg::MarkerArray(const rclcpp::Time & now, const int32_t id)>;
+
+class VirtualWallMarkerCreator
+{
+public:
+  virtual ~VirtualWallMarkerCreator() = default;
+
+  visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
+    const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
+    int32_t id, const double longitudinal_offset = 0.0);
+
+  visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
+    const std::vector<Pose> & slow_down_poses, const std::string & module_name,
+    const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0);
+
+  visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
+    const std::vector<Pose> & dead_line_poses, const std::string & module_name,
+    const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0);
+
+private:
+  visualization_msgs::msg::MarkerArray handleVirtualWallMarker(
+    const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
+    int32_t id, create_wall_function function_create_wall_marker,
+    delete_wall_function function_delete_wall_marker,
+    std::vector<geometry_msgs::msg::Pose> & previous_poses, const double longitudinal_offset = 0.0);
+
+  std::vector<Pose> previous_stop_poses_;
+  std::vector<Pose> previous_slow_down_poses_;
+  std::vector<Pose> previous_dead_line_poses_;
+};
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_

common/motion_utils/src/marker/marker_helper.cpp

@@ -16,13 +16,16 @@
 
 #include <string>
 
+using tier4_autoware_utils::appendMarkerArray;
 using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createDeletedDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
+using visualization_msgs::msg::MarkerArray;
 
 namespace
 {
+
 inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(
   const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name,
   const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id,
@@ -128,4 +131,77 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
 {
   return createDeletedVirtualWallMarkerArray("dead_line_", now, id);
 }
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWallMarker(
+  const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
+  int32_t id, create_wall_function function_create_wall_marker,
+  delete_wall_function function_delete_wall_marker,
+  std::vector<geometry_msgs::msg::Pose> & previous_virtual_wall_poses,
+  const double longitudinal_offset)
+{
+  size_t id_to_create = id;
+  size_t id_to_delete = id;
+  visualization_msgs::msg::MarkerArray wall_marker;
+
+  if (poses.size() == 0 || previous_virtual_wall_poses.empty()) {
+    return wall_marker;
+  }
+
+  for (const auto & p : previous_virtual_wall_poses) {
+    const bool previous_stop_pose_is_in_stop_pose =
+      std::any_of(poses.begin(), poses.end(), [&](const geometry_msgs::msg::Pose & elem) {
+        std::vector<Pose> poses;
+        poses.push_back(p);
+        poses.push_back(elem);
+        return resample_utils::validate_points_duplication(poses);
+      });
+
+    if (!previous_stop_pose_is_in_stop_pose) {
+      appendMarkerArray(function_delete_wall_marker(now, id_to_delete), &wall_marker, now);
+    }
+    id_to_delete++;
+  }
+
+  for (const auto & p : poses) {
+    appendMarkerArray(
+      function_create_wall_marker(p, module_name, now, id_to_create++, longitudinal_offset),
+      &wall_marker);
+  }
+  previous_virtual_wall_poses = poses;
+  return wall_marker;
+}
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker(
+  const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
+  int32_t id, const double longitudinal_offset)
+{
+  create_wall_function f1 = motion_utils::createStopVirtualWallMarker;
+  delete_wall_function f2 = motion_utils::createDeletedStopVirtualWallMarker;
+
+  return handleVirtualWallMarker(
+    stop_poses, module_name, now, id, f1, f2, previous_stop_poses_, longitudinal_offset);
+}
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker(
+  const std::vector<Pose> & slow_down_poses, const std::string & module_name,
+  const rclcpp::Time & now, int32_t id, const double longitudinal_offset)
+{
+  create_wall_function f1 = motion_utils::createSlowDownVirtualWallMarker;
+  delete_wall_function f2 = motion_utils::createDeletedSlowDownVirtualWallMarker;
+
+  return handleVirtualWallMarker(
+    slow_down_poses, module_name, now, id, f1, f2, previous_slow_down_poses_, longitudinal_offset);
+}
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker(
+  const std::vector<Pose> & dead_line_poses, const std::string & module_name,
+  const rclcpp::Time & now, int32_t id, const double longitudinal_offset)
+{
+  create_wall_function f1 = motion_utils::createDeadLineVirtualWallMarker;
+  delete_wall_function f2 = motion_utils::createDeletedDeadLineVirtualWallMarker;
+
+  return handleVirtualWallMarker(
+    dead_line_poses, module_name, now, id, f1, f2, previous_dead_line_poses_, longitudinal_offset);
+}
+
 }  // namespace motion_utils

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

@@ -217,6 +217,9 @@ class BlindSpotModule : public SceneModuleInterface
 
   // Debug
   mutable DebugData debug_data_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp

@@ -38,6 +38,7 @@
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <memory>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -174,6 +175,9 @@ class CrosswalkModule : public SceneModuleInterface
 
   // whether ego passed safety_slow_point
   bool passed_safety_slow_point_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_crosswalk_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp

@@ -28,6 +28,7 @@
 #include <lanelet2_routing/RoutingGraph.h>
 #include <lanelet2_routing/RoutingGraphContainer.h>
 
+#include <memory>
 #include <utility>
 #include <vector>
 
@@ -72,6 +73,9 @@ class WalkwayModule : public SceneModuleInterface
 
   // Debug
   DebugData debug_data_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_walkway_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

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

@@ -99,6 +99,9 @@ class DetectionAreaModule : public SceneModuleInterface
 
   // Debug
   DebugData debug_data_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -15,6 +15,7 @@
 #ifndef SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_
 #define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_
 
+#include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/scene_module_interface.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
@@ -260,6 +261,9 @@ class IntersectionModule : public SceneModuleInterface
 
   // Debug
   mutable DebugData debug_data_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp

@@ -100,6 +100,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
 
   // Debug
   mutable DebugData debug_data_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp

@@ -174,6 +174,9 @@ class NoStoppingAreaModule : public SceneModuleInterface
 
   // Debug
   DebugData debug_data_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp

@@ -68,6 +68,9 @@ class OcclusionSpotModule : public SceneModuleInterface
 
   // Debug
   mutable DebugData debug_data_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp

@@ -16,6 +16,7 @@
 
 #include "scene_module/run_out/dynamic_obstacle.hpp"
 
+#include <memory>
 #include <string>
 #include <vector>
 namespace behavior_velocity_planner
@@ -130,6 +131,8 @@ class RunOutDebug
   DebugValues debug_values_;
   AccelReason accel_reason_;
   double height_{0};
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 
 }  // namespace behavior_velocity_planner

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

@@ -105,6 +105,9 @@ class StopLineModule : public SceneModuleInterface
 
   // Debug
   DebugData debug_data_;
+
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 };
 }  // namespace behavior_velocity_planner
 

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

@@ -94,6 +94,8 @@ class VirtualTrafficLightModule : public SceneModuleInterface
   tier4_v2x_msgs::msg::InfrastructureCommand command_;
   MapData map_data_;
   ModuleData module_data_;
+  std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
+    std::make_shared<motion_utils::VirtualWallMarkerCreator>();
 
   void updateInfrastructureCommand();
 

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

@@ -78,8 +78,8 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr
 
   if (!isActivated() && !is_over_pass_judge_line_) {
     appendMarkerArray(
-      motion_utils::createStopVirtualWallMarker(
-        debug_data_.virtual_wall_pose, "blind_spot", now, module_id_),
+      virtual_wall_marker_creator_->createStopVirtualWallMarker(
+        {debug_data_.virtual_wall_pose}, "blind_spot", now, module_id_),
       &wall_marker, now);
   }
   return wall_marker;

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

@@ -223,17 +223,26 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr
 {
   const auto now = this->clock_->now();
   auto id = module_id_;
+  std::vector<Pose> stop_poses;
+  std::vector<Pose> slow_down_poses;
 
   visualization_msgs::msg::MarkerArray wall_marker;
   for (const auto & p : debug_data_.stop_poses) {
     const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
-    appendMarkerArray(createStopVirtualWallMarker(p_front, "crosswalk", now, id++), &wall_marker);
+    stop_poses.push_back(p_front);
   }
+  appendMarkerArray(
+    virtual_wall_marker_creator_crosswalk_->createStopVirtualWallMarker(
+      stop_poses, "crosswalk", now, id),
+    &wall_marker);
   for (const auto & p : debug_data_.slow_poses) {
     const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
-    appendMarkerArray(
-      createSlowDownVirtualWallMarker(p_front, "crosswalk", now, id++), &wall_marker);
+    slow_down_poses.push_back(p_front);
   }
+  appendMarkerArray(
+    virtual_wall_marker_creator_crosswalk_->createSlowDownVirtualWallMarker(
+      slow_down_poses, "crosswalk", now, id),
+    &wall_marker);
 
   return wall_marker;
 }
@@ -242,12 +251,17 @@ visualization_msgs::msg::MarkerArray WalkwayModule::createVirtualWallMarkerArray
 {
   const auto now = this->clock_->now();
   auto id = module_id_;
+  std::vector<Pose> stop_poses;
 
   visualization_msgs::msg::MarkerArray wall_marker;
   for (const auto & p : debug_data_.stop_poses) {
     const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
-    appendMarkerArray(createStopVirtualWallMarker(p_front, "walkway", now, id++), &wall_marker);
+    stop_poses.push_back(p_front);
   }
+  appendMarkerArray(
+    virtual_wall_marker_creator_walkway_->createStopVirtualWallMarker(
+      stop_poses, "walkway", now, id),
+    &wall_marker);
   return wall_marker;
 }