-
Notifications
You must be signed in to change notification settings - Fork 137
/
Copy pathmit.launch
65 lines (47 loc) · 2.81 KB
/
mit.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
<launch>
<!-- Set use_sim_time true for datasets-->
<param name="use_sim_time" value="true" />
<!-- Read S-PTAM parameters file -->
<rosparam command="load" ns="sptam" file="$(find sptam)/configurationFiles/mit.yaml" />
<param name="sptam/LoopDetectorVocabulary" value="$(find sptam)/bow_voc/DBoW2/brief_mit_malaga_vocabulary.yml.gz" />
<!-- PR2 robot model (for visualization) -->
<!-- <include file="$(find pr2_description)/robots/upload_pr2.launch" />-->
<group ns="wide_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="/wide_stereo/left/image_raw" />
<remap from="/stereo/right/image_raw" to="/wide_stereo/right/image_raw" />
<remap from="/stereo/left/camera_info" to="/wide_stereo/left/camera_info" />
<remap from="/stereo/right/camera_info" to="/wide_stereo/right/camera_info" />
</node>
</group>
<!-- S-PTAM pose path publisher. Used for visualization. -->
<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>
<!-- Nodelet Manager -->
<!--<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true"/>-->
<!-- 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="use_prediction" value="true" />
<param name="approximate_sync" value="false" />
<param name="publish_on_fail" value="true" />
<param name="prediction_frame" value="odom_combined" />
<param name="camera_frame" value="wide_stereo_gazebo_l_stereo_camera_optical_frame" />
<!-- input remaps -->
<remap from="/imu/data" to="/torso_lift_imu/data"/>
<remap from="/stereo/left/image_features" to="/wide_stereo/left/image_features"/>
<remap from="/stereo/right/image_features" to="/wide_stereo/right/image_features"/>
<remap from="/stereo/left/camera_info" to="/wide_stereo/left/camera_info"/>
<remap from="/stereo/right/camera_info" to="/wide_stereo/right/camera_info"/>
<!-- when debugging the node subscribes to the images -->
<remap from="/stereo/left/image_rect" to="/wide_stereo/left/image_rect_color"/>
<remap from="/stereo/right/image_rect" to="/wide_stereo/right/image_rect_color"/>
<remap from="/stereo/left/camera_info" to="/wide_stereo/left/camera_info"/>
<remap from="/stereo/right/camera_info" to="/wide_stereo/right/camera_info"/>
<!-- Remap Odometry message -->
<remap from="robot/pose" to="odom" />
</node>
</launch>