From 6738c79b9222cec2bbda802c8309cb9f499bcae1 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Wed, 20 Jul 2022 15:23:48 +0900
Subject: [PATCH] fix(behavior_velocity_planner): output stop reason when RTC
 sends deactivate command (#1372)

* fix(behavior_velocity_planner): output stop reason when RTC sends deactivate command

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(behavior_velocity_plnaner): avoid stop factor overwrite

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../crosswalk/scene_crosswalk.hpp             |  4 ++--
 .../crosswalk/scene_crosswalk.cpp             | 21 ++++++++++++-------
 2 files changed, 15 insertions(+), 10 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
index 26605b403a01f..bc3b19d3b4adc 100644
--- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp
@@ -107,10 +107,10 @@ class CrosswalkModule : public SceneModuleInterface
   int64_t module_id_;
 
   boost::optional<std::pair<size_t, PathPointWithLaneId>> findRTCStopPoint(
-    const PathWithLaneId & ego_path);
+    const PathWithLaneId & ego_path, StopFactor & stop_factor);
 
   boost::optional<std::pair<size_t, PathPointWithLaneId>> findNearestStopPoint(
-    const PathWithLaneId & ego_path, StopReason & stop_reason);
+    const PathWithLaneId & ego_path, StopFactor & stop_factor);
 
   boost::optional<std::pair<double, geometry_msgs::msg::Point>> getStopLine(
     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
index 01f479e0d9bec..8abdad368290a 100644
--- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
@@ -356,8 +356,11 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     logger_, planner_param_.show_processing_time, "- step2: %f ms",
     stop_watch_.toc("total_processing_time", false));
 
-  const auto nearest_stop_point = findNearestStopPoint(ego_path, *stop_reason);
-  const auto rtc_stop_point = findRTCStopPoint(ego_path);
+  StopFactor stop_factor{};
+  StopFactor stop_factor_rtc{};
+
+  const auto nearest_stop_point = findNearestStopPoint(ego_path, stop_factor);
+  const auto rtc_stop_point = findRTCStopPoint(ego_path, stop_factor_rtc);
 
   RCLCPP_INFO_EXPRESSION(
     logger_, planner_param_.show_processing_time, "- step3: %f ms",
@@ -380,8 +383,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
 
   if (nearest_stop_point) {
     insertDecelPoint(nearest_stop_point.get(), 0.0, *path);
+    planning_utils::appendStopReason(stop_factor, stop_reason);
   } else if (rtc_stop_point) {
     insertDecelPoint(rtc_stop_point.get(), 0.0, *path);
+    planning_utils::appendStopReason(stop_factor_rtc, stop_reason);
   }
 
   RCLCPP_INFO_EXPRESSION(
@@ -421,7 +426,7 @@ boost::optional<std::pair<double, geometry_msgs::msg::Point>> CrosswalkModule::g
 }
 
 boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findRTCStopPoint(
-  const PathWithLaneId & ego_path)
+  const PathWithLaneId & ego_path, StopFactor & stop_factor)
 {
   const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
 
@@ -437,14 +442,15 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findRTC
   const auto residual_length = calcLongitudinalOffsetToSegment(ego_path.points, base_idx, p_stop);
   const auto update_margin = margin - residual_length + base_link2front;
 
-  return getBackwardInsertPointFromBasePoint(base_idx, ego_path, update_margin);
+  const auto stop_point = getBackwardInsertPointFromBasePoint(base_idx, ego_path, update_margin);
+  stop_factor.stop_pose = stop_point.get().second.point.pose;
+
+  return stop_point;
 }
 
 boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNearestStopPoint(
-  const PathWithLaneId & ego_path, StopReason & stop_reason)
+  const PathWithLaneId & ego_path, StopFactor & stop_factor)
 {
-  StopFactor stop_factor{};
-
   bool found_pedestrians = false;
   bool found_stuck_vehicle = false;
 
@@ -567,7 +573,6 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
 
   const auto stop_point = getBackwardInsertPointFromBasePoint(base_idx, ego_path, update_margin);
   stop_factor.stop_pose = stop_point.get().second.point.pose;
-  planning_utils::appendStopReason(stop_factor, &stop_reason);
 
   return stop_point;
 }