Merge branch 'beta/v0.19.1' into cherry-pick/pr6438+6470

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -9,6 +9,7 @@
   <arg name="planning_validator_param_path"/>
   <!-- Auto mode setting-->
   <arg name="enable_all_modules_auto_mode"/>
+  <arg name="is_simulation"/>
 
   <group>
     <push-ros-namespace namespace="planning"/>
@@ -25,6 +26,7 @@
       <push-ros-namespace namespace="scenario_planning"/>
       <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
         <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+        <arg name="is_simulation" value="$(var is_simulation)"/>
       </include>
     </group>
 

launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml

@@ -1,5 +1,6 @@
 <launch>
   <arg name="enable_all_modules_auto_mode"/>
+  <arg name="is_simulation"/>
 
   <!-- lane_driving scenario -->
   <group>
@@ -11,6 +12,7 @@
         <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml">
           <arg name="container_type" value="component_container_mt"/>
           <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+          <arg name="is_simulation" value="$(var is_simulation)"/>
           <!-- This condition should be true if run_out module is enabled and its detection method is Points -->
           <arg name="launch_compare_map_pipeline" value="false"/>
         </include>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -5,6 +5,7 @@
   <arg name="input_vector_map_topic_name" default="/map/vector_map"/>
   <arg name="input_pointcloud_map_topic_name" default="/map/pointcloud_map"/>
   <arg name="enable_all_modules_auto_mode"/>
+  <arg name="is_simulation"/>
 
   <arg name="launch_avoidance_module" default="true"/>
   <arg name="launch_avoidance_by_lane_change_module" default="true"/>
@@ -202,6 +203,7 @@
       <param from="$(var nearest_search_param_path)"/>
       <param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
       <param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+      <param name="is_simulation" value="$(var is_simulation)"/>
       <param from="$(var behavior_path_planner_side_shift_module_param_path)"/>
       <param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
       <param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
@@ -243,6 +245,7 @@
       <param from="$(var behavior_velocity_smoother_type_param_path)"/>
       <param from="$(var behavior_velocity_planner_common_param_path)"/>
       <param name="launch_modules" value="$(var behavior_velocity_planner_launch_modules)"/>
+      <param name="is_simulation" value="$(var is_simulation)"/>
       <!-- <param from="$(var template_param_path)"/> -->
       <param from="$(var behavior_velocity_planner_blind_spot_module_param_path)"/>
       <param from="$(var behavior_velocity_planner_crosswalk_module_param_path)"/>

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -1,5 +1,6 @@
 <launch>
   <arg name="enable_all_modules_auto_mode"/>
+  <arg name="is_simulation"/>
   <!-- scenario selector -->
   <group>
     <include file="$(find-pkg-share scenario_selector)/launch/scenario_selector.launch.xml">
@@ -53,6 +54,7 @@
     <group>
       <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving.launch.xml">
         <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+        <arg name="is_simulation" value="$(var is_simulation)"/>
       </include>
     </group>
     <!-- parking -->

planning/behavior_velocity_planner/src/node.cpp

@@ -150,6 +150,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
     declare_parameter<double>("ego_nearest_dist_threshold");
   planner_data_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
+  // is simulation or not
+  planner_data_.is_simulation = declare_parameter<bool>("is_simulation");
+
   // Initialize PlannerManager
   for (const auto & name : declare_parameter<std::vector<std::string>>("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<std::mutex> 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;

planning/behavior_velocity_planner/test/src/test_node_interface.cpp

@@ -78,6 +78,7 @@ std::shared_ptr<BehaviorVelocityPlannerNode> generateNode()
 
   std::vector<rclcpp::Parameter> params;
   params.emplace_back("launch_modules", module_names);
+  params.emplace_back("is_simulation", false);
   node_options.parameter_overrides(params);
 
   test_utils::updateNodeOptions(

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -82,6 +82,10 @@ struct PlannerData
   std::map<int, TrafficSignalTimeToRedStamped> 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<motion_velocity_smoother::SmootherBase> velocity_smoother_;
   // route handler

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<Time>(clock_->now());
       }
@@ -301,14 +299,15 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
 bool TrafficLightModule::isStopSignal()
 {
-  updateTrafficSignal();
-
-  // If it never receives traffic signal, it will PASS.
+  // If there is no upcoming traffic signal information,
+  //   SIMULATION: it will PASS to prevent stopping on the planning simulator
+  //   or scenario simulator.
+  //   REAL ENVIRONMENT: it will STOP for safety in cases such that traffic light
+  //   recognition is not working properly or the map is incorrect.
   if (!traffic_signal_stamp_) {
-    return false;
-  }
-
-  if (isTrafficSignalTimedOut()) {
+    if (planner_data_->is_simulation) {
+      return false;
+    }
     return true;
   }
 
@@ -514,4 +513,39 @@ bool TrafficLightModule::isDataTimeout(const rclcpp::Time & data_time) const
   return is_data_timeout;
 }
 
+bool TrafficLightModule::canPassStopLineBeforeRed(
+  const TrafficSignalTimeToRedStamped & time_to_red_signal,
+  const double distance_to_stop_line) const
+{
+  if (isDataTimeout(time_to_red_signal.stamp)) {
+    return false;
+  }
+
+  const double rest_time_allowed_to_go_ahead =
+    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 <= distance_to_stop_line) {
+      return false;
+    }
+  } else {
+    if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool TrafficLightModule::isUnknownSignal(const TrafficSignal & tl_state) const
+{
+  if (tl_state.elements.empty()) {
+    return false;
+  }
+
+  const bool has_unknown = hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN);
+  return has_unknown;
+}
+
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_traffic_light_module/src/scene.hpp

@@ -110,10 +110,16 @@ class TrafficLightModule : public SceneModuleInterface
 
   bool isTrafficSignalTimedOut() const;
 
+  bool isUnknownSignal(const TrafficSignal & tl_state) const;
+
   void updateTrafficSignal();
 
   bool isDataTimeout(const rclcpp::Time & data_time) const;
 
+  bool canPassStopLineBeforeRed(
+    const TrafficSignalTimeToRedStamped & time_to_red_signal,
+    const double distance_to_stop_line) const;
+
   // Lane id
   const int64_t lane_id_;
 

system/duplicated_node_checker/config/duplicated_node_checker.param.yaml

@@ -1,4 +1,4 @@
 /**:
   ros__parameters:
     update_rate: 10.0
-    add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg
+    add_duplicated_node_names_to_msg: true # if true, duplicated node names are added to msg

system/duplicated_node_checker/package.xml

@@ -6,6 +6,7 @@
   <description>A package to find out nodes with common names</description>
   <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
   <maintainer email="uken.ryu@tier4.jp">yliuhb</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

system/duplicated_node_checker/src/duplicated_node_checker_core.cpp

@@ -69,7 +69,7 @@ void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticSta
     if (add_duplicated_node_names_to_msg_) {
       std::set<std::string> unique_identical_names(identical_names.begin(), identical_names.end());
       for (const auto & name : unique_identical_names) {
-        msg += " " + name;
+        msg += "[" + name + "], ";
       }
     }
     for (auto name : identical_names) {