Merge pull request #1251 from tier4/cherry-pick-ensure-stopping-tlr-2

feat(behavior_velocity_traffic_light): ensure stopping if a signal is not received

Author: TomohitoAndo

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -47,6 +47,7 @@
   <arg name="velocity_smoother_type_param_path"/>
   <!-- planning validator -->
   <arg name="planning_validator_param_path"/>
+  <arg name="is_simulation"/>
 
   <group>
     <push-ros-namespace namespace="planning"/>
@@ -68,6 +69,7 @@
         <arg name="use_surround_obstacle_check" value="$(var use_surround_obstacle_check)"/>
         <arg name="cruise_planner_type" value="$(var cruise_planner_type)"/>
         <arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)"/>
+        <arg name="is_simulation" value="$(var is_simulation)"/>
       </include>
     </group>
 

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

@@ -1,4 +1,6 @@
 <launch>
+  <arg name="is_simulation"/>
+
   <!-- lane_driving scenario -->
   <group>
     <push-ros-namespace namespace="lane_driving"/>
@@ -12,6 +14,7 @@
           <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
           <arg name="container_name" value="$(var pointcloud_container_name)"/>
           <arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)"/>
+          <arg name="is_simulation" value="$(var is_simulation)"/>
         </include>
       </group>
       <group>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -205,6 +205,7 @@ def launch_setup(context, *args, **kwargs):
             common_param,
             motion_velocity_smoother_param,
             behavior_velocity_smoother_type_param,
+            {"is_simulation": LaunchConfiguration("is_simulation")},
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
@@ -301,6 +302,9 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("use_pointcloud_container", "true")
     add_launch_arg("container_name", "pointcloud_container")
 
+    # whether this is a simulation or not
+    add_launch_arg("is_simulation", "false", "whether this is a simulation or not")
+
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
         "component_container",

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

@@ -1,4 +1,5 @@
 <launch>
+  <arg name="is_simulation"/>
   <!-- scenario selector -->
   <group>
     <include file="$(find-pkg-share scenario_selector)/launch/scenario_selector.launch.xml">
@@ -58,6 +59,7 @@
         <arg name="use_surround_obstacle_check" value="$(var use_surround_obstacle_check)"/>
         <arg name="cruise_planner_type" value="$(var cruise_planner_type)"/>
         <arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)"/>
+        <arg name="is_simulation" value="$(var is_simulation)"/>
       </include>
     </group>
     <!-- parking -->

planning/behavior_velocity_planner/src/node.cpp

@@ -141,6 +141,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
   planner_data_.ego_nearest_yaw_threshold =
     this->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")) {
     planner_manager_.launchScenePlugin(*this, name);

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -89,6 +89,10 @@ struct PlannerData
   boost::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
   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

@@ -302,11 +302,19 @@ 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 (planner_data_->is_simulation) {
+      return false;
+    }
+    return true;
   }
 
+  // Stop if the traffic signal information has timed out
   if (isTrafficSignalTimedOut()) {
     return true;
   }