-
Notifications
You must be signed in to change notification settings - Fork 16
/
labiomep_6dof.launch
193 lines (173 loc) · 14.6 KB
/
labiomep_6dof.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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="use_slam" default="0" />
<arg name="play_rosbags" default="1" />
<arg name="play_rosbags_rate" default="0.005" if="$(arg use_slam)" />
<arg name="play_rosbags_rate" default="0.25" unless="$(arg use_slam)" />
<arg name="play_rosbags_start" default="39" />
<arg name="play_rosbags_options" default="--start=$(arg play_rosbags_start) /camera/depth/image_raw/compressedDepth:=/camera/depth/image_raw_rosbag/compressedDepth" />
<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/labiomep/labiomep_2015-05-28-17-00-08_walls_environment_move_up_on_right_of_ramp/labiomep_robot_2015-05-28-17-00-08" />
<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/labiomep/labiomep_2015-05-28-17-00-08_walls_environment_move_up_on_right_of_ramp" />
<arg name="create_depth_pointclouds_from_images" default="false" />
<arg name="publish_kinects_tf_trees" default="false" />
<arg name="publish_kinects_qualisys_tfs" default="false" />
<arg name="world_rosbag_play_extra_filenames" default="$(arg world_rosbag_folder)/labiomep_robot_2015-05-28-17-00-08_pointclouds.bag" />
<arg name="world_rosbag_play_extra_filenames_kinects" default="$(arg world_rosbag_folder)/labiomep_kinect1_2015-05-28-17-00-10_pointclouds.bag $(arg world_rosbag_folder)/labiomep_kinect2_2015-05-28-17-00-01_pointclouds.bag" />
<arg name="publish_external_kinects_bags" default="false" />
<arg name="publish_markers_pointcloud" default="true" />
<arg name="publish_qualisys_ground_truth" default="true" />
<arg name="markers_pointcloud_filename" default="$(arg world_rosbag_folder)/2015-05-28-16-59-57_walls_environment_move_up_on_right_of_ramp.tsv" />
<arg name="qualisys_ground_truth_filename" default="$(arg world_rosbag_folder)/2015-05-28-16-59-57_walls_environment_move_up_on_right_of_ramp_6D.tsv" />
<arg name="qualisys_ground_truth_time_offset" default="1432801117.25089637" /> <!-- 1432828808.25 - 27693.09910363 + 2.1 -->
<arg name="use_drl" default="1" />
<arg name="use_map_server" default="0" />
<arg name="map_file" default="" />
<arg name="reference_pointcloud_filename" default="$(find dynamic_robot_localization_tests)/maps/labiomep/walls_10mm.ply" />
<arg name="robot_initial_pose_in_base_to_map" default="true" />
<arg name="robot_initial_x" default="1.79" />
<arg name="robot_initial_y" default="2.48" />
<arg name="robot_initial_z" default="0.0" />
<arg name="robot_initial_roll" default="0.0" />
<arg name="robot_initial_pitch" default="0.0" />
<arg name="robot_initial_yaw" default="-2.314" />
<arg name="ground_truth_correction_x" default="1.235896507969484" />
<arg name="ground_truth_correction_y" default="2.018507675156786" />
<arg name="ground_truth_correction_z" default="0.17412013366857" />
<arg name="ground_truth_correction_roll" default="-0.39860509178472" />
<arg name="ground_truth_correction_pitch" default="-0.41835916483351" />
<arg name="ground_truth_correction_yaw" default="-3.060195723484054" />
<arg name="map_frame_id" default="map" />
<arg name="odom_frame_id" default="odom" />
<arg name="base_link_frame_id" default="base_footprint" />
<arg name="sensor_frame_id" default="base_footprint" />
<arg name="map_ground_truth_frame_id" default="map_qualisys" />
<arg name="localization_ground_truth_frame_id" default="map_ground_truth_corrected" />
<arg name="ambient_pointcloud_topic" default="/camera/depth/points" />
<arg name="use_ground_truth_as_pose_estimation" default="1" if="$(arg use_slam)" /> <!-- must also activate external tracking in drl -->
<arg name="use_ground_truth_as_pose_estimation" default="0" unless="$(arg use_slam)" />
<arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization)/yaml/configs/filters/filters_3d_tof_slam.yaml" if="$(arg use_slam)" />
<arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization_tests)/launch/environments/labiomep/filters_3d_tof.yaml" unless="$(arg use_slam)" />
<!-- <arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization)/yaml/configs/filters/filters_3d_tof_lasers.yaml" unless="$(arg use_slam)" /> -->
<!-- <arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization_tests)/launch/environments/robotica/filters_3d_tof.yaml" unless="$(arg use_slam)" /> -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find guardian_description)/urdf/guardian_tracks.urdf.xacro' useSimulatedKinect:=false useSchunkArm:=false publishOdometryMsg:=true publishOdometryTf:=true" if="$(arg play_rosbags)" />
<node pkg="tf" type="static_transform_publisher" name="tf_map_correction" args="0 0 0.03 0 0 0 map map_corrected 10" />
<node pkg="image_transport" type="republish" name="republish_uncompress_kinect_data" args="in:=/camera/depth/image_raw_rosbag _image_transport:=compressedDepth raw out:=/camera/depth/image_raw" if="$(arg create_depth_pointclouds_from_images)" />
<include file="$(find openni2_launch)/launch/openni2.launch" if="$(arg create_depth_pointclouds_from_images)" >
<arg name="load_driver" default="false" />
<arg name="publish_tf" default="false" />
<arg name="rgb_processing" default="false" />
<arg name="depth_processing" default="true" />
<arg name="depth_registration" default="false" />
<arg name="depth_registered_processing" default="false" />
</include>
<include file="$(find rgbd_launch)/launch/kinect_frames.launch" if="$(arg publish_kinects_tf_trees)" >
<arg name="camera" default="camera1" />
<arg name="tf_prefix" default="" />
</include>
<include file="$(find rgbd_launch)/launch/kinect_frames.launch" if="$(arg publish_kinects_tf_trees)" >
<arg name="camera" default="camera2" />
<arg name="tf_prefix" default="" />
</include>
<node pkg="tf" type="static_transform_publisher" name="tf_qualisys_ground_truth_correction" args="$(arg ground_truth_correction_x) $(arg ground_truth_correction_y) $(arg ground_truth_correction_z) $(arg ground_truth_correction_yaw) $(arg ground_truth_correction_pitch) $(arg ground_truth_correction_roll) $(arg map_ground_truth_frame_id) $(arg localization_ground_truth_frame_id) 10" />
<node pkg="tf" type="static_transform_publisher" name="tf_qualisys_to_camera1_link" args="-1.09273758 1.74801899 1.06342472 -1.0247334184 0.3937602419 -0.0610182814 map_qualisys camera1_link 10" if="$(arg publish_kinects_qualisys_tfs)" />
<node pkg="tf" type="static_transform_publisher" name="tf_qualisys_to_camera2_link" args="1.265 2.041 1.455 -2.4864717997 0.3826997413 -0.0451942538 map_qualisys camera2_link 10" if="$(arg publish_kinects_qualisys_tfs)" />
<include file="$(find qualisys_tsv_parsers)/launch/tsv_to_pointcloud.launch" if="$(arg publish_markers_pointcloud)" ns="qualisys" >
<arg name="tsv_filename" default="$(arg markers_pointcloud_filename)" />
<arg name="tsv_time_offset" default="$(arg qualisys_ground_truth_time_offset)" />
<arg name="load_base_time_from_tsv_header" default="true" />
</include>
<include file="$(find qualisys_tsv_parsers)/launch/tsv_to_tf.launch" if="$(arg publish_qualisys_ground_truth)" ns="qualisys" >
<arg name="tsv_filename" default="$(arg qualisys_ground_truth_filename)" />
<arg name="tsv_time_offset" default="$(arg qualisys_ground_truth_time_offset)" />
<arg name="tsv_data_columns" default="2" />
<arg name="tsv_data_offset_x" default="-0.185" />
<arg name="tsv_data_offset_z" default="-0.606" />
<arg name="tsv_data_poses_offset_x" default="-0.185" />
<arg name="tsv_data_poses_offset_z" default="-0.606" />
<arg name="source_frame_ids" default="base_footprint" />
<arg name="target_frame_ids" default="map_qualisys" />
<arg name="invert_tf_transforms" default="1" />
<arg name="invert_tf_hierarchies" default="1" />
<arg name="pose_topics" default="pose_robot" />
<arg name="pose_array_topics" default="poses_robot" />
<arg name="load_base_time_from_tsv_header" default="true" />
<arg name="number_msgs_to_skip_in_pose_array_msgs" default="19" />
</include>
<include file="$(find dynamic_robot_localization_tests)/launch/localization_tests.launch" if="$(arg use_drl)" >
<arg name="show_rviz" default="0" />
<arg name="show_plots" default="0" />
<arg name="robot_initial_pose_in_base_to_map" default="$(arg robot_initial_pose_in_base_to_map)" />
<arg name="robot_initial_x" default="$(arg robot_initial_x)" />
<arg name="robot_initial_y" default="$(arg robot_initial_y)" />
<arg name="robot_initial_z" default="$(arg robot_initial_z)" />
<arg name="robot_initial_roll" default="$(arg robot_initial_roll)" />
<arg name="robot_initial_pitch" default="$(arg robot_initial_pitch)" />
<arg name="robot_initial_yaw" default="$(arg robot_initial_yaw)" />
<arg name="ambient_pointcloud_topic" default="$(arg ambient_pointcloud_topic)" />
<arg name="use_dynamic_robot_localization" default="1" />
<arg name="use_6_dof" default="1" />
<arg name="use_tof" default="1" />
<arg name="load_point_features" default="0" />
<arg name="save_point_features" default="0" />
<arg name="reference_pointcloud_filename" default="" if="$(arg use_slam)" />
<arg name="reference_pointcloud_filename" default="$(arg reference_pointcloud_filename)" unless="$(arg use_slam)" />
<arg name="reference_pointcloud_type_3d" default="true" />
<arg name="reference_pointcloud_update_mode" default="FullIntegration" if="$(arg use_slam)" /> <!-- NoIntegration | FullIntegration | InliersIntegration | OutliersIntegration -->
<arg name="reference_pointcloud_update_mode" default="NoIntegration" unless="$(arg use_slam)" />
<arg name="reference_pointcloud_available" default="0" if="$(arg use_slam)" />
<arg name="reference_pointcloud_available" default="1" unless="$(arg use_slam)" />
<arg name="reference_costmap_topic" default="" />
<arg name="reference_pointcloud_topic" default="reference_pointcloud_update" />
<arg name="use_octomap_for_dynamic_map_update" default="0" />
<arg name="octomap_pointcloud_topic" default="aligned_pointcloud" /> <!-- $(arg ambient_pointcloud_topic) | aligned_pointcloud | aligned_pointcloud_outliers | aligned_pointcloud_inliers -->
<arg name="octomap_minimum_amount_of_time_between_ros_msg_publishing" default="5.0" />
<arg name="octomap_minimum_number_of_integrations_before_ros_msg_publishing" default="20" />
<arg name="octomap_resolution" default="0.005" />
<arg name="yaml_configuration_filters_filename" default="$(arg yaml_configuration_filters_filename)" />
<!-- <arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/empty.yaml" /> -->
<arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_estimation/initial_pose_estimation_3d.yaml" />
<arg name="yaml_configuration_tracking_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/cluttered_environments_dynamic_large_map_slam_3d.yaml" if="$(arg use_slam)" />
<arg name="yaml_configuration_tracking_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/cluttered_environments_dynamic_large_map_3d.yaml" unless="$(arg use_slam)" />
<!-- <arg name="yaml_configuration_tracking_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/unstable_ground_3d_tof.yaml" unless="$(arg use_slam)" /> -->
<arg name="yaml_configuration_recovery_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_recovery/recovery_3d.yaml" />
<arg name="publish_ground_truth" default="0" />
<arg name="use_map_server" default="$(arg use_map_server)" />
<arg name="map_file" default="$(arg map_file)" />
<arg name="map_server_frame_id" default="map" />
<arg name="map_frame_id" default="$(arg localization_ground_truth_frame_id)" if="$(arg use_ground_truth_as_pose_estimation)" />
<arg name="map_frame_id" default="$(arg map_frame_id)" unless="$(arg use_ground_truth_as_pose_estimation)" />
<arg name="odom_frame_id" default="$(arg odom_frame_id)" />
<arg name="base_link_frame_id" default="$(arg base_link_frame_id)" />
<arg name="sensor_frame_id" default="$(arg sensor_frame_id)" />
<arg name="localization_ground_truth_frame_id" default="$(arg localization_ground_truth_frame_id)" />
<arg name="publish_tf_map_odom" default="false" if="$(arg use_ground_truth_as_pose_estimation)" />
<arg name="publish_tf_map_odom" default="true" unless="$(arg use_ground_truth_as_pose_estimation)" />
<arg name="invert_tf_transform" default="true" />
<arg name="invert_tf_hierarchy" default="true" />
<arg name="transform_pose_to_map_frame_id" default="true" />
<arg name="play_rosbags" default="$(arg play_rosbags)" />
<arg name="play_rosbags_rate" default="$(arg play_rosbags_rate)" />
<arg name="play_rosbags_options" default="$(arg play_rosbags_options)" />
<arg name="world_rosbag_folder" default="$(arg world_rosbag_folder)" />
<arg name="world_rosbag_filename" default="$(arg world_rosbag_filename)" />
<arg name="world_rosbag_play_extra_filenames" default="$(arg world_rosbag_play_extra_filenames) $(arg world_rosbag_play_extra_filenames_kinects)" if="$(arg publish_external_kinects_bags)"/>
<arg name="world_rosbag_play_extra_filenames" default="$(arg world_rosbag_play_extra_filenames)" unless="$(arg publish_external_kinects_bags)"/>
<arg name="world_name" default="labiomep" />
<arg name="move_robot_on_path" default="0" />
<arg name="robot_path" default="" />
<arg name="rviz_folder" default="$(find dynamic_robot_localization_tests)/rviz/guardian/crob_lab" />
<!-- laser scan assembler -->
<arg name="use_laser_assembler" default="0" />
<arg name="laser_scan_topics" default="/guardian/laser_tilt_front+/guardian/laser_horizontal_back" />
<arg name="number_of_tf_queries_for_spherical_interpolation" default="4" />
<arg name="number_of_scans_to_assemble_per_cloud" default="4" />
<arg name="timeout_for_cloud_assembly" default="1.0" />
<arg name="odometry_topic" default="/guardian/odom" />
<arg name="min_number_of_scans_to_assemble_per_cloud" default="2" />
<arg name="max_number_of_scans_to_assemble_per_cloud" default="20" />
<arg name="min_timeout_seconds_for_cloud_assembly" default="0.15" />
<arg name="max_timeout_seconds_for_cloud_assembly" default="1.1" />
<arg name="max_linear_velocity" default="0.025" /> <!-- when the linear velocity of the robot is equal or greater than this value, the number of scans to join and assembly timeout will be set to their minimum values specified above -->
<arg name="max_angular_velocity" default="0.174532925" /> <!-- when the angular velocity of the robot is equal or greater than this value, the number of scans to join and assembly timeout will be set to their minimum values specified above -->
</include>
</launch>