fix(behavior_velocity_planner::intersection): show virtual wall when …

…interrupted by RTC (autowarefoundation#3185) Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>
swiftfile · Mar 28, 2023 · 69c7cdc · 69c7cdc
1 parent e00c636
commit 69c7cdc
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 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
@@ -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
@@ -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> & 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
@@ -212,14 +212,16 @@ 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(
     lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, ego_lane,
     ego_lane_with_next_lane, objects_ptr, closest_idx, time_delay);
 
   /* calculate final stop lines */
-  std::optional<size_t> stop_line_idx = std::nullopt;
+  std::optional<size_t> stop_line_idx =
+    std::make_optional<size_t>(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
     {