diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml
index e02ba883d5115..710fb20631a45 100644
--- a/launch/tier4_planning_launch/launch/planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/planning.launch.xml
@@ -9,6 +9,7 @@
+
@@ -25,6 +26,7 @@
+
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
index bae084f80b51e..1ec74793b741b 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
@@ -1,5 +1,6 @@
+
@@ -11,6 +12,7 @@
+
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 04ec688de6f60..29d44f7075d05 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -5,6 +5,7 @@
+
@@ -202,6 +203,7 @@
+
@@ -243,6 +245,7 @@
+
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
index e673d28804480..0a30204ca3c99 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -1,5 +1,6 @@
+
@@ -53,6 +54,7 @@
+
diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp
index 88b09903eef39..b5d019820bebe 100644
--- a/planning/behavior_velocity_planner/src/node.cpp
+++ b/planning/behavior_velocity_planner/src/node.cpp
@@ -150,6 +150,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
declare_parameter("ego_nearest_dist_threshold");
planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold");
+ // is simulation or not
+ planner_data_.is_simulation = declare_parameter("is_simulation");
+
// Initialize PlannerManager
for (const auto & name : declare_parameter>("launch_modules")) {
// workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
@@ -341,6 +344,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I(
std::lock_guard lock(mutex_);
for (const auto & car_light : msg->car_lights) {
+ // If the time to red is not available, skip it
+ if (!car_light.has_max_rest_time || !car_light.has_min_rest_time) {
+ continue;
+ }
+
for (const auto & state : car_light.states) {
TrafficSignalTimeToRedStamped time_to_red;
time_to_red.stamp = msg->header.stamp;
diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp
index 491df4c7a8766..eaba921ea1b1c 100644
--- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp
+++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp
@@ -78,6 +78,7 @@ std::shared_ptr generateNode()
std::vector params;
params.emplace_back("launch_modules", module_names);
+ params.emplace_back("is_simulation", false);
node_options.parameter_overrides(params);
test_utils::updateNodeOptions(
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
index 0ab939f225031..d1f0c39c71205 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
@@ -82,6 +82,10 @@ struct PlannerData
std::map traffic_light_time_to_red_id_map;
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
+ // This variable is used when the Autoware's behavior has to depend on whether it's simulation or
+ // not.
+ bool is_simulation = false;
+
// velocity smoother
std::shared_ptr velocity_smoother_;
// route handler
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index bd80ae6bb5082..23fce55983e18 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -228,33 +228,31 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
first_ref_stop_path_point_index_ = stop_line_point_idx;
- // Check if stop is coming.
- const bool is_stop_signal = isStopSignal();
+ // Update traffic signal information
+ updateTrafficSignal();
+
+ const bool is_unknown_signal = isUnknownSignal(looking_tl_state_);
+ const bool is_signal_timed_out = isTrafficSignalTimedOut();
// Decide if stop or proceed using the remaining time to red signal
+ // If the upcoming traffic signal color is unknown or timed out, do not use the time to red and
+ // stop for the traffic signal
const auto rest_time_to_red_signal =
planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id());
- if (
- planner_param_.v2i_use_rest_time && rest_time_to_red_signal &&
- !isDataTimeout(rest_time_to_red_signal->stamp)) {
- const double rest_time_allowed_to_go_ahead =
- rest_time_to_red_signal->time_to_red - 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);
- }
+ const bool is_stop_required = is_unknown_signal || is_signal_timed_out;
+
+ if (planner_param_.v2i_use_rest_time && rest_time_to_red_signal && !is_stop_required) {
+ if (!canPassStopLineBeforeRed(*rest_time_to_red_signal, signed_arc_length_to_stop_point)) {
+ *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason);
}
return true;
}
+ // Check if stop is coming.
+ const bool is_stop_signal = isStopSignal();
+
// Update stop signal received time
- if (is_stop_signal) {
+ if (is_stop_signal || is_unknown_signal || is_signal_timed_out) {
if (!stop_signal_received_time_ptr_) {
stop_signal_received_time_ptr_ = std::make_unique