fix(behavior_velocity_planner): fix rtc behavior in crosswalk module (t…

…ier4#1296)

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

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml

@@ -10,7 +10,7 @@
       stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk
 
       # param for ego velocity
-      slow_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
+      min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
       max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake
       max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake
       no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph)

planning/behavior_velocity_planner/config/crosswalk.param.yaml

@@ -10,7 +10,7 @@
       stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk
 
       # param for ego velocity
-      slow_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
+      min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
       max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake
       max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake
       no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph)

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

@@ -68,7 +68,7 @@ class CrosswalkModule : public SceneModuleInterface
     double stop_line_margin;
     double stop_position_threshold;
     // param for ego velocity
-    double slow_velocity;
+    float min_slow_down_velocity;
     double max_slow_down_jerk;
     double max_slow_down_accel;
     double no_relax_velocity;

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

@@ -103,7 +103,7 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   cp.stop_position_threshold = node.declare_parameter(ns + ".stop_position_threshold", 1.0);
 
   // param for ego velocity
-  cp.slow_velocity = node.declare_parameter(ns + ".slow_velocity", 5.0 / 3.6);
+  cp.min_slow_down_velocity = node.declare_parameter(ns + ".min_slow_down_velocity", 5.0 / 3.6);
   cp.max_slow_down_jerk = node.declare_parameter(ns + ".max_slow_down_jerk", -0.7);
   cp.max_slow_down_accel = node.declare_parameter(ns + ".max_slow_down_accel", -2.5);
   cp.no_relax_velocity = node.declare_parameter(ns + ".no_relax_velocity", 2.78);

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

@@ -363,20 +363,23 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
 
   setSafe(!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);
+    if (!nearest_stop_point) {
+      return true;
     }
+
+    const auto target_velocity = calcTargetVelocity(nearest_stop_point.get().second, ego_path);
+    insertDecelPoint(
+      nearest_stop_point.get(), std::max(planner_param_.min_slow_down_velocity, target_velocity),
+      *path);
+
     return true;
   }
 
   if (nearest_stop_point) {
-    insertDecelPoint(nearest_stop_point.get(), target_velocity, *path);
+    insertDecelPoint(nearest_stop_point.get(), 0.0, *path);
   } else if (rtc_stop_point) {
-    insertDecelPoint(rtc_stop_point.get(), target_velocity, *path);
+    insertDecelPoint(rtc_stop_point.get(), 0.0, *path);
   }
 
   RCLCPP_INFO_EXPRESSION(
@@ -442,13 +445,6 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
 
   bool found_pedestrians = false;
   bool found_stuck_vehicle = false;
-  const bool external_slowdown = isTargetExternalInputStatus(CrosswalkStatus::SLOWDOWN);
-  const bool external_stop = isTargetExternalInputStatus(CrosswalkStatus::STOP);
-  const bool external_go = isTargetExternalInputStatus(CrosswalkStatus::GO);
-
-  if (external_go) {
-    return {};
-  }
 
   const auto crosswalk_attention_range = getAttentionRange(ego_path);
   const auto & ego_pos = planner_data_->current_pose.pose.position;
@@ -540,8 +536,7 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
     }
   }
 
-  const auto need_to_stop =
-    found_pedestrians || found_stuck_vehicle || external_stop || external_slowdown;
+  const auto need_to_stop = found_pedestrians || found_stuck_vehicle;
   if (!need_to_stop) {
     return {};
   }
@@ -646,18 +641,6 @@ void CrosswalkModule::insertDecelPoint(
 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;
-  }
-
-  if (isTargetExternalInputStatus(CrosswalkStatus::STOP)) {
-    return 0.0;
-  }
-
   const auto & max_jerk = planner_param_.max_slow_down_jerk;
   const auto & max_accel = planner_param_.max_slow_down_accel;
   const auto & ego_pos = planner_data_->current_pose.pose.position;