From eb15a2be1a7f4c9df5bb23b825a942fc8f65e697 Mon Sep 17 00:00:00 2001
From: Tomohito ANDO <tomohito.ando@tier4.jp>
Date: Fri, 8 Mar 2024 11:43:01 +0900
Subject: [PATCH] =?UTF-8?q?feat(traffic=5Flight):=20depend=20on=20is=5Fsim?=
 =?UTF-8?q?ulation=20for=20scenario=20simulator=20(=E2=80=A6=20(#1171)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

feat(traffic_light): depend on is_simulation for scenario simulator (#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>
---
 .../launch/planning.launch.xml                  |  2 ++
 .../scenario_planning/lane_driving.launch.xml   |  2 ++
 .../behavior_planning.launch.xml                |  3 +++
 .../scenario_planning.launch.xml                |  2 ++
 planning/behavior_velocity_planner/src/node.cpp |  5 +++--
 .../test/src/test_node_interface.cpp            |  1 +
 .../planner_data.hpp                            |  5 +++--
 .../src/scene.cpp                               | 17 ++++++++---------
 8 files changed, 24 insertions(+), 13 deletions(-)

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 @@
   <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>
 
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 @@
 <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>
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 @@
   <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)"/>
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 @@
 <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 -->
diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp
index 1cb04a37724c5..2ece273715425 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<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.
@@ -327,8 +330,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;
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<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(
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 2b361ae8c8bec..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,8 +82,9 @@ struct PlannerData
   std::map<int, TrafficSignalTimeToRedStamped> traffic_light_time_to_red_id_map;
   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_;
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index 23a7c2e757795..aa6d5297b7f99 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -303,16 +303,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;
   }