fix(behavior_velocity_planner): fix safety slow down crosswalk inters…

…ection check (#1708) Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>
autowarefoundation · Aug 26, 2022 · 7f3389e · 7f3389e
1 parent eda487d
commit 7f3389e
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 4 deletions.
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
@@ -133,7 +133,7 @@ class CrosswalkModule : public SceneModuleInterface
 
   CollisionPointState getCollisionPointState(const double ttc, const double ttv) const;
 
-  void applySafetySlowDownSpeed(PathWithLaneId & output);
+  bool applySafetySlowDownSpeed(PathWithLaneId & output);
 
   float calcTargetVelocity(
     const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const;

diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
@@ -172,8 +172,9 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
 
   if (crosswalk_.hasAttribute("safety_slow_down_speed")) {
     // Safety slow down is on
-    applySafetySlowDownSpeed(*path);
-    ego_path = *path;
+    if (applySafetySlowDownSpeed(*path)) {
+      ego_path = *path;
+    }
   }
 
   RCLCPP_INFO_EXPRESSION(
@@ -731,8 +732,12 @@ CollisionPointState CrosswalkModule::getCollisionPointState(
   return CollisionPointState::YIELD;
 }
 
-void CrosswalkModule::applySafetySlowDownSpeed(PathWithLaneId & output)
+bool CrosswalkModule::applySafetySlowDownSpeed(PathWithLaneId & output)
 {
+  if (path_intersects_.empty()) {
+    return false;
+  }
+
   const auto & ego_pos = planner_data_->current_pose.pose.position;
   const auto ego_path = output;
 
@@ -772,6 +777,7 @@ void CrosswalkModule::applySafetySlowDownSpeed(PathWithLaneId & output)
       }
     }
   }
+  return true;
 }
 
 bool CrosswalkModule::isStuckVehicle(