|
5 | 5 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution |
6 | 6 | from launch_ros.actions import Node |
7 | 7 | from launch_ros.substitutions import FindPackageShare |
8 | | -import os |
9 | 8 |
|
10 | 9 |
|
11 | 10 | def generate_launch_description(): |
12 | 11 | provide_odom_frame_arg = DeclareLaunchArgument( |
13 | | - 'provide_odom_frame', |
14 | | - default_value='false', |
15 | | - description='Whether to provide odom frame' |
| 12 | + "provide_odom_frame", |
| 13 | + default_value="false", |
| 14 | + description="Whether to provide odom frame", |
16 | 15 | ) |
17 | | - |
| 16 | + |
18 | 17 | expected_sensor_ids_arg = DeclareLaunchArgument( |
19 | | - 'expected_sensor_ids', |
| 18 | + "expected_sensor_ids", |
20 | 19 | default_value='["scan"]', |
21 | | - description='Expected sensor IDs for cartographer' |
| 20 | + description="Expected sensor IDs for cartographer", |
22 | 21 | ) |
23 | | - |
| 22 | + |
24 | 23 | resolution_arg = DeclareLaunchArgument( |
25 | | - 'resolution', |
26 | | - default_value='0.05', |
27 | | - description='Resolution for occupancy grid' |
| 24 | + "resolution", |
| 25 | + default_value="0.05", |
| 26 | + description="Resolution for occupancy grid", |
28 | 27 | ) |
29 | | - |
| 28 | + |
30 | 29 | publish_period_arg = DeclareLaunchArgument( |
31 | | - 'publish_period_sec', |
32 | | - default_value='1.0', |
33 | | - description='Publish period for occupancy grid' |
| 30 | + "publish_period_sec", |
| 31 | + default_value="1.0", |
| 32 | + description="Publish period for occupancy grid", |
34 | 33 | ) |
35 | 34 |
|
36 | | - slam_package_dir = FindPackageShare('slam') |
37 | | - |
38 | | - config_dir = PathJoinSubstitution([slam_package_dir, 'config']) |
| 35 | + slam_package_dir = FindPackageShare("slam") |
| 36 | + |
| 37 | + config_dir = PathJoinSubstitution([slam_package_dir, "config"]) |
39 | 38 |
|
40 | 39 | # SF45B driver process |
41 | 40 | sf45b_process = ExecuteProcess( |
42 | | - cmd=['./sf45b'], |
43 | | - cwd='/workspace', |
44 | | - name='sf45b_driver', |
45 | | - output='screen' |
| 41 | + cmd=["./sf45b"], cwd="/workspace", name="sf45b_driver", output="screen" |
46 | 42 | ) |
47 | 43 |
|
48 | 44 | # Static transform publisher node |
49 | 45 | static_transform_publisher = Node( |
50 | | - package='tf2_ros', |
51 | | - executable='static_transform_publisher', |
52 | | - name='laser_transform_publisher', |
53 | | - arguments=['0', '0', '0', '0', '0', '0', 'laser', 'laser_frame'], |
54 | | - output='screen' |
| 46 | + package="tf2_ros", |
| 47 | + executable="static_transform_publisher", |
| 48 | + name="laser_transform_publisher", |
| 49 | + arguments=["0", "0", "0", "0", "0", "0", "laser", "laser_frame"], |
| 50 | + output="screen", |
55 | 51 | ) |
56 | 52 |
|
57 | 53 | # Cartographer node |
58 | 54 | cartographer_node = Node( |
59 | | - package='cartographer_ros', |
60 | | - executable='cartographer_node', |
61 | | - name='cartographer_node', |
| 55 | + package="cartographer_ros", |
| 56 | + executable="cartographer_node", |
| 57 | + name="cartographer_node", |
62 | 58 | parameters=[ |
63 | | - {'provide_odom_frame': LaunchConfiguration('provide_odom_frame')}, |
64 | | - {'expected_sensor_ids': LaunchConfiguration('expected_sensor_ids')} |
| 59 | + {"provide_odom_frame": LaunchConfiguration("provide_odom_frame")}, |
| 60 | + {"expected_sensor_ids": LaunchConfiguration("expected_sensor_ids")}, |
65 | 61 | ], |
66 | 62 | arguments=[ |
67 | | - '-configuration_directory', config_dir, |
68 | | - '-configuration_basename', 'sf45b_2d.lua' |
| 63 | + "-configuration_directory", |
| 64 | + config_dir, |
| 65 | + "-configuration_basename", |
| 66 | + "sf45b_2d.lua", |
69 | 67 | ], |
70 | | - remappings=[('scan', '/scan')], |
71 | | - output='screen' |
| 68 | + remappings=[("scan", "/scan")], |
| 69 | + output="screen", |
72 | 70 | ) |
73 | 71 |
|
74 | 72 | # Cartographer occupancy grid node |
75 | 73 | occupancy_grid_node = Node( |
76 | | - package='cartographer_ros', |
77 | | - executable='cartographer_occupancy_grid_node', |
78 | | - name='cartographer_occupancy_grid_node', |
| 74 | + package="cartographer_ros", |
| 75 | + executable="cartographer_occupancy_grid_node", |
| 76 | + name="cartographer_occupancy_grid_node", |
79 | 77 | arguments=[ |
80 | | - '-resolution', LaunchConfiguration('resolution'), |
81 | | - '-publish_period_sec', LaunchConfiguration('publish_period_sec') |
| 78 | + "-resolution", |
| 79 | + LaunchConfiguration("resolution"), |
| 80 | + "-publish_period_sec", |
| 81 | + LaunchConfiguration("publish_period_sec"), |
82 | 82 | ], |
83 | | - output='screen' |
| 83 | + output="screen", |
84 | 84 | ) |
85 | 85 |
|
86 | 86 | # RViz2 node |
87 | 87 | rviz_node = Node( |
88 | | - package='rviz2', |
89 | | - executable='rviz2', |
90 | | - name='rviz2', |
91 | | - output='screen' |
| 88 | + package="rviz2", |
| 89 | + executable="rviz2", |
| 90 | + name="rviz2", |
| 91 | + output="screen", |
92 | 92 | ) |
93 | 93 |
|
94 | | - return LaunchDescription([ |
95 | | - provide_odom_frame_arg, |
96 | | - expected_sensor_ids_arg, |
97 | | - resolution_arg, |
98 | | - publish_period_arg, |
99 | | - sf45b_process, |
100 | | - static_transform_publisher, |
101 | | - cartographer_node, |
102 | | - occupancy_grid_node, |
103 | | - rviz_node |
104 | | - ]) |
| 94 | + return LaunchDescription( |
| 95 | + [ |
| 96 | + provide_odom_frame_arg, |
| 97 | + expected_sensor_ids_arg, |
| 98 | + resolution_arg, |
| 99 | + publish_period_arg, |
| 100 | + sf45b_process, |
| 101 | + static_transform_publisher, |
| 102 | + cartographer_node, |
| 103 | + occupancy_grid_node, |
| 104 | + rviz_node, |
| 105 | + ] |
| 106 | + ) |
0 commit comments