From b803dbfca9025954d37519529d2559407022a061 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 28 Mar 2023 14:00:15 +0900 Subject: [PATCH] fix(behavior_velocity_planner::intersection): show virtual wall when interrupted by RTC (#3185) Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/scene_intersection.hpp | 2 ++ .../src/scene_module/intersection/debug.cpp | 3 +-- .../scene_module/intersection/scene_intersection.cpp | 11 ++++++----- 3 files changed, 9 insertions(+), 7 deletions(-) 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 7a4873c5dc66a..e92d1ef6693a2 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 @@ -48,6 +48,8 @@ class IntersectionModule : public SceneModuleInterface public: struct DebugData { + bool stop_required = false; + geometry_msgs::msg::Pose stop_wall_pose; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; 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 b2174e69512d8..394e542164b10 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -176,7 +176,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker const auto now = this->clock_->now(); - if (state_machine_.getState() == StateMachine::State::STOP) { + if (debug_data_.stop_required) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( {debug_data_.stop_wall_pose}, "intersection", now, module_id_), @@ -207,7 +207,6 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createVirtualWa visualization_msgs::msg::MarkerArray wall_marker; const auto state = state_machine_.getState(); - const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { const std::vector & pose = {debug_data_.virtual_wall_pose}; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 359f3eeaa4765..58364c2fcd1d0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -212,6 +212,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); /* calculate dynamic collision around detection area */ + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const double time_delay = is_go_out_ ? 0.0 : (planner_param_.state_transit_margin_time - state_machine_.getDuration()); const bool has_collision = checkCollision( @@ -219,7 +220,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * ego_lane_with_next_lane, objects_ptr, closest_idx, time_delay); /* calculate final stop lines */ - std::optional stop_line_idx = std::nullopt; + std::optional stop_line_idx = + std::make_optional(stop_lines_idx_opt.value().collision_stop_line); if (external_go) { is_entry_prohibited = false; } else if (external_stop) { @@ -269,11 +271,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block is_go_out_ = false; - constexpr double v = 0.0; - planning_utils::setVelocityFromIndex(stop_line_idx.value(), v, path); - const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + planning_utils::setVelocityFromIndex(stop_line_idx.value(), 0.0 /* [m/s] */, path); + debug_data_.stop_required = true; // dangerous or disabled by RTC debug_data_.stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx.value(), base_link2front, *path); + planning_utils::getAheadPose(stop_line_idx.value(), baselink2front, *path); // Get stop point and stop factor {