From 250defe83011632a0f2d3629346d5bdf544ae172 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 31 Aug 2023 11:43:31 +0900 Subject: [PATCH 1/3] fix(control_validator): default false for publishing diag and display terminal (#545) default false for publishing diag and display terminal Signed-off-by: kyoichi-sugahara --- .../control/control_validator/control_validator.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/control/control_validator/control_validator.param.yaml b/autoware_launch/config/control/control_validator/control_validator.param.yaml index 9ce677b2c2..c51cbafba2 100644 --- a/autoware_launch/config/control/control_validator/control_validator.param.yaml +++ b/autoware_launch/config/control/control_validator/control_validator.param.yaml @@ -1,14 +1,14 @@ /**: ros__parameters: - publish_diag: true # if true, diagnostic msg is published + publish_diag: false # if true, diagnostic msg is published # If the number of consecutive invalid predicted_path exceeds this threshold, the Diag will be set to ERROR. # (For example, threshold = 1 means, even if the predicted_path is invalid, Diag will not be ERROR if # the next predicted_path is valid.) diag_error_count_threshold: 0 - display_on_terminal: true # show error msg on terminal + display_on_terminal: false # show error msg on terminal thresholds: max_distance_deviation: 1.0 From fb5691ae9f1a40112cdcad7661d909044d6e7e12 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 31 Aug 2023 17:27:55 +0900 Subject: [PATCH 2/3] chore(rviz_config): add localization debug config (#544) Signed-off-by: kminoda --- autoware_launch/rviz/autoware.rviz | 104 ++++++++++++++++++++++++++++- 1 file changed, 103 insertions(+), 1 deletion(-) diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 296d707b13..9b130902a9 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2429,9 +2429,111 @@ Visualization Manager: Enabled: true Name: Sensing - Class: rviz_common/Group - Displays: ~ Enabled: true Name: Localization + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: NDT pointclouds + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: NDTLoadedPCDMap + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/debug/loaded_pointcloud_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: NDTPoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: true + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: EKFPoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true - Class: rviz_common/Group Displays: ~ Enabled: true From 90b98b4d58105a4ac40b6f1e18f0d14e92ca2c05 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 1 Sep 2023 07:56:11 +0900 Subject: [PATCH 3/3] feat(interface): add new option `keep_last` (#543) feat(planner_manager): keep last module Signed-off-by: satoshi-ota --- .../scene_module_manager.param.yaml | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml index 65582656b9..a3c0194d03 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml @@ -8,6 +8,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 7 max_module_size: 1 @@ -16,6 +17,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 7 max_module_size: 1 @@ -24,6 +26,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 @@ -32,6 +35,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 @@ -40,6 +44,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 0 max_module_size: 1 @@ -48,6 +53,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 2 max_module_size: 1 @@ -56,6 +62,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: true priority: 1 max_module_size: 1 @@ -64,6 +71,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 5 max_module_size: 1 @@ -72,6 +80,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 4 max_module_size: 1 @@ -80,5 +89,6 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 3 max_module_size: 1