-
Notifications
You must be signed in to change notification settings - Fork 137
/
Copy patheuroc.launch
90 lines (65 loc) · 4.19 KB
/
euroc.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
<launch>
<arg name="detector" default="orb"/>
<arg name="descriptor" default="brief"/>
<!-- Set use_sim_time true for datasets-->
<param name="use_sim_time" value="true"/>
<group ns="stereo">
<!-- Call stereo_image_proc to undistort and rectify images -->
<!--<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc" >
<remap from="/stereo/left/image_raw" to="/cam0/image_raw"/>
<remap from="/stereo/right/image_raw" to="/cam1/image_raw"/>
<remap from="/stereo/left/camera_info" to="/cam0/camera_info"/>
<remap from="/stereo/right/camera_info" to="/cam1/camera_info"/>
</node>-->
</group>
<!-- Publish static transform between robot origin and left camera origin-->
<node pkg="tf2_ros" type="static_transform_publisher" name="imu_cam0_broadcaster" args="-0.0216401454975 -0.064676986768 0.00981073058949 1.5559252954916372 0.02577729130330495 0.003757445479965962 imu4 cam0"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="imu_cam1_broadcaster" args="-0.0216401454975 0.064676986768 0.00981073058949 1.5582366807216512 0.025392529572065364 0.01790731339694836 imu4 cam1"/>
<!-- orientation from GT dataset -->
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_broadcaster" args="0 0 0 -0.81743 0.011702 -0.57591 0.0015811 base_link imu4"/>
<!-- relative transforms for GT handling (TODO: recheck, should match starting position of SPTAM) -->
<node pkg="tf2_ros" type="static_transform_publisher" name="gr_broadcaster2" args="0 0 0 0 0 0 map_prev map"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="gr_broadcaster" args="4.566874 -1.676021 0.572093 -0.086352 -0.791972 -0.023578 0.603961 leica map_prev"/>
<!-- S-PTAM path publisher. Transforms poses into path. -->
<node pkg="ros_utils" type="pose_to_path" name="sptam_path">
<remap from="pose" to="sptam/robot/pose" />
<remap from="path" to="sptam/robot/path" />
</node>
<!-- Clean S-PTAM parameters -->
<rosparam command="delete" ns="sptam"/>
<!-- Read S-PTAM parameters file -->
<rosparam command="load" ns="sptam" file="$(eval find('sptam') + '/configurationFiles/euroc_' + arg('detector') + '_' + arg('descriptor') + '.yaml')"/>
<param name="sptam/LoopDetectorVocabulary" value="$(find sptam)/bow_voc/DBoW2/brief_mit_malaga_vocabulary.yml.gz" />
<!-- Nodelet Manager -->
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true"/>
<!-- image proc nodelets -->
<node pkg="nodelet" type="nodelet" name="image_proc_left" args="load image_proc/rectify nodelet_manager" output="screen" clear_params="true">
<param name="queue_size" value="1"/>
<remap from="image_mono" to="cam0/image_raw"/>
<remap from="camera_info" to="cam0/camera_info"/>
<remap from="image_rect" to="/stereo/left/image_rect"/>
</node>
<node pkg="nodelet" type="nodelet" name="image_proc_right" args="load image_proc/rectify nodelet_manager" output="screen" clear_params="true">
<param name="queue_size" value="1"/>
<remap from="image_mono" to="cam1/image_raw"/>
<remap from="camera_info" to="cam1/camera_info"/>
<remap from="image_rect" to="/stereo/right/image_rect"/>
</node>
<!-- Choose between Nodelet and Node S-PTAM -->
<node pkg="sptam" type="sptam_node" name="sptam" output="screen" >
<!-- <node pkg="nodelet" type="nodelet" name="sptam" args="load sptam/sptam_nodelet nodelet_manager" output="screen" clear_params="true" >-->
<param name="approximate_sync" value="false" />
<param name="use_prediction" value="false"/>
<param name="publish_transform" value="true"/>
<param name="base_frame" value="base_link"/>
<param name="prediction_frame" value="map"/>
<param name="camera_frame" value="cam0"/> <!-- set left camera frame -->
<!-- Remap topics -->
<!--<remap from="/stereo/left/image_rect" to="/stereo/left/image_rect_color"/>
<remap from="/stereo/right/image_rect" to="/stereo/right/image_rect_color"/>-->
<remap from="/stereo/left/camera_info" to="/cam0/camera_info"/>
<remap from="/stereo/right/camera_info" to="/cam1/camera_info"/>
<!-- Remap Odometry message -->
<remap from="robot/pose" to="odom"/>
</node>
</launch>