fix(goal_planner): visualize stop wall (#5408)

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Oct 27, 2023 · 0af019e · 0af019e
1 parent 4e58c9a
commit 0af019e
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 0 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp
@@ -104,6 +104,8 @@ PathWithLaneId calcCenterLinePath(
   const std::optional<PathWithLaneId> & prev_module_path = std::nullopt);
 
 PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
+
+boost::optional<Pose> getFirstStopPoseFromPath(const PathWithLaneId & path);
 }  // namespace behavior_path_planner::utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -775,6 +775,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output)
   } else {
     // not_safe -> not_safe: use previous stop path
     output.path = status_.get_prev_stop_path();
+    stop_pose_ = utils::getFirstStopPoseFromPath(*output.path);
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path");
   }
@@ -804,6 +805,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output
   } else {
     // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path
     output.path = status_.get_prev_stop_path_after_approval();
+    stop_pose_ = utils::getFirstStopPoseFromPath(*output.path);
     RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path");
   }
   output.reference_path = getPreviousModuleOutput().reference_path;

diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp
@@ -580,4 +580,13 @@ PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId &
   return filtered_path;
 }
 
+boost::optional<Pose> getFirstStopPoseFromPath(const PathWithLaneId & path)
+{
+  for (const auto & p : path.points) {
+    if (std::abs(p.point.longitudinal_velocity_mps) < 0.01) {
+      return p.point.pose;
+    }
+  }
+  return boost::none;
+}
 }  // namespace behavior_path_planner::utils