address non-V2I environments

Signed-off-by: Yuki Takagi <[email protected]>

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -141,12 +141,14 @@ struct PlannerData
     }
     return std::make_shared<TrafficSignalStamped>(traffic_light_id_map.at(id));
   }
-  double getRestTimeToRedSignal(const int id) const
+
+  std::optional<double> getRestTimeToRedSignal(const int id) const
   {
-    if (traffic_light_time_to_red_id_map.count(id) == 0) {
-      return std::numeric_limits<double>::max();
+    try {
+      return traffic_light_time_to_red_id_map.at(id);
+    } catch (std::out_of_range &) {
+      return std::nullopt;
     }
-    return traffic_light_time_to_red_id_map.at(id);
   }
 };
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml

@@ -8,6 +8,7 @@
       enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
 
       v2i:
-        last_time_allowed_to_pass: 2.0 # # relative time against at the time of turn to red
+        use_rest_time: true
+        last_time_allowed_to_pass: 2.0 # relative time against at the time of turn to red
         velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not
-        required_time_to_departure: 3.0
+        required_time_to_departure: 3.0 # prevent low speed pass

planning/behavior_velocity_traffic_light_module/src/manager.cpp

@@ -41,6 +41,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node)
   planner_param_.enable_pass_judge = getOrDeclareParameter<bool>(node, ns + ".enable_pass_judge");
   planner_param_.yellow_lamp_period =
     getOrDeclareParameter<double>(node, ns + ".yellow_lamp_period");
+  planner_param_.v2i_use_rest_time = getOrDeclareParameter<bool>(node, ns + ".v2i.use_rest_time");
   planner_param_.v2i_last_time_allowed_to_pass =
     getOrDeclareParameter<double>(node, ns + ".v2i.last_time_allowed_to_pass");
   planner_param_.v2i_velocity_threshold =

planning/behavior_velocity_traffic_light_module/src/scene.cpp

@@ -225,36 +225,33 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
     first_ref_stop_path_point_index_ = stop_line_point_idx;
 
-    const double rest_time_to_red_signal =
+    const std::optional<double> rest_time_to_red_signal =
       planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id());
-    const double rest_time_allowed_to_go_ahead =
-      rest_time_to_red_signal - planner_param_.v2i_last_time_allowed_to_pass;
-
-    RCLCPP_DEBUG(
-      logger_, "\nrest_time_allowed_to_go_ahead: %2.2f = %2.2f - %2.2f",
-      rest_time_allowed_to_go_ahead, rest_time_to_red_signal,
-      planner_param_.v2i_last_time_allowed_to_pass);
-
-    const double ego_v = planner_data_->current_velocity->twist.linear.x;
-    if (ego_v >= planner_param_.v2i_velocity_threshold) {
-      RCLCPP_DEBUG(
-        logger_, "\nego moves enough fast %2.2f > %2.2f", ego_v,
-        planner_param_.v2i_velocity_threshold);
-      if (ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) {
-        RCLCPP_DEBUG(
-          logger_, "\nplan to stop because ego will not reach %2.2f <= %2.2f",
-          ego_v * rest_time_allowed_to_go_ahead, signed_arc_length_to_stop_point);
-        *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason);
-      }
-    } else {
-      if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) {
-        RCLCPP_DEBUG(
-          logger_, "\nplan to stop because there is enough rest time to departure %2.2f < %2.2f",
-          rest_time_allowed_to_go_ahead, planner_param_.v2i_required_time_to_departure);
-        *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason);
+    if (planner_param_.v2i_use_rest_time && rest_time_to_red_signal) {
+      const double rest_time_allowed_to_go_ahead =
+        rest_time_to_red_signal.value() - planner_param_.v2i_last_time_allowed_to_pass;
+
+      const double ego_v = planner_data_->current_velocity->twist.linear.x;
+      if (ego_v >= planner_param_.v2i_velocity_threshold) {
+        if (ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) {
+          *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason);
+        }
+      } else {
+        if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) {
+          *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason);
+        }
       }
+      return true;
+    }
+    setSafe(!isStopSignal());
+    if (isActivated()) {
+      is_prev_state_stop_ = false;
+      return true;
+    }
+    if (!isPassthrough(signed_arc_length_to_stop_point)) {
+      *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason);
+      is_prev_state_stop_ = true;
     }
-    return true;
   } else if (state_ == State::GO_OUT) {
     // Initialize if vehicle is far from stop_line
     constexpr bool use_initialization_after_start = true;
@@ -295,8 +292,7 @@ bool TrafficLightModule::updateTrafficSignal()
   return true;
 }
 
-bool TrafficLightModule::isPassthrough(
-  const double & signed_arc_length) const  // unused in the V2I temporary implementation
+bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
 {
   const double max_acc = planner_data_->max_stop_acceleration_threshold;
   const double max_jerk = planner_data_->max_stop_jerk_threshold;

planning/behavior_velocity_traffic_light_module/src/scene.hpp

@@ -61,6 +61,7 @@ class TrafficLightModule : public SceneModuleInterface
     double tl_state_timeout;
     double yellow_lamp_period;
     bool enable_pass_judge;
+    bool v2i_use_rest_time;
     double v2i_last_time_allowed_to_pass;
     double v2i_velocity_threshold;
     double v2i_required_time_to_departure;