fix(behavior_velocity_planner): output stop reason when RTC sends dea…

…ctivate command (tier4#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>
boyali · Sep 28, 2022 · 1c552c7 · 1c552c7
1 parent 939022f
commit 1c552c7
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 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
@@ -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
@@ -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;
 }