Skip to content

Commit df96678

Browse files
takayuki5168HansRobo
authored andcommitted
feat(traffic_light): depend on is_simulation for scenario simulator (#6498)
* feat(traffic_light): depend on is_simulation for scenario simulator Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix comments Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
1 parent 433a59a commit df96678

File tree

8 files changed

+24
-13
lines changed

8 files changed

+24
-13
lines changed

launch/tier4_planning_launch/launch/planning.launch.xml

+2
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
<arg name="planning_validator_param_path"/>
1010
<!-- Auto mode setting-->
1111
<arg name="enable_all_modules_auto_mode"/>
12+
<arg name="is_simulation"/>
1213

1314
<group>
1415
<push-ros-namespace namespace="planning"/>
@@ -25,6 +26,7 @@
2526
<push-ros-namespace namespace="scenario_planning"/>
2627
<include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
2728
<arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
29+
<arg name="is_simulation" value="$(var is_simulation)"/>
2830
</include>
2931
</group>
3032

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

+2
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
<launch>
22
<arg name="enable_all_modules_auto_mode"/>
3+
<arg name="is_simulation"/>
34

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

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

+3
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
<arg name="input_vector_map_topic_name" default="/map/vector_map"/>
66
<arg name="input_pointcloud_map_topic_name" default="/map/pointcloud_map"/>
77
<arg name="enable_all_modules_auto_mode"/>
8+
<arg name="is_simulation"/>
89

910
<arg name="launch_avoidance_module" default="true"/>
1011
<arg name="launch_avoidance_by_lane_change_module" default="true"/>
@@ -203,6 +204,7 @@
203204
<param from="$(var nearest_search_param_path)"/>
204205
<param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
205206
<param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
207+
<param name="is_simulation" value="$(var is_simulation)"/>
206208
<param from="$(var behavior_path_planner_side_shift_module_param_path)"/>
207209
<param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
208210
<param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
@@ -245,6 +247,7 @@
245247
<param from="$(var behavior_velocity_planner_common_param_path)"/>
246248
<param name="launch_modules" value="$(var behavior_velocity_planner_launch_modules)"/>
247249
<param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
250+
<param name="is_simulation" value="$(var is_simulation)"/>
248251
<!-- <param from="$(var template_param_path)"/> -->
249252
<param from="$(var behavior_velocity_planner_blind_spot_module_param_path)"/>
250253
<param from="$(var behavior_velocity_planner_crosswalk_module_param_path)"/>

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

+2
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
<launch>
22
<arg name="enable_all_modules_auto_mode"/>
3+
<arg name="is_simulation"/>
34
<!-- scenario selector -->
45
<group>
56
<include file="$(find-pkg-share scenario_selector)/launch/scenario_selector.launch.xml">
@@ -53,6 +54,7 @@
5354
<group>
5455
<include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving.launch.xml">
5556
<arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
57+
<arg name="is_simulation" value="$(var is_simulation)"/>
5658
</include>
5759
</group>
5860
<!-- parking -->

planning/behavior_velocity_planner/src/node.cpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
146146
declare_parameter<double>("ego_nearest_dist_threshold");
147147
planner_data_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
148148

149+
// is simulation or not
150+
planner_data_.is_simulation = declare_parameter<bool>("is_simulation");
151+
149152
// Initialize PlannerManager
150153
for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
151154
// workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
@@ -323,8 +326,6 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
323326
{
324327
std::lock_guard<std::mutex> lock(mutex_);
325328

326-
planner_data_.has_received_signal_ = true;
327-
328329
// clear previous observation
329330
planner_data_.traffic_light_id_map_raw_.clear();
330331
const auto traffic_light_id_map_last_observed_old =

planning/behavior_velocity_planner/test/src/test_node_interface.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,7 @@ std::shared_ptr<BehaviorVelocityPlannerNode> generateNode()
7979

8080
std::vector<rclcpp::Parameter> params;
8181
params.emplace_back("launch_modules", module_names);
82+
params.emplace_back("is_simulation", false);
8283
node_options.parameter_overrides(params);
8384

8485
test_utils::updateNodeOptions(

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -83,8 +83,9 @@ struct PlannerData
8383
std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
8484
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
8585

86-
// this value becomes true once the signal message is received
87-
bool has_received_signal_ = false;
86+
// This variable is used when the Autoware's behavior has to depend on whether it's simulation or
87+
// not.
88+
bool is_simulation = false;
8889

8990
// velocity smoother
9091
std::shared_ptr<motion_velocity_smoother::SmootherBase> velocity_smoother_;

planning/behavior_velocity_traffic_light_module/src/scene.cpp

+8-9
Original file line numberDiff line numberDiff line change
@@ -281,16 +281,15 @@ bool TrafficLightModule::isStopSignal()
281281
{
282282
updateTrafficSignal();
283283

284-
// Pass through if no traffic signal information has been received yet
285-
// This is to prevent stopping on the planning simulator
286-
if (!planner_data_->has_received_signal_) {
287-
return false;
288-
}
289-
290-
// Stop if there is no upcoming traffic signal information
291-
// This is to safely stop in cases such that traffic light recognition is not working properly or
292-
// the map is incorrect
284+
// If there is no upcoming traffic signal information,
285+
// SIMULATION: it will PASS to prevent stopping on the planning simulator
286+
// or scenario simulator.
287+
// REAL ENVIRONMENT: it will STOP for safety in cases such that traffic light
288+
// recognition is not working properly or the map is incorrect.
293289
if (!traffic_signal_stamp_) {
290+
if (planner_data_->is_simulation) {
291+
return false;
292+
}
294293
return true;
295294
}
296295

0 commit comments

Comments
 (0)