diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 714220d5dedc4..166d3a6661689 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/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 #include +#include 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 & stop_poses, const std::vector & previous_poses, + const rclcpp::Time & now, int32_t id); + +class VirtualWallMarkerCreator +{ +public: + virtual ~VirtualWallMarkerCreator() = default; + + using create_wall_function = std::function; + + using delete_wall_function = + std::function; + + visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( + const std::vector & 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 & 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 & 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 & 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 & previous_poses, const double longitudinal_offset = 0.0); + + std::vector previous_stop_poses_; + std::vector previous_slow_down_poses_; + std::vector previous_dead_line_poses_; +}; } // namespace motion_utils #endif // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index d88bcada25193..60e5f09bbea89 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -16,13 +16,16 @@ #include +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,79 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( { return createDeletedVirtualWallMarkerArray("dead_line_", now, id); } + +visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWallMarker( + const std::vector & 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 & 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 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 & stop_poses, const std::string & module_name, const rclcpp::Time & now, + int32_t id, const double longitudinal_offset) +{ + create_wall_function creator = motion_utils::createStopVirtualWallMarker; + delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker; + + return handleVirtualWallMarker( + stop_poses, module_name, now, id, creator, deleter, previous_stop_poses_, longitudinal_offset); +} + +visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker( + const std::vector & slow_down_poses, const std::string & module_name, + const rclcpp::Time & now, int32_t id, const double longitudinal_offset) +{ + create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker; + delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker; + + return handleVirtualWallMarker( + slow_down_poses, module_name, now, id, creator, deleter, previous_slow_down_poses_, + longitudinal_offset); +} + +visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker( + const std::vector & dead_line_poses, const std::string & module_name, + const rclcpp::Time & now, int32_t id, const double longitudinal_offset) +{ + create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker; + delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker; + + return handleVirtualWallMarker( + dead_line_poses, module_name, now, id, creator, deleter, previous_dead_line_poses_, + longitudinal_offset); +} + } // namespace motion_utils diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index 4b1c672ff2b08..d85bc002297df 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/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 virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp index f3930291d5b9f..acd0c45755b71 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -174,6 +175,9 @@ class CrosswalkModule : public SceneModuleInterface // whether ego passed safety_slow_point bool passed_safety_slow_point_; + + std::shared_ptr virtual_wall_marker_creator_crosswalk_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp index 8d05fc4f52d39..42be22f1386b6 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -72,6 +73,9 @@ class WalkwayModule : public SceneModuleInterface // Debug DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_walkway_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index 3d2ede0c970ab..b6896f4b68eb9 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/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 virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index f8e0fea2f23de..98589555bd7aa 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/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 #include #include #include @@ -256,6 +257,9 @@ class IntersectionModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 137a3c6286705..a2d884240747d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/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 virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp index 181293144bd8b..e387b1b39cf2d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp +++ b/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 virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index 05eab13f79e63..c7ee714b03a7b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/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 virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp index f833e4e0e46b2..414ce63ed8d7f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp @@ -16,6 +16,7 @@ #include "scene_module/run_out/dynamic_obstacle.hpp" +#include #include #include namespace behavior_velocity_planner @@ -130,6 +131,8 @@ class RunOutDebug DebugValues debug_values_; AccelReason accel_reason_; double height_{0}; + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp index b22c61b19f549..f9846f970beb5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp +++ b/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 virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 130e4a1715fde..2bd7078d93aeb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/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 virtual_wall_marker_creator_ = + std::make_shared(); void updateInfrastructureCommand(); diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp index fbd525294a50e..67b364033b4fd 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp +++ b/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; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp index 6aab24b9a24fa..1b27e2b06cf55 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp +++ b/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 stop_poses; + std::vector 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 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; } diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp index 5d62dfbc35d4e..9bd56c79cf70d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp @@ -175,21 +175,29 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createVirtualWallMarke const rclcpp::Time now = clock_->now(); auto id = getModuleId(); + + std::vector stop_poses; + std::vector dead_line_poses; + for (const auto & p : debug_data_.stop_poses) { const auto p_front = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p_front, "detection_area", now, id++), &wall_marker, - now); + stop_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + stop_poses, "detection_area", now, id), + &wall_marker, now); for (const auto & p : debug_data_.dead_line_poses) { const auto p_front = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray( - motion_utils::createDeadLineVirtualWallMarker(p_front, "detection_area", now, id++), - &wall_marker, now); + dead_line_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_->createDeadLineVirtualWallMarker( + dead_line_poses, "detection_area", now, id), + &wall_marker, now); return wall_marker; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 8697be34d0222..23ebdcef0f09a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -193,13 +193,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker if (debug_data_.stop_required) { appendMarkerArray( - motion_utils::createStopVirtualWallMarker( - debug_data_.stop_wall_pose, "intersection", now, module_id_), + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.stop_wall_pose}, "intersection", now, module_id_), &wall_marker, now); } else if (state == StateMachine::State::STOP) { appendMarkerArray( - motion_utils::createStopVirtualWallMarker( - debug_data_.slow_wall_pose, "intersection", now, module_id_), + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.slow_wall_pose}, "intersection", now, module_id_), &wall_marker, now); } return wall_marker; @@ -230,9 +230,10 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createVirtualWa const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { + const std::vector & pose = {debug_data_.virtual_wall_pose}; appendMarkerArray( - motion_utils::createStopVirtualWallMarker( - debug_data_.virtual_wall_pose, "merge_from_private_road", now, module_id_), + virtual_wall_marker_creator_->createStopVirtualWallMarker( + pose, "merge_from_private_road", now, module_id_), &wall_marker, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp index b50fa38e48d6d..205d9314c2505 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp @@ -168,13 +168,18 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createVirtualWallMark const auto now = clock_->now(); auto id = module_id_; + std::vector stop_poses; + for (const auto & p : debug_data_.stop_poses) { const auto p_front = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p_front, "no_stopping_area", now, id++), - &wall_marker, now); + stop_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + stop_poses, "no_stopping_area", now, id), + &wall_marker, now); + return wall_marker; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 73c8b12b79162..bf82bd74552df 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -219,11 +219,15 @@ MarkerArray OcclusionSpotModule::createVirtualWallMarkerArray() MarkerArray wall_marker; std::string module_name = "occlusion_spot"; + std::vector slow_down_poses; + size_t module_id = 0; for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) { const auto p_front = calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); + slow_down_poses.push_back(p_front); appendMarkerArray( - motion_utils::createSlowDownVirtualWallMarker(p_front, module_name, current_time, id), + virtual_wall_marker_creator_->createSlowDownVirtualWallMarker( + slow_down_poses, module_name, current_time, module_id), &wall_marker, current_time); } return wall_marker; diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp index 61577891224c3..cdd94bfb9b001 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp @@ -165,12 +165,12 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVirtualWallMarkerArray() { visualization_msgs::msg::MarkerArray wall_marker; rclcpp::Time now = node_.now(); + size_t id = 0; - for (const auto & p : stop_pose_) { - appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p, "run_out", now, id++), &wall_marker); - } + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker(stop_pose_, "run_out", now, id), + &wall_marker, now); stop_pose_.clear(); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp index 72daa9012a8b7..d86c6f72b1649 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp @@ -98,6 +98,7 @@ visualization_msgs::msg::MarkerArray StopLineModule::createVirtualWallMarkerArra { const auto now = this->clock_->now(); visualization_msgs::msg::MarkerArray wall_marker; + if (!debug_data_.stop_pose) { return wall_marker; } @@ -105,9 +106,11 @@ visualization_msgs::msg::MarkerArray StopLineModule::createVirtualWallMarkerArra *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); if (state_ == State::APPROACH || state_ == State::STOPPED) { appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p_front, "stopline", now, module_id_), &wall_marker, - now); + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {p_front}, "stopline", now, module_id_), + &wall_marker, now); } + return wall_marker; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp index e001e581232eb..f60a317de996e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp @@ -58,18 +58,18 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createVirtualWal // virtual_wall_stop_line if (d.stop_head_pose_at_stop_line) { - const auto markers = createStopVirtualWallMarker( - *d.stop_head_pose_at_stop_line, "virtual_traffic_light", now, module_id_); - - appendMarkerArray(markers, &wall_marker); + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {*d.stop_head_pose_at_stop_line}, "virtual_traffic_light", now, module_id_), + &wall_marker, now); } // virtual_wall_end_line if (d.stop_head_pose_at_end_line) { - const auto markers = createStopVirtualWallMarker( - *d.stop_head_pose_at_end_line, "virtual_traffic_light", now, module_id_); - - appendMarkerArray(markers, &wall_marker); + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {*d.stop_head_pose_at_end_line}, "virtual_traffic_light", now, module_id_), + &wall_marker, now); } return wall_marker;