fix(behavior_velocity_planner): fix rtc in crosswalk module (tier4#1273)

* fix(behavior_velocity_planner): fix rtc in crosswalk module

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

* fix(behavior_velocity_planner): set distance in insert function

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

* fix(behavior_velocity_planner): update activated hehavior

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

planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp

@@ -106,6 +106,9 @@ class CrosswalkModule : public SceneModuleInterface
 private:
   int64_t module_id_;
 
+  boost::optional<std::pair<size_t, PathPointWithLaneId>> findRTCStopPoint(
+    const PathWithLaneId & ego_path);
+
   boost::optional<std::pair<size_t, PathPointWithLaneId>> findNearestStopPoint(
     const PathWithLaneId & ego_path, StopReason & stop_reason);
 
@@ -119,7 +122,8 @@ class CrosswalkModule : public SceneModuleInterface
   std::pair<double, double> getAttentionRange(const PathWithLaneId & ego_path);
 
   void insertDecelPoint(
-    const std::pair<size_t, PathPointWithLaneId> & stop_point, PathWithLaneId & output);
+    const std::pair<size_t, PathPointWithLaneId> & stop_point, const float target_velocity,
+    PathWithLaneId & output);
 
   void clampAttentionRangeByNeighborCrosswalks(
     const PathWithLaneId & ego_path, double & near_attention_range, double & far_attention_range);

planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp

@@ -355,19 +355,28 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     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);
 
   RCLCPP_INFO_EXPRESSION(
     logger_, planner_param_.show_processing_time, "- step3: %f ms",
     stop_watch_.toc("total_processing_time", false));
 
   setSafe(!nearest_stop_point);
 
-  if (!nearest_stop_point) {
+  const auto target_velocity =
+    nearest_stop_point ? calcTargetVelocity(nearest_stop_point.get().second, ego_path) : 0.0;
+
+  if (isActivated()) {
+    if (1e-3 < target_velocity) {
+      insertDecelPoint(nearest_stop_point.get(), target_velocity, *path);
+    }
     return true;
   }
 
-  if (!isActivated()) {
-    insertDecelPoint(nearest_stop_point.get(), *path);
+  if (nearest_stop_point) {
+    insertDecelPoint(nearest_stop_point.get(), target_velocity, *path);
+  } else if (rtc_stop_point) {
+    insertDecelPoint(rtc_stop_point.get(), target_velocity, *path);
   }
 
   RCLCPP_INFO_EXPRESSION(
@@ -406,6 +415,26 @@ boost::optional<std::pair<double, geometry_msgs::msg::Point>> CrosswalkModule::g
   return {};
 }
 
+boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findRTCStopPoint(
+  const PathWithLaneId & ego_path)
+{
+  const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
+
+  const auto p_stop_line = getStopLine(ego_path);
+  if (!p_stop_line) {
+    return {};
+  }
+
+  const auto & p_stop = p_stop_line.get().second;
+  const auto & margin = planner_param_.stop_line_distance;
+
+  const size_t base_idx = findNearestSegmentIndex(ego_path.points, p_stop);
+  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);
+}
+
 boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNearestStopPoint(
   const PathWithLaneId & ego_path, StopReason & stop_reason)
 {
@@ -514,8 +543,6 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
   const auto need_to_stop =
     found_pedestrians || found_stuck_vehicle || external_stop || external_slowdown;
   if (!need_to_stop) {
-    setDistance(
-      std::abs(p_stop_line.get().first - planner_param_.stop_line_distance - base_link2front));
     return {};
   }
 
@@ -542,9 +569,6 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
   stop_factor.stop_pose = stop_point.get().second.point.pose;
   planning_utils::appendStopReason(stop_factor, &stop_reason);
 
-  setDistance(
-    std::abs(calcSignedArcLength(ego_path.points, ego_pos, p_stop) - margin - base_link2front));
-
   return stop_point;
 }
 
@@ -576,7 +600,8 @@ std::pair<double, double> CrosswalkModule::getAttentionRange(const PathWithLaneI
 }
 
 void CrosswalkModule::insertDecelPoint(
-  const std::pair<size_t, PathPointWithLaneId> & stop_point, PathWithLaneId & output)
+  const std::pair<size_t, PathPointWithLaneId> & stop_point, const float target_velocity,
+  PathWithLaneId & output)
 {
   const auto traj_end_idx = output.points.size() - 1;
   const auto & stop_idx = stop_point.first;
@@ -603,20 +628,28 @@ void CrosswalkModule::insertDecelPoint(
     update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx);
   }
 
-  const auto target_velocity = calcTargetVelocity(p_insert, output);
   for (size_t i = update_stop_idx; i < output.points.size(); ++i) {
     const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps;
     output.points.at(i).point.longitudinal_velocity_mps =
       std::min(original_velocity, target_velocity);
   }
 
+  const auto & ego_pos = planner_data_->current_pose.pose.position;
+  const auto & stop_point_distance =
+    calcSignedArcLength(output.points, ego_pos, getPoint(stop_point.second));
+  setDistance(std::abs(stop_point_distance));
+
   debug_data_.first_stop_pose = stop_point.second.point.pose;
   debug_data_.stop_poses.push_back(stop_point.second.point.pose);
 }
 
 float CrosswalkModule::calcTargetVelocity(
   const PathPointWithLaneId & stop_point, const PathWithLaneId & ego_path) const
 {
+  if (!isActivated()) {
+    return 0.0;
+  }
+
   if (isTargetExternalInputStatus(CrosswalkStatus::SLOWDOWN)) {
     return planner_param_.slow_velocity;
   }