nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
lidar_launch.py
[詳解]
1"""LiDARドライバ、サーボ制御、点群生成、RVizのノードを起動する。"""
2from launch import LaunchDescription
3from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
4from launch.conditions import IfCondition
5from launch.launch_description_sources import PythonLaunchDescriptionSource
6from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
7from launch_ros.actions import Node
8from launch_ros.parameter_descriptions import ParameterValue
9from launch_ros.substitutions import FindPackageShare
10
11
13 params = PathJoinSubstitution(
14 [FindPackageShare('nodding_lidar'), 'config', 'nodding_lidar.yaml'])
15
16 return LaunchDescription([
17 DeclareLaunchArgument(
18 'enable_nodding_lidar', default_value='true',
19 description='首振り2D LiDARと点群生成ノードを起動する'),
20 DeclareLaunchArgument(
21 'use_pca9685', default_value='false',
22 description='true にするとシリアル入力の代わりにPCA9685サーボ指令ノードを使う'),
23 DeclareLaunchArgument('servo_serial_port', default_value='/dev/ttyACM0'),
24 DeclareLaunchArgument('lidar_port', default_value='/dev/ttyUSB0'),
25 DeclareLaunchArgument('lidar_baudrate', default_value='256000'),
26 DeclareLaunchArgument('lidar_frame', default_value='laser'),
27 DeclareLaunchArgument(
28 'fixed_frame', default_value='base_link',
29 description='スキャン中の移動補正用フレーム(odomがない単体起動ではbase_link)'),
30 DeclareLaunchArgument('enable_lidar', default_value='true'),
31 DeclareLaunchArgument('use_sim_time', default_value='false'),
32 DeclareLaunchArgument(
33 'publish_base_to_servo', default_value='true',
34 description='base_link -> servo_link の静的TFを配信する'),
35 DeclareLaunchArgument('base_to_servo_x', default_value='0.0'),
36 DeclareLaunchArgument('base_to_servo_y', default_value='0.0'),
37 DeclareLaunchArgument(
38 'base_to_servo_z', default_value='0.142',
39 description='既存LiDAR高さ0.1745mとセンサオフセット0.0325mからの暫定値'),
40 DeclareLaunchArgument('base_to_servo_roll', default_value='0.0'),
41 DeclareLaunchArgument('base_to_servo_pitch', default_value='0.0'),
42 DeclareLaunchArgument('base_to_servo_yaw', default_value='0.0'),
43 DeclareLaunchArgument(
44 'use_hardware', default_value='true',
45 description='false にすると、サーボを動かさず、角度とTFだけ出す'),
46 DeclareLaunchArgument(
47 'enable_rviz', default_value='true',
48 description='RViz2で /points を表示する'),
49
50 IncludeLaunchDescription(
51 PythonLaunchDescriptionSource(PathJoinSubstitution([
52 FindPackageShare('sllidar_ros2'), 'launch', 'sllidar_s1_launch.py'])),
53 launch_arguments={
54 'serial_port': LaunchConfiguration('lidar_port'),
55 'serial_baudrate': LaunchConfiguration('lidar_baudrate'),
56 'frame_id': LaunchConfiguration('lidar_frame'),
57 }.items(),
58 condition=IfCondition(LaunchConfiguration('enable_lidar')),
59 ),
60
61 Node(
62 package='nodding_lidar',
63 executable='serial_angle_bridge',
64 name='serial_angle_bridge',
65 output='screen',
66 parameters=[params, {
67 'use_sim_time': LaunchConfiguration('use_sim_time'),
68 'serial_port': LaunchConfiguration('servo_serial_port'),
69 }],
70 condition=IfCondition(PythonExpression([
71 "'", LaunchConfiguration('enable_nodding_lidar'),
72 "' == 'true' and '", LaunchConfiguration('use_pca9685'), "' == 'false'",
73 ])),
74 ),
75
76 Node(
77 package='nodding_lidar',
78 executable='servo_tf_node',
79 name='servo_tf_node',
80 output='screen',
81 parameters=[params, {
82 'use_sim_time': LaunchConfiguration('use_sim_time'),
83 'child_frame': LaunchConfiguration('lidar_frame'),
84 }],
85 condition=IfCondition(PythonExpression([
86 "'", LaunchConfiguration('enable_nodding_lidar'),
87 "' == 'true' and '", LaunchConfiguration('use_pca9685'), "' == 'false'",
88 ])),
89 ),
90
91 Node(
92 package='nodding_lidar',
93 executable='servo_node',
94 name='servo_node',
95 output='screen',
96 parameters=[
97 params,
98 {'use_sim_time': LaunchConfiguration('use_sim_time')},
99 {'use_hardware': ParameterValue(
100 LaunchConfiguration('use_hardware'), value_type=bool)},
101 {'child_frame': LaunchConfiguration('lidar_frame')},
102 ],
103 condition=IfCondition(PythonExpression([
104 "'", LaunchConfiguration('enable_nodding_lidar'),
105 "' == 'true' and '", LaunchConfiguration('use_pca9685'), "' == 'true'",
106 ])),
107 ),
108
109 Node(
110 package='nodding_lidar',
111 executable='scan_to_cloud',
112 name='scan_to_cloud',
113 output='screen',
114 parameters=[params, {
115 'use_sim_time': LaunchConfiguration('use_sim_time'),
116 'fixed_frame': LaunchConfiguration('fixed_frame'),
117 }],
118 condition=IfCondition(LaunchConfiguration('enable_nodding_lidar')),
119 ),
120
121 Node(
122 package='rviz2',
123 executable='rviz2',
124 name='nodding_pointcloud_rviz',
125 arguments=['-d', PathJoinSubstitution([
126 FindPackageShare('nodding_lidar'), 'rviz', 'pointcloud.rviz'])],
127 parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
128 output='screen',
129 condition=IfCondition(LaunchConfiguration('enable_rviz')),
130 ),
131
132 Node(
133 package='tf2_ros',
134 executable='static_transform_publisher',
135 name='base_to_servo_static_tf',
136 arguments=[
137 '--x', LaunchConfiguration('base_to_servo_x'),
138 '--y', LaunchConfiguration('base_to_servo_y'),
139 '--z', LaunchConfiguration('base_to_servo_z'),
140 '--roll', LaunchConfiguration('base_to_servo_roll'),
141 '--pitch', LaunchConfiguration('base_to_servo_pitch'),
142 '--yaw', LaunchConfiguration('base_to_servo_yaw'),
143 '--frame-id', 'base_link', '--child-frame-id', 'servo_link',
144 ],
145 condition=IfCondition(PythonExpression([
146 "'", LaunchConfiguration('enable_nodding_lidar'),
147 "' == 'true' and '", LaunchConfiguration('publish_base_to_servo'),
148 "' == 'true'",
149 ])),
150 ),
151 ])