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