fix(crosswalk): set distance even when decision is GO (#4551)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -252,6 +252,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   // Set safe or unsafe
   setSafe(!nearest_stop_factor);
 
+  // Set distance
+  // NOTE: If no stop point is inserted, distance to the virtual stop line has to be calculated.
+  setDistanceToStop(*path, default_stop_pose, nearest_stop_factor);
+
   // plan Go/Stop
   if (isActivated()) {
     planGo(*path, nearest_stop_factor);
@@ -402,7 +406,7 @@ std::pair<double, double> CrosswalkModule::getAttentionRange(
 
 void CrosswalkModule::insertDecelPointWithDebugInfo(
   const geometry_msgs::msg::Point & stop_point, const float target_velocity,
-  PathWithLaneId & output)
+  PathWithLaneId & output) const
 {
   const auto stop_pose = planning_utils::insertDecelPoint(stop_point, output, target_velocity);
   if (!stop_pose) {
@@ -1007,25 +1011,34 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon(
   return polygon;
 }
 
-void CrosswalkModule::planGo(
-  PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor)
+void CrosswalkModule::setDistanceToStop(
+  const PathWithLaneId & ego_path,
+  const std::optional<geometry_msgs::msg::Pose> & default_stop_pose,
+  const std::optional<StopFactor> & stop_factor)
 {
-  if (!stop_factor) {
-    setDistance(std::numeric_limits<double>::lowest());
-    return;
+  // calculate stop position
+  const auto stop_pos = [&]() -> std::optional<geometry_msgs::msg::Point> {
+    if (stop_factor) return stop_factor->stop_pose.position;
+    if (default_stop_pose) return default_stop_pose->position;
+    return std::nullopt;
+  }();
+
+  // Set distance
+  if (stop_pos) {
+    const auto & ego_pos = planner_data_->current_odometry->pose.position;
+    const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos);
+    setDistance(dist_ego2stop);
   }
+}
 
+void CrosswalkModule::planGo(
+  PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor) const
+{
   // Plan slow down
   const auto target_velocity = calcTargetVelocity(stop_factor->stop_pose.position, ego_path);
   insertDecelPointWithDebugInfo(
     stop_factor->stop_pose.position,
     std::max(planner_param_.min_slow_down_velocity, target_velocity), ego_path);
-
-  // Set distance
-  const auto & ego_pos = planner_data_->current_odometry->pose.position;
-  const double dist_ego2stop =
-    calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position);
-  setDistance(dist_ego2stop);
 }
 
 void CrosswalkModule::planStop(
@@ -1038,22 +1051,11 @@ void CrosswalkModule::planStop(
     return std::nullopt;
   }();
 
-  if (!stop_factor) {
-    setDistance(std::numeric_limits<double>::lowest());
-    return;
-  }
-
   // Plan stop
   insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path);
   planning_utils::appendStopReason(*stop_factor, stop_reason);
   velocity_factor_.set(
     ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose,
     VelocityFactor::UNKNOWN);
-
-  // set distance
-  const auto & ego_pos = planner_data_->current_odometry->pose.position;
-  const double dist_ego2stop =
-    calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position);
-  setDistance(dist_ego2stop);
 }
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -295,7 +295,12 @@ class CrosswalkModule : public SceneModuleInterface
     const std::optional<StopFactor> & stop_factor_for_crosswalk_users,
     const std::optional<StopFactor> & stop_factor_for_stuck_vehicles);
 
-  void planGo(PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor);
+  void setDistanceToStop(
+    const PathWithLaneId & ego_path,
+    const std::optional<geometry_msgs::msg::Pose> & default_stop_pose,
+    const std::optional<StopFactor> & stop_factor);
+
+  void planGo(PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor) const;
 
   void planStop(
     PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
@@ -308,7 +313,7 @@ class CrosswalkModule : public SceneModuleInterface
 
   void insertDecelPointWithDebugInfo(
     const geometry_msgs::msg::Point & stop_point, const float target_velocity,
-    PathWithLaneId & output);
+    PathWithLaneId & output) const;
 
   std::pair<double, double> clampAttentionRangeByNeighborCrosswalks(
     const PathWithLaneId & ego_path, const double near_attention_range,