File tree 8 files changed +24
-13
lines changed
launch/tier4_planning_launch/launch
lane_driving/behavior_planning
behavior_velocity_planner
behavior_velocity_planner_common/include/behavior_velocity_planner_common
behavior_velocity_traffic_light_module/src
8 files changed +24
-13
lines changed Original file line number Diff line number Diff line change 9
9
<arg name =" planning_validator_param_path" />
10
10
<!-- Auto mode setting-->
11
11
<arg name =" enable_all_modules_auto_mode" />
12
+ <arg name =" is_simulation" />
12
13
13
14
<group >
14
15
<push-ros-namespace namespace =" planning" />
25
26
<push-ros-namespace namespace =" scenario_planning" />
26
27
<include file =" $(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml" >
27
28
<arg name =" enable_all_modules_auto_mode" value =" $(var enable_all_modules_auto_mode)" />
29
+ <arg name =" is_simulation" value =" $(var is_simulation)" />
28
30
</include >
29
31
</group >
30
32
Original file line number Diff line number Diff line change 1
1
<launch >
2
2
<arg name =" enable_all_modules_auto_mode" />
3
+ <arg name =" is_simulation" />
3
4
4
5
<!-- lane_driving scenario -->
5
6
<group >
11
12
<include file =" $(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml" >
12
13
<arg name =" container_type" value =" component_container_mt" />
13
14
<arg name =" enable_all_modules_auto_mode" value =" $(var enable_all_modules_auto_mode)" />
15
+ <arg name =" is_simulation" value =" $(var is_simulation)" />
14
16
<!-- This condition should be true if run_out module is enabled and its detection method is Points -->
15
17
<arg name =" launch_compare_map_pipeline" value =" false" />
16
18
</include >
Original file line number Diff line number Diff line change 5
5
<arg name =" input_vector_map_topic_name" default =" /map/vector_map" />
6
6
<arg name =" input_pointcloud_map_topic_name" default =" /map/pointcloud_map" />
7
7
<arg name =" enable_all_modules_auto_mode" />
8
+ <arg name =" is_simulation" />
8
9
9
10
<arg name =" launch_avoidance_module" default =" true" />
10
11
<arg name =" launch_avoidance_by_lane_change_module" default =" true" />
203
204
<param from =" $(var nearest_search_param_path)" />
204
205
<param name =" launch_modules" value =" $(var behavior_path_planner_launch_modules)" />
205
206
<param name =" enable_all_modules_auto_mode" value =" $(var enable_all_modules_auto_mode)" />
207
+ <param name =" is_simulation" value =" $(var is_simulation)" />
206
208
<param from =" $(var behavior_path_planner_side_shift_module_param_path)" />
207
209
<param from =" $(var behavior_path_planner_avoidance_module_param_path)" />
208
210
<param from =" $(var behavior_path_planner_avoidance_by_lc_module_param_path)" />
245
247
<param from =" $(var behavior_velocity_planner_common_param_path)" />
246
248
<param name =" launch_modules" value =" $(var behavior_velocity_planner_launch_modules)" />
247
249
<param name =" enable_all_modules_auto_mode" value =" $(var enable_all_modules_auto_mode)" />
250
+ <param name =" is_simulation" value =" $(var is_simulation)" />
248
251
<!-- <param from="$(var template_param_path)"/> -->
249
252
<param from =" $(var behavior_velocity_planner_blind_spot_module_param_path)" />
250
253
<param from =" $(var behavior_velocity_planner_crosswalk_module_param_path)" />
Original file line number Diff line number Diff line change 1
1
<launch >
2
2
<arg name =" enable_all_modules_auto_mode" />
3
+ <arg name =" is_simulation" />
3
4
<!-- scenario selector -->
4
5
<group >
5
6
<include file =" $(find-pkg-share scenario_selector)/launch/scenario_selector.launch.xml" >
53
54
<group >
54
55
<include file =" $(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving.launch.xml" >
55
56
<arg name =" enable_all_modules_auto_mode" value =" $(var enable_all_modules_auto_mode)" />
57
+ <arg name =" is_simulation" value =" $(var is_simulation)" />
56
58
</include >
57
59
</group >
58
60
<!-- parking -->
Original file line number Diff line number Diff line change @@ -146,6 +146,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
146
146
declare_parameter<double >(" ego_nearest_dist_threshold" );
147
147
planner_data_.ego_nearest_yaw_threshold = declare_parameter<double >(" ego_nearest_yaw_threshold" );
148
148
149
+ // is simulation or not
150
+ planner_data_.is_simulation = declare_parameter<bool >(" is_simulation" );
151
+
149
152
// Initialize PlannerManager
150
153
for (const auto & name : declare_parameter<std::vector<std::string>>(" launch_modules" )) {
151
154
// workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
@@ -323,8 +326,6 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
323
326
{
324
327
std::lock_guard<std::mutex> lock (mutex_);
325
328
326
- planner_data_.has_received_signal_ = true ;
327
-
328
329
// clear previous observation
329
330
planner_data_.traffic_light_id_map_raw_ .clear ();
330
331
const auto traffic_light_id_map_last_observed_old =
Original file line number Diff line number Diff line change @@ -79,6 +79,7 @@ std::shared_ptr<BehaviorVelocityPlannerNode> generateNode()
79
79
80
80
std::vector<rclcpp::Parameter> params;
81
81
params.emplace_back (" launch_modules" , module_names);
82
+ params.emplace_back (" is_simulation" , false );
82
83
node_options.parameter_overrides (params);
83
84
84
85
test_utils::updateNodeOptions (
Original file line number Diff line number Diff line change @@ -83,8 +83,9 @@ struct PlannerData
83
83
std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
84
84
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
85
85
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 ;
88
89
89
90
// velocity smoother
90
91
std::shared_ptr<motion_velocity_smoother::SmootherBase> velocity_smoother_;
Original file line number Diff line number Diff line change @@ -281,16 +281,15 @@ bool TrafficLightModule::isStopSignal()
281
281
{
282
282
updateTrafficSignal ();
283
283
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.
293
289
if (!traffic_signal_stamp_) {
290
+ if (planner_data_->is_simulation ) {
291
+ return false ;
292
+ }
294
293
return true ;
295
294
}
296
295
You can’t perform that action at this time.
0 commit comments