fix(behavior_velocity_planner): set distance in insert function

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

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

@@ -413,7 +413,6 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findRTC
   const PathWithLaneId & ego_path)
 {
   const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
-  const auto & ego_pos = planner_data_->current_pose.pose.position;
 
   const auto p_stop_line = getStopLine(ego_path);
   if (!p_stop_line) {
@@ -427,9 +426,6 @@ 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;
 
-  setDistance(
-    std::abs(calcSignedArcLength(ego_path.points, ego_pos, p_stop) - margin - base_link2front));
-
   return getBackwardInsertPointFromBasePoint(base_idx, ego_path, update_margin);
 }
 
@@ -541,8 +537,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 {};
   }
 
@@ -569,9 +563,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;
 }
 
@@ -637,6 +628,11 @@ void CrosswalkModule::insertDecelPoint(
       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);
 }