From c1a98f7f8822d7766a65c9fedf4ef805861d320e Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 25 Jan 2023 14:23:31 +0900 Subject: [PATCH] refactor(motion_utils): delete wall marker explicitly (#2506) * delete wall marker in intersection module Signed-off-by: kyoichi-sugahara * delete wall marker in stop_line module Signed-off-by: kyoichi-sugahara * delete wall marker in blind_spot module Signed-off-by: kyoichi-sugahara * delete wall marker in no_stopping_area Signed-off-by: kyoichi-sugahara * delete wall marker in run_out module Signed-off-by: kyoichi-sugahara * make motion util function to delete wall marker Signed-off-by: kyoichi-sugahara * add common function * temp * fix(default_ad_api): fix autoware state to add wait time (#2407) * fix(default_ad_api): fix autoware state to add wait time Signed-off-by: Takagi, Isamu * Update system/default_ad_api/src/compatibility/autoware_state.cpp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: Takagi, Isamu Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * chore(behaviror_velocity_planner): changed logging level for intersection (#2459) changed logging level Signed-off-by: Mamoru Sobue * fix(tvm_utility): copy test result to CPU (#2414) Also remove dependency to autoware_auto_common. Issue-Id: SCM-5401 Signed-off-by: Ambroise Vincent Change-Id: I83b859742df2f2ff7df1d0bd2d287bfe0aa04c3d Signed-off-by: Ambroise Vincent Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> * fix(raw_vehicle_cmd_converter): fix column index for map validation (#2450) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara * revert(default_ad_api): fix autoware state to add wait time (#2407) (#2460) Revert "fix(default_ad_api): fix autoware state to add wait time (#2407)" This reverts commit c4224854a7e57a9526dde998f742741fe383471c. * feat(behavior_path_planner): cut overlapped path (#2451) * feat(behavior_path_planner): cut overlapped path Signed-off-by: yutaka * clean code Signed-off-by: yutaka Signed-off-by: yutaka * perf(behavior_velocity_planner): remove unnecessary debug data (#2462) Signed-off-by: veqcc * docs: update training data for traffic light (#2464) * update traffic light cnn classifier README.md * correct to upper case Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * feat(motion_utils): add points resample function (#2465) Signed-off-by: yutaka Signed-off-by: yutaka * feat: remove web controller (#2405) Signed-off-by: Yukihiro Saito Signed-off-by: Yukihiro Saito * fix(motion_utils): rename sampling function (#2469) Signed-off-by: yutaka Signed-off-by: yutaka * fix: rename `use_external_emergency_stop` to `check_external_emergency_heartbeat` (#2455) * fix: rename use_external_emergency_stop to check_external_emergency_heartbeat * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(tier4_autoware_api_launch): add rosbridge_server dependency (#2470) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka * fix: remove unnecessary DEBUG_INFO declarations (#2457) Signed-off-by: kminoda Signed-off-by: kminoda * feat(gyro_odometer): publish twist when both data arrives (#2423) * feat(gyro_odometer): publish when both data arrive Signed-off-by: kminoda * remove unnecessary commentouts Signed-off-by: kminoda * ci(pre-commit): autofix * use latest timestamp Signed-off-by: kminoda * small fix Signed-off-by: kminoda * debugged Signed-off-by: kminoda * update gyro_odometer Signed-off-by: kminoda * ci(pre-commit): autofix * add comments Signed-off-by: kminoda * add comments Signed-off-by: kminoda * ci(pre-commit): autofix * fix timestamp validation flow Signed-off-by: kminoda * ci(pre-commit): autofix * remove unnecessary commentouts Signed-off-by: kminoda * pre-commit Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(freespace_planning_algorithms): comment out failing tests (#2440) Signed-off-by: kosuke55 Signed-off-by: kosuke55 * feat(behavior_velocity_planner): add velocity factors (#1985) * (editting) add intersection_coordination to stop reason Signed-off-by: TakumiKozaka-T4 * (editting) add intersection coordination to stop reasons Signed-off-by: TakumiKozaka-T4 * (Editting) add v2x to stop reason Signed-off-by: TakumiKozaka-T4 * (editting) add stop reason2 publisher Signed-off-by: TakumiKozaka-T4 * (editting) add stop reason2 to scene modules Signed-off-by: TakumiKozaka-T4 * add stop reason2 to obstacle stop planner and surround obstacle checker Signed-off-by: TakumiKozaka-T4 * Modify files including unintended change by rebase Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * Modification 1: not to publsh vacant stop reason, 2: change default status in obstacle stop and surround obstacle checker Signed-off-by: TakumiKozaka-T4 * fix error Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * modification for renaming stop_reason2 to motion_factor Signed-off-by: TakumiKozaka-T4 * (Editting) rename variables Signed-off-by: TakumiKozaka-T4 * bug fix Signed-off-by: TakumiKozaka-T4 * (WIP) Add motion factor message. Modify scene modules due to new motion factor. Moving motion factor aggregator. Signed-off-by: TakumiKozaka-T4 * (WIP) Save current work. Modify aggregator, CMakeList. Add launcher Signed-off-by: TakumiKozaka-T4 * (WIP) Solved build error, but not launched Signed-off-by: TakumiKozaka-T4 * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 * (WIP) fixing launch error Signed-off-by: TakumiKozaka-T4 * Fix error in launching motion factor aggregator Signed-off-by: TakumiKozaka-T4 * Delete unnecessary comment-out in CMakelists. Change remapping in launcher. Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * pull the latest foundation/main Signed-off-by: TakumiKozaka-T4 * (fix for pre-commit.ci) Add to motion_factor_aggregator.hpp Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * feat: add velocity factor interface Signed-off-by: Takagi, Isamu * fix: fix build error Signed-off-by: Takagi, Isamu * feat: stop sign Signed-off-by: Takagi, Isamu * WIP Signed-off-by: Takagi, Isamu * feat: update visualizer Signed-off-by: Takagi, Isamu * feat: modify traffic light manager Signed-off-by: Takagi, Isamu * feat: update velocity factors Signed-off-by: Takagi, Isamu * feat: update api Signed-off-by: Takagi, Isamu * feat: move adapi msgs Signed-off-by: Takagi, Isamu * feat: remove old aggregator Signed-off-by: Takagi, Isamu * feat: move api Signed-off-by: Takagi, Isamu * feat: rename message Signed-off-by: Takagi, Isamu * feat: add using Signed-off-by: Takagi, Isamu * feat: add distance Signed-off-by: Takagi, Isamu * feat: fix build error Signed-off-by: Takagi, Isamu * feat: use nan as default distance Signed-off-by: Takagi, Isamu * fix: set virtual traffic light detail Signed-off-by: Takagi, Isamu * fix: remove debug code Signed-off-by: Takagi, Isamu * fix: copyright Signed-off-by: Takagi, Isamu Signed-off-by: TakumiKozaka-T4 Signed-off-by: Takagi, Isamu Co-authored-by: TakumiKozaka-T4 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(emergency_handler): fix mrm handling when mrm behavior is none (#2476) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara * feat(behavior_velocity_planner): clean walkway module (#2480) Signed-off-by: yutaka Signed-off-by: yutaka * fix(trajectory_follower): fix mpc trajectory z pos (#2482) Signed-off-by: takahoribe Signed-off-by: takahoribe * fix(ground-segmentation): recheck gnd cluster pointcloud (#2448) * fix: reclassify ground cluster pcl Signed-off-by: badai-nguyen * fix: add lowest-based recheck Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen Signed-off-by: badai-nguyen Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> * fix(freespace_planning_algorithms): fix rrtstar can't arrive goal error (#2350) Signed-off-by: NorahXiong Signed-off-by: NorahXiong Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> * revert(behavior_path): revert removal of refineGoalFunction (#2340)" (#2485) This reverts commit 8e13ced6dfb6edfea77a589ef4cb93d82683bf51. Signed-off-by: mitsudome-r Signed-off-by: mitsudome-r * fix(behavior_path_planner): minimum distance for lane change (#2413) Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi * fix(behavior_path_planner): lane change candidate resolution (#2426) * fix(behavior_path_planner): lane change candidate resolution Signed-off-by: Muhammad Zulfaqar Azmi * rework sampling based on current speed Signed-off-by: Muhammad Zulfaqar Azmi * refactor code Signed-off-by: Muhammad Zulfaqar Azmi * use util's resampler Signed-off-by: Muhammad Zulfaqar Azmi * consider min_resampling_points and resampling dt Signed-off-by: Muhammad Zulfaqar * simplify code Signed-off-by: Muhammad Zulfaqar Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar * chore(simulator_compatibility_test): suppress setuptools warnings (#2483) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * docs(tier4_state_rviz_plugin): update readme (#2475) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * feat(trajectory_follower): pub steer converged marker (#2441) * feat(trajectory_follower): pub steer converged marker Signed-off-by: kosuke55 * Revert "feat(trajectory_follower): pub steer converged marker" This reverts commit a6f6917bc542d5b533150f6abba086121e800974. Signed-off-by: kosuke55 * add steer converged debug marker in contoller_node Signed-off-by: kosuke55 Signed-off-by: kosuke55 * feat: replace python launch with xml launch for system monitor (#2430) * feat: replace python launch with xml launch for system monitor Signed-off-by: Daisuke Nishimatsu * ci(pre-commit): autofix * update figure Signed-off-by: Daisuke Nishimatsu Signed-off-by: Daisuke Nishimatsu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * refactor(vehicle_cmd_gate): remove old emergency topics (#2403) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * feat(behavior_path_planner, obstacle_avoidance_planner): add new drivable area (#2472) * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update obstacle avoidance planner Signed-off-by: yutaka * update Signed-off-by: yutaka * clean code Signed-off-by: yutaka * uddate Signed-off-by: yutaka * clean code Signed-off-by: yutaka * remove resample Signed-off-by: yutaka * update Signed-off-by: yutaka * add orientation Signed-off-by: yutaka * change color Signed-off-by: yutaka * update Signed-off-by: yutaka * remove drivable area Signed-off-by: yutaka * add flag Signed-off-by: yutaka * update Signed-off-by: yutaka * update color Signed-off-by: yutaka * fix some codes Signed-off-by: yutaka * change to makerker array Signed-off-by: yutaka * change avoidance utils Signed-off-by: yutaka Signed-off-by: yutaka * feat(default_ad_api): split parameters into file (#2488) * feat(default_ad_api): split parameters into file Signed-off-by: Takagi, Isamu * feat: remove old parameter Signed-off-by: Takagi, Isamu * fix: test Signed-off-by: Takagi, Isamu * feat: add default config Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * docs(default_ad_api): add readme (#2491) * docs(default_ad_api): add readme Signed-off-by: Takagi, Isamu * feat: update table Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * feat(map_loader): add differential map loading interface (#2417) * first commit Signed-off-by: kminoda * ci(pre-commit): autofix * added module load in _node.cpp Signed-off-by: kminoda * ci(pre-commit): autofix * create pcd metadata dict when either of the flag is true Signed-off-by: kminoda * ci(pre-commit): autofix * fix readme * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat(behavior_path_planner): change turn signal output timing (#2493) Signed-off-by: yutaka Signed-off-by: yutaka * feat(behavior_path_planner): remove unnecessary code and clean turn signal decider (#2494) * feat(behavior_path_planner): clean drivable area code Signed-off-by: yutaka * make a function for turn signal decider Signed-off-by: yutaka Signed-off-by: yutaka * feat(trajectory_visualizer): update for steer limit, remove tf for pose source (#2267) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe * feat(mission_planner): check goal footprint (#2088) Signed-off-by: ismet atabay * feat(behavior_path_planner): fix overlap checker (#2498) * feat(behavior_path_planner): fix overlap checker Signed-off-by: yutaka * remove reserve Signed-off-by: yutaka Signed-off-by: yutaka * fix(behavior_path_planner): fix find nearest function from lateral distance (#2499) * feat(behavior_path_planner): fix find nearest function from lateral distance * empty commit * fix(behavior_path_planner): fix planner data copy (#2501) Signed-off-by: yutaka Signed-off-by: yutaka * refactor(simple_planning_simulator): make function for duplicated code (#2502) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe * fix build error Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * revert intersection module change Signed-off-by: kyoichi-sugahara * delete wallmarker in intersection module Signed-off-by: kyoichi-sugahara * compare pose with resample_uitils Signed-off-by: kyoichi-sugahara * delete wallmarker in merge_from_private module Signed-off-by: kyoichi-sugahara * delete wallmarker in no_stopping_area Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * add dead line creator Signed-off-by: kyoichi-sugahara * fix base function Signed-off-by: kyoichi-sugahara * delete wall marker in each module Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * empty gurad Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * instantiation wall maeker creator Signed-off-by: kyoichi-sugahara * initialize pointer Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * define create_wall_function and delete_wall_function in VirtualWallMarkerCreator Signed-off-by: kyoichi-sugahara * change instance name Signed-off-by: kyoichi-sugahara Signed-off-by: kyoichi-sugahara Signed-off-by: Takagi, Isamu Signed-off-by: Mamoru Sobue Signed-off-by: Ambroise Vincent Signed-off-by: Makoto Kurihara Signed-off-by: yutaka Signed-off-by: veqcc Signed-off-by: Yukihiro Saito Signed-off-by: Takayuki Murooka Signed-off-by: kminoda Signed-off-by: kosuke55 Signed-off-by: TakumiKozaka-T4 Signed-off-by: takahoribe Signed-off-by: badai-nguyen Signed-off-by: NorahXiong Signed-off-by: mitsudome-r Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Signed-off-by: Daisuke Nishimatsu Signed-off-by: Takamasa Horibe Signed-off-by: ismet atabay Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Mamoru Sobue Co-authored-by: Ambroise Vincent Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Ryuta Kambe Co-authored-by: Mingyu1991 <115005477+Mingyu1991@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takayuki Murooka Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Kosuke Takeuchi Co-authored-by: TakumiKozaka-T4 Co-authored-by: Takamasa Horibe Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> --- .../motion_utils/marker/marker_helper.hpp | 57 ++++++++++++-- .../motion_utils/src/marker/marker_helper.cpp | 78 +++++++++++++++++++ .../include/scene_module/blind_spot/scene.hpp | 3 + .../crosswalk/scene_crosswalk.hpp | 4 + .../scene_module/crosswalk/scene_walkway.hpp | 4 + .../scene_module/detection_area/scene.hpp | 3 + .../intersection/scene_intersection.hpp | 4 + .../scene_merge_from_private_road.hpp | 3 + .../scene_no_stopping_area.hpp | 3 + .../occlusion_spot/scene_occlusion_spot.hpp | 3 + .../include/scene_module/run_out/debug.hpp | 3 + .../include/scene_module/stop_line/scene.hpp | 3 + .../virtual_traffic_light/scene.hpp | 2 + .../src/scene_module/blind_spot/debug.cpp | 4 +- .../src/scene_module/crosswalk/debug.cpp | 22 +++++- .../src/scene_module/detection_area/debug.cpp | 20 +++-- .../src/scene_module/intersection/debug.cpp | 13 ++-- .../scene_module/no_stopping_area/debug.cpp | 11 ++- .../src/scene_module/occlusion_spot/debug.cpp | 6 +- .../src/scene_module/run_out/debug.cpp | 8 +- .../src/scene_module/stop_line/debug.cpp | 7 +- .../virtual_traffic_light/debug.cpp | 16 ++-- 22 files changed, 235 insertions(+), 42 deletions(-) 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;