feat(traffic_light): depend on is_simulation for scenario simulator (… (#1171)

feat(traffic_light): depend on is_simulation for scenario simulator (autowarefoundation#6498)

* feat(traffic_light): depend on is_simulation for scenario simulator

* fix comments

* fix

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>

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);
@@ -298,8 +301,6 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
 {
   std::lock_guard<std::mutex> lock(mutex_);
 
-  planner_data_.has_received_signal_ = true;
-
   for (const auto & signal : msg->signals) {
     TrafficSignalStamped traffic_signal;
     traffic_signal.stamp = msg->stamp;

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -89,8 +89,9 @@ struct PlannerData
   boost::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
   tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
-  // this value becomes true once the signal message is received
-  bool has_received_signal_ = false;
+  // 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_;

planning/behavior_velocity_traffic_light_module/src/scene.cpp

@@ -302,16 +302,15 @@ bool TrafficLightModule::isStopSignal()
 {
   updateTrafficSignal();
 
-  // Pass through if no traffic signal information has been received yet
-  // This is to prevent stopping on the planning simulator
-  if (!planner_data_->has_received_signal_) {
-    return false;
-  }
-
-  // Stop if there is no upcoming traffic signal information
-  // This is to safely stop in cases such that traffic light recognition is not working properly or
-  // the map is incorrect
+  // 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_) {
+    if (planner_data_->is_simulation) {
+      return false;
+    }
     return true;
   }