From 5c2c1ace2974a9d9fc4e2a14b9a86217c0084644 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Wed, 8 Dec 2021 13:57:55 +0900 Subject: [PATCH 1/9] update global_parameter loader readme Signed-off-by: tomoya.kimura --- common/autoware_global_parameter_loader/Readme.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common/autoware_global_parameter_loader/Readme.md b/common/autoware_global_parameter_loader/Readme.md index 017df849b169..31dc8432c214 100644 --- a/common/autoware_global_parameter_loader/Readme.md +++ b/common/autoware_global_parameter_loader/Readme.md @@ -13,6 +13,8 @@ Add the following lines to the launch file of the node in which you want to get ``` +The vehicle model parameter is read from `config/vehicle_info.param.yaml` in `vehicle_model`_description package. + ## Assumptions / Known limits Currently only vehicle_info is loaded by this launcher. From 16265764e11b786cf495057f1d373edadd63dd71 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Wed, 8 Dec 2021 13:58:13 +0900 Subject: [PATCH 2/9] remove unused dependency Signed-off-by: tomoya.kimura --- perception/occupancy_grid_map_outlier_filter/package.xml | 1 - sensing/pointcloud_preprocessor/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml index 882def4a80e8..b420de794de4 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -40,7 +40,6 @@ tf2_ros tier4_debug_msgs tier4_pcl_extensions - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index d28cca0073d8..0fd2a175c183 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -39,7 +39,6 @@ tf2_ros tier4_debug_msgs tier4_pcl_extensions - vehicle_info_util ament_lint_auto autoware_lint_common From 58fc6a5b45bce2c615d76418cb0373902b76dc7d Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Wed, 8 Dec 2021 13:58:30 +0900 Subject: [PATCH 3/9] add default vehicle_info_param to launch files Signed-off-by: tomoya.kimura --- .../controller_performance_analysis.launch.xml | 3 ++- .../launch/lane_departure_checker.launch.xml | 4 ++++ .../launch/obstacle_collision_checker.launch.xml | 3 +++ .../launch/vehicle_cmd_gate.launch.xml | 4 ++++ .../launch/scan_ground_filter.launch.py | 14 +++++++++++++- .../launch/behavior_path_planner.launch.xml | 4 ++++ .../launch/behavior_velocity_planner.launch.xml | 4 +++- .../launch/freespace_planner.launch.xml | 4 ++++ .../launch/obstacle_stop_planner.launch.xml | 4 ++++ .../launch/surround_obstacle_checker.launch.xml | 4 ++++ .../launch/simple_planning_simulator.launch.py | 12 ++++++++++++ .../launch/pacmod_interface.launch.xml | 3 +++ 12 files changed, 60 insertions(+), 3 deletions(-) diff --git a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml index 8a248c9e8745..433ae43f371f 100644 --- a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml +++ b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml @@ -8,10 +8,11 @@ - + + diff --git a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml index 29db32f53e56..3be1bae25bfa 100644 --- a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml +++ b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml @@ -6,6 +6,9 @@ + + + @@ -13,5 +16,6 @@ + diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index 1391692ad6b8..e63f0d6dbc50 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -5,9 +5,12 @@ + + + diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index 8658ea51d84c..0d78d4de5c3c 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -4,6 +4,9 @@ + + + @@ -42,6 +45,7 @@ + diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/ground_segmentation/launch/scan_ground_filter.launch.py index 10617cc155ef..2454fbb69a37 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.py @@ -26,6 +26,16 @@ def generate_launch_description(): def add_launch_arg(name: str, default_value=None): return DeclareLaunchArgument(name, default_value=default_value) + default_vehicle_info_param = os.path.join( + get_package_share_directory('vehicle_info_util'), + 'config/vehicle_info.param.yaml') + + vehicle_info_param = DeclareLaunchArgument( + 'vehicle_info_param_file', + default_value=default_vehicle_info_param, + description='Path to config file for vehicle information' + ) + nodes = [ ComposableNode( package="ground_segmentation", @@ -41,7 +51,8 @@ def add_launch_arg(name: str, default_value=None): "local_slope_max_angle_deg": 30.0, "split_points_distance_tolerance": 0.2, "split_height_distance": 0.2, - } + }, + LaunchConfiguration('vehicle_info_param_file'), ], ), ] @@ -64,6 +75,7 @@ def add_launch_arg(name: str, default_value=None): return launch.LaunchDescription( [ + vehicle_info_param, add_launch_arg("container", ""), add_launch_arg("input/pointcloud", "pointcloud"), add_launch_arg("output/pointcloud", "no_ground/pointcloud"), diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml index 2d35f8bb871a..315e4fa13ab1 100644 --- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml +++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml @@ -2,6 +2,9 @@ + + + @@ -11,5 +14,6 @@ + diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 07d592f3695a..c470b6681116 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -1,6 +1,7 @@ - + + @@ -36,6 +37,7 @@ + diff --git a/planning/freespace_planner/launch/freespace_planner.launch.xml b/planning/freespace_planner/launch/freespace_planner.launch.xml index b8ffb0b20de6..ed83f5aae828 100644 --- a/planning/freespace_planner/launch/freespace_planner.launch.xml +++ b/planning/freespace_planner/launch/freespace_planner.launch.xml @@ -8,6 +8,9 @@ + + + @@ -16,5 +19,6 @@ + diff --git a/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml b/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml index 10e1df879520..403c0c224a5a 100644 --- a/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml +++ b/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml @@ -3,6 +3,9 @@ + + + @@ -17,6 +20,7 @@ + diff --git a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml index 3a2c0eeb8870..121b15a36fbf 100644 --- a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml +++ b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml @@ -1,6 +1,9 @@ + + + @@ -9,6 +12,7 @@ + diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 72ce051a55d8..12ab779d7ecb 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -35,6 +35,16 @@ def generate_launch_description(): description='Path to config file for vehicle characteristics' ) + default_vehicle_info_param = os.path.join( + get_package_share_directory('vehicle_info_util'), + '/config/vehicle_info.param.yaml') + + vehicle_info_param = DeclareLaunchArgument( + 'vehicle_info_param_file', + default_value=default_vehicle_info_param, + description='Path to config file for vehicle information' + ) + simple_planning_simulator_node = launch_ros.actions.Node( package='simple_planning_simulator', executable='simple_planning_simulator_exe', @@ -48,6 +58,7 @@ def generate_launch_description(): ) ), LaunchConfiguration('vehicle_characteristics_param_file'), + LaunchConfiguration('vehicle_info_param_file') ], remappings=[ ('input/ackermann_control_command', '/control/command/control_cmd'), @@ -75,6 +86,7 @@ def generate_launch_description(): ld = launch.LaunchDescription([ vehicle_characteristics_param, + vehicle_info_param, simple_planning_simulator_node, map_to_odom_tf_publisher ]) diff --git a/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml b/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml index d8fc978cc8cf..a1ae8e0617c1 100644 --- a/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml +++ b/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml @@ -4,10 +4,13 @@ + + + From 20bdd44e9de70b6d06976116d0b09c70d191732b Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Wed, 8 Dec 2021 14:11:00 +0900 Subject: [PATCH 4/9] fix: import os Signed-off-by: tomoya.kimura --- .../ground_segmentation/launch/scan_ground_filter.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/ground_segmentation/launch/scan_ground_filter.launch.py index 2454fbb69a37..7dbb1d21676e 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.py @@ -21,6 +21,7 @@ from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +import os def generate_launch_description(): def add_launch_arg(name: str, default_value=None): From efda443ee3f33c9e6b659b3f65e25d9af299601d Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 9 Dec 2021 17:24:22 +0900 Subject: [PATCH 5/9] Update simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Signed-off-by: tomoya.kimura --- .../launch/simple_planning_simulator.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 12ab779d7ecb..3c46303bc6bd 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): default_vehicle_info_param = os.path.join( get_package_share_directory('vehicle_info_util'), - '/config/vehicle_info.param.yaml') + 'config/vehicle_info.param.yaml') vehicle_info_param = DeclareLaunchArgument( 'vehicle_info_param_file', From 96f737fd2f56253eb12972c185ad1167ecf464aa Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 9 Dec 2021 17:24:39 +0900 Subject: [PATCH 6/9] Update perception/ground_segmentation/launch/scan_ground_filter.launch.py Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Signed-off-by: tomoya.kimura --- .../ground_segmentation/launch/scan_ground_filter.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/ground_segmentation/launch/scan_ground_filter.launch.py index 7dbb1d21676e..6eaac9dadc22 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.py @@ -13,6 +13,7 @@ # limitations under the License. import launch +from ament_index_python.packages import get_package_share_directory from launch.actions import DeclareLaunchArgument from launch.conditions import LaunchConfigurationEquals from launch.conditions import LaunchConfigurationNotEquals From ea5dfb9ce06fe3d1b7d80cc68553ca3820fc9cd2 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Thu, 9 Dec 2021 17:27:05 +0900 Subject: [PATCH 7/9] fix dependency Signed-off-by: tomoya.kimura --- perception/ground_segmentation/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/ground_segmentation/package.xml b/perception/ground_segmentation/package.xml index c2ff693b8aeb..75f5349baba5 100644 --- a/perception/ground_segmentation/package.xml +++ b/perception/ground_segmentation/package.xml @@ -28,6 +28,7 @@ tf2 tf2_eigen tf2_ros + vehicle_info_util ament_lint_auto autoware_lint_common From dcef243fc5f5ca5d8eaf9f7ca0de97853c2d9638 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Thu, 9 Dec 2021 19:47:28 +0900 Subject: [PATCH 8/9] fix scan_ground_filter.launch Signed-off-by: tomoya.kimura --- .../launch/scan_ground_filter.launch.py | 53 +++++++++++++------ 1 file changed, 36 insertions(+), 17 deletions(-) diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/ground_segmentation/launch/scan_ground_filter.launch.py index 6eaac9dadc22..0ac247229824 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.py @@ -12,9 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os +import yaml + import launch from ament_index_python.packages import get_package_share_directory from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction from launch.conditions import LaunchConfigurationEquals from launch.conditions import LaunchConfigurationNotEquals from launch.substitutions import LaunchConfiguration @@ -24,19 +29,10 @@ import os -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - default_vehicle_info_param = os.path.join( - get_package_share_directory('vehicle_info_util'), - 'config/vehicle_info.param.yaml') - - vehicle_info_param = DeclareLaunchArgument( - 'vehicle_info_param_file', - default_value=default_vehicle_info_param, - description='Path to config file for vehicle information' - ) +def launch_setup(context, *args, **kwargs): + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] nodes = [ ComposableNode( @@ -54,7 +50,7 @@ def add_launch_arg(name: str, default_value=None): "split_points_distance_tolerance": 0.2, "split_height_distance": 0.2, }, - LaunchConfiguration('vehicle_info_param_file'), + vehicle_info_param, ], ), ] @@ -75,13 +71,36 @@ def add_launch_arg(name: str, default_value=None): condition=LaunchConfigurationEquals("container", ""), ) + + group = GroupAction( + [ + container, + loader, + ] + ) + + return [group] + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + default_vehicle_info_param = os.path.join( + get_package_share_directory("vehicle_info_util"), "config/vehicle_info.param.yaml" + ) + + vehicle_info_param = DeclareLaunchArgument( + "vehicle_info_param_file", + default_value=default_vehicle_info_param, + description="Path to config file for vehicle information", + ) + return launch.LaunchDescription( [ vehicle_info_param, add_launch_arg("container", ""), add_launch_arg("input/pointcloud", "pointcloud"), - add_launch_arg("output/pointcloud", "no_ground/pointcloud"), - container, - loader, + add_launch_arg("output/pointcloud", "no_ground/pointcloud") ] + + [OpaqueFunction(function=launch_setup)] ) From 8def489b5d6d42109037fc6d71adc286dca39566 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 10 Dec 2021 07:00:33 +0000 Subject: [PATCH 9/9] ci(pre-commit): autofix --- common/autoware_global_parameter_loader/Readme.md | 2 +- .../launch/scan_ground_filter.launch.py | 9 ++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/common/autoware_global_parameter_loader/Readme.md b/common/autoware_global_parameter_loader/Readme.md index 31dc8432c214..7b818c26a87f 100644 --- a/common/autoware_global_parameter_loader/Readme.md +++ b/common/autoware_global_parameter_loader/Readme.md @@ -13,7 +13,7 @@ Add the following lines to the launch file of the node in which you want to get ``` -The vehicle model parameter is read from `config/vehicle_info.param.yaml` in `vehicle_model`_description package. +The vehicle model parameter is read from `config/vehicle_info.param.yaml` in `vehicle_model`\_description package. ## Assumptions / Known limits diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/ground_segmentation/launch/scan_ground_filter.launch.py index 0ac247229824..d3d856925b9d 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.py @@ -13,10 +13,9 @@ # limitations under the License. import os -import yaml -import launch from ament_index_python.packages import get_package_share_directory +import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction from launch.actions import OpaqueFunction @@ -26,8 +25,8 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +import yaml -import os def launch_setup(context, *args, **kwargs): vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) @@ -71,7 +70,6 @@ def launch_setup(context, *args, **kwargs): condition=LaunchConfigurationEquals("container", ""), ) - group = GroupAction( [ container, @@ -81,6 +79,7 @@ def launch_setup(context, *args, **kwargs): return [group] + def generate_launch_description(): def add_launch_arg(name: str, default_value=None): return DeclareLaunchArgument(name, default_value=default_value) @@ -100,7 +99,7 @@ def add_launch_arg(name: str, default_value=None): vehicle_info_param, add_launch_arg("container", ""), add_launch_arg("input/pointcloud", "pointcloud"), - add_launch_arg("output/pointcloud", "no_ground/pointcloud") + add_launch_arg("output/pointcloud", "no_ground/pointcloud"), ] + [OpaqueFunction(function=launch_setup)] )