Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
Show file tree
Hide file tree
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions launch/tier4_planning_launch/launch/planning.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand All @@ -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>

Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
<launch>
<arg name="enable_all_modules_auto_mode"/>
<arg name="is_simulation"/>

<!-- lane_driving scenario -->
<group>
Expand All @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down Expand Up @@ -203,6 +204,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)"/>
Expand Down Expand Up @@ -245,6 +247,7 @@
<param from="$(var behavior_velocity_planner_common_param_path)"/>
<param name="launch_modules" value="$(var behavior_velocity_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 template_param_path)"/> -->
<param from="$(var behavior_velocity_planner_blind_spot_module_param_path)"/>
<param from="$(var behavior_velocity_planner_crosswalk_module_param_path)"/>
Expand Down
Original file line number Diff line number Diff line change
@@ -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">
Expand Down Expand Up @@ -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 -->
Expand Down
5 changes: 3 additions & 2 deletions planning/behavior_velocity_planner/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,9 @@
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");

Check warning on line 151 in planning/behavior_velocity_planner/src/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method

BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode increases from 72 to 73 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
// 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.
Expand Down Expand Up @@ -323,8 +326,6 @@
{
std::lock_guard<std::mutex> lock(mutex_);

planner_data_.has_received_signal_ = true;

// clear previous observation
planner_data_.traffic_light_id_map_raw_.clear();
const auto traffic_light_id_map_last_observed_old =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,9 @@ struct PlannerData
std::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_;
Expand Down
17 changes: 8 additions & 9 deletions planning/behavior_velocity_traffic_light_module/src/scene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -281,16 +281,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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

unnecessary space

Suggested change
// recognition is not working properly or the map is incorrect.
// recognition is not working properly or the map is incorrect.

if (!traffic_signal_stamp_) {
if (planner_data_->is_simulation) {
return false;
}
return true;
}

Expand Down
Loading