fix(behavior_velocity_planner): fix condition to show virtual wall (t…

…ier4#1298) Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
boyali · Oct 3, 2022 · f06c76a · f06c76a
1 parent f8e2b19
commit f06c76a
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 5 deletions.
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
@@ -126,6 +126,7 @@ class BlindSpotModule : public SceneModuleInterface
   int64_t lane_id_;
   TurnDirection turn_direction_;
   bool has_traffic_light_;
+  bool is_over_pass_judge_line_;
 
   // Parameter
   PlannerParam planner_param_;

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
@@ -156,7 +156,7 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr
 
   const auto now = this->clock_->now();
 
-  if (!isActivated()) {
+  if (!isActivated() && !is_over_pass_judge_line_) {
     appendMarkerArray(
       tier4_autoware_utils::createStopVirtualWallMarker(
         debug_data_.virtual_wall_pose, "blind_spot", now, lane_id_),

diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp
@@ -42,7 +42,8 @@ BlindSpotModule::BlindSpotModule(
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
-  turn_direction_(TurnDirection::INVALID)
+  turn_direction_(TurnDirection::INVALID),
+  is_over_pass_judge_line_(false)
 {
   planner_param_ = planner_param;
   const auto & assigned_lanelet =
@@ -115,13 +116,13 @@ bool BlindSpotModule::modifyPathVelocity(
   debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx).point.pose;
 
   /* if current_state = GO, and current_pose is over judge_line, ignore planning. */
-  bool is_over_pass_judge_line = static_cast<bool>(closest_idx > pass_judge_line_idx);
+  is_over_pass_judge_line_ = static_cast<bool>(closest_idx > pass_judge_line_idx);
   if (closest_idx == pass_judge_line_idx) {
     geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose;
-    is_over_pass_judge_line = util::isAheadOf(current_pose.pose, pass_judge_line);
+    is_over_pass_judge_line_ = util::isAheadOf(current_pose.pose, pass_judge_line);
   }
   if (planner_param_.use_pass_judge_line) {
-    if (current_state == State::GO && is_over_pass_judge_line) {
+    if (current_state == State::GO && is_over_pass_judge_line_) {
       RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed.");
       *path = input_path;  // reset path
       setSafe(true);