mapping-3d
読み取り中…
検索中…
一致する文字列を見つけられません
realsense_infra_stereo.launch.py
[詳解]
1"""RealSense D435i/D455の左右の赤外画像を使い、RTAB-Mapで地図を作る。
2
3次の左右の赤外画像をステレオ画像として使う。
4 /camera/infra1/image_rect_raw
5 /camera/infra2/image_rect_raw
6"""
7
8import os
9
10from ament_index_python.packages import get_package_share_directory
11from launch import LaunchDescription
12from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
13from launch.launch_description_sources import PythonLaunchDescriptionSource
14from launch.substitutions import LaunchConfiguration
15from launch_ros.actions import Node
16
17
19 parameters = {
20 "frame_id": "camera_link",
21 "subscribe_stereo": True,
22 "subscribe_odom_info": True,
23 "wait_imu_to_init": False,
24 }
25
26 remappings = [
27 ("left/image_rect", "/camera/infra1/image_rect_raw"),
28 ("left/camera_info", "/camera/infra1/camera_info"),
29 ("right/image_rect", "/camera/infra2/image_rect_raw"),
30 ("right/camera_info", "/camera/infra2/camera_info"),
31 ]
32
33 realsense_launch = IncludeLaunchDescription(
34 PythonLaunchDescriptionSource(
35 os.path.join(
36 get_package_share_directory("realsense2_camera"),
37 "launch",
38 "rs_launch.py",
39 )
40 ),
41 launch_arguments={
42 "camera_namespace": "",
43 "enable_color": "false",
44 "enable_depth": "false",
45 "enable_gyro": "false",
46 "enable_accel": "false",
47 "enable_infra1": "true",
48 "enable_infra2": "true",
49 "enable_sync": "true",
50 }.items(),
51 )
52
53 stereo_odometry = Node(
54 package="rtabmap_odom",
55 executable="stereo_odometry",
56 name="stereo_odometry",
57 output="screen",
58 parameters=[parameters],
59 arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
60 remappings=remappings,
61 )
62
63 rtabmap = Node(
64 package="rtabmap_slam",
65 executable="rtabmap",
66 output="screen",
67 parameters=[
68 parameters,
69 {
70 # Mapping is intentionally odometry-only for this experiment.
71 # A likelihood of 1.0 cannot be reached by the normalised
72 # loop-closure hypothesis, so loop closures are rejected.
73 "Rtabmap/LoopThr": "1.0",
74 "RGBD/ProximityBySpace": "false",
75 "RGBD/ProximityByTime": "false",
76 "RGBD/CreateOccupancyGrid": "true",
77 },
78 ],
79 arguments=["-d", LaunchConfiguration("args")],
80 remappings=remappings,
81 )
82
83 rtabmap_viz = Node(
84 package="rtabmap_viz",
85 executable="rtabmap_viz",
86 output="screen",
87 parameters=[parameters, {"odometry_node_name": "stereo_odometry"}],
88 remappings=remappings,
89 )
90
91 return LaunchDescription(
92 [
93 DeclareLaunchArgument(
94 "args",
95 default_value="",
96 description="Extra arguments for RTAB-Map and stereo odometry.",
97 ),
98 DeclareLaunchArgument(
99 "odom_args",
100 default_value="",
101 description="Extra arguments for stereo odometry.",
102 ),
103 realsense_launch,
104 stereo_odometry,
105 rtabmap,
106 rtabmap_viz,
107 ]
108 )