nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
standalone_slam_launch.py
[詳解]
1"""LiDARの点群同士を照合して移動量を求め、地図を作る。機体の制御やESP32接続は使用しない。"""
2from launch import LaunchDescription
3from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
4from launch.conditions import IfCondition
5from launch.launch_description_sources import PythonLaunchDescriptionSource
6from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution
7from launch_ros.actions import Node
8from launch_ros.substitutions import FindPackageShare
9
10
12 database_path = LaunchConfiguration('database_path')
13 use_sim_time = LaunchConfiguration('use_sim_time')
14 nodding = IncludeLaunchDescription(
15 PythonLaunchDescriptionSource(PathJoinSubstitution([
16 FindPackageShare('nodding_lidar'), 'launch', 'lidar_launch.py'])),
17 launch_arguments={
18 'enable_nodding_lidar': 'true',
19 'servo_serial_port': LaunchConfiguration('servo_angle_port'),
20 'lidar_port': LaunchConfiguration('lidar_port'),
21 'lidar_baudrate': LaunchConfiguration('lidar_baudrate'),
22 'enable_lidar': 'true',
23 'use_sim_time': LaunchConfiguration('use_sim_time'),
24 # No external odometry: avoid a points -> odom -> points cycle.
25 'fixed_frame': 'base_link',
26 'enable_rviz': 'false',
27 }.items(),
28 )
29 odometry = Node(
30 package='rtabmap_odom', executable='icp_odometry', name='icp_odometry',
31 output='screen', parameters=[{
32 'use_sim_time': use_sim_time,
33 'frame_id': 'base_link',
34 'odom_frame_id': 'odom',
35 'publish_tf': True,
36 'wait_imu_to_init': False,
37 'deskewing': False,
38 # Each input is a thin scan; do not require surface normals.
39 'Icp/PointToPlane': 'false',
40 'Icp/VoxelSize': '0.05',
41 'OdomF2M/ScanMaxSize': '15000',
42 }],
43 remappings=[('scan_cloud', '/points'), ('odom', '/odometry/lidar')],
44 )
45 rtabmap = Node(
46 package='rtabmap_slam', executable='rtabmap', name='rtabmap',
47 output='screen', parameters=[
48 PathJoinSubstitution([
49 FindPackageShare('nodding_lidar'), 'config', 'rtabmap.yaml']),
50 {'use_sim_time': use_sim_time, 'database_path': database_path,
51 'Icp/PointToPlane': 'false'},
52 ], remappings=[('scan_cloud', '/points'), ('odom', '/odometry/lidar')],
53 )
54 rtabmap_viz = Node(
55 package='rtabmap_viz', executable='rtabmap_viz', name='rtabmap_viz',
56 output='screen', parameters=[{
57 'use_sim_time': use_sim_time,
58 'frame_id': 'base_link',
59 'subscribe_scan_cloud': True,
60 'subscribe_rgb': False,
61 'subscribe_depth': False,
62 'subscribe_rgbd': False,
63 'subscribe_odom_info': True,
64 'odometry_node_name': 'icp_odometry',
65 'approx_sync': True,
66 'wait_for_transform': 0.2,
67 }],
68 remappings=[('scan_cloud', '/points'), ('odom', '/odometry/lidar')],
69 condition=IfCondition(LaunchConfiguration('enable_rtabmap_viz')),
70 )
71 rviz = Node(
72 package='rviz2', executable='rviz2', name='rtabmap_3d_rviz',
73 arguments=['-d', PathJoinSubstitution([
74 FindPackageShare('nodding_lidar'), 'rviz', 'rtabmap_3d.rviz'])],
75 parameters=[{'use_sim_time': use_sim_time}], output='screen',
76 condition=IfCondition(LaunchConfiguration('enable_rviz')),
77 )
78 return LaunchDescription([
79 DeclareLaunchArgument('lidar_port', default_value='/dev/ttyUSB0'),
80 DeclareLaunchArgument('servo_angle_port', default_value='/dev/ttyACM0'),
81 DeclareLaunchArgument('lidar_baudrate', default_value='256000'),
82 DeclareLaunchArgument('use_sim_time', default_value='false'),
83 DeclareLaunchArgument('enable_rviz', default_value='true'),
84 DeclareLaunchArgument('enable_rtabmap_viz', default_value='true'),
85 DeclareLaunchArgument(
86 'database_path',
87 default_value=PathJoinSubstitution([
88 EnvironmentVariable('HOME'), '.ros', 'nodding_standalone.db']),
89 description='RTAB-Map 3D mapping database'),
90 # Keep the child RViz override from disabling the SLAM viewer.
91 GroupAction(actions=[nodding], scoped=True),
92 odometry,
93 rtabmap,
94 rtabmap_viz,
95 rviz,
96 ])