-
Notifications
You must be signed in to change notification settings - Fork 16
/
ethzasl_kinect_dataset.launch
128 lines (116 loc) · 10.2 KB
/
ethzasl_kinect_dataset.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
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="use_slam" default="0" />
<arg name="use_dynamic_robot_localization" default="1" />
<arg name="use_ethzasl_icp_mapper" default="0" />
<arg name="ethzasl_icp_mapper_min_registration_overlap" default="0.47" />
<arg name="ethzasl_icp_mapper_icp_config_filename" default="icp.yaml" />
<arg name="play_rosbags" default="1" />
<arg name="play_rosbags_rate" default="0.01" if="$(arg use_slam)" />
<arg name="play_rosbags_rate" default="1.0" unless="$(arg use_slam)" />
<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/asl/ethzasl_kinect_dataset/0high-0slow-0fly-0_2011-02-19-11-44-41" />
<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/asl/ethzasl_kinect_dataset" />
<arg name="use_map_server" default="0" />
<arg name="save_preprocessed_reference_cloud" default="0" />
<arg name="map_file" default="" />
<arg name="reference_pointcloud_filename" default="$(find dynamic_robot_localization_tests)/maps/asl/ethzasl_kinect_dataset/high-complexity-environment_icp-point-point_0.0025-voxel-grid_mls_0.005-rosbag-speed.ply" if="$(arg use_dynamic_robot_localization)" />
<arg name="reference_pointcloud_filename" default="$(find dynamic_robot_localization_tests)/maps/asl/ethzasl_kinect_dataset/high-complexity-environment_icp-point-point_0.0025-voxel-grid_mls_0.005-rosbag-speed_preprocessed_ascii.ply" if="$(arg use_ethzasl_icp_mapper)" />
<arg name="rviz_folder" default="$(find dynamic_robot_localization_tests)/rviz/asl" if="$(arg use_dynamic_robot_localization)" />
<arg name="rviz_folder" default="$(find dynamic_robot_localization_tests)/rviz/asl_icp_mapper" if="$(arg use_ethzasl_icp_mapper)" />
<arg name="robot_initial_pose_in_base_to_map" default="true" />
<arg name="robot_initial_x" default="0.0" />
<arg name="robot_initial_y" default="0.0" />
<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="0.0" />
<arg name="ground_truth_correction_x" default="0.0" />
<arg name="ground_truth_correction_y" default="0.0" />
<arg name="ground_truth_correction_z" default="0.0" />
<arg name="ground_truth_correction_roll" default="0.0" />
<arg name="ground_truth_correction_pitch" default="0.0" />
<arg name="ground_truth_correction_yaw" default="0.0" />
<arg name="ground_truth_tf_time_offset" default="0.0" />
<arg name="map_ground_truth_frame_id" default="map_ground_truth" />
<arg name="localization_ground_truth_frame_id" default="map_ground_truth_corrected" />
<arg name="map_frame_id" default="map" />
<arg name="odom_frame_id" default="odom" />
<arg name="base_link_frame_id" default="base_link" />
<arg name="sensor_frame_id" default="openni_rgb_optical_frame" />
<arg name="ambient_pointcloud_topic" default="/camera/depth/points2" />
<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)" />
<node pkg="tf" type="static_transform_publisher" name="tf_pub1" args="0 0 0 0.70711 0.70711 0 0 $(arg map_ground_truth_frame_id) ned 10" />
<node pkg="tf" type="static_transform_publisher" name="tf_pub2" args="0.0182521 -0.00901447 -0.0427566 0.508233 0.500217 0.510305 0.480699 vicon_vehicle_20 vicon_vehicle_20_corrected 10" />
<node pkg="tf" type="static_transform_publisher" name="tf_pub3" args="0 0 0 -0.5 0.5 -0.5 -0.5 vicon_vehicle_20_corrected $(arg base_link_frame_id) 10" />
<node pkg="tf" type="static_transform_publisher" name="tf_pub4" args="0 0 0 -0.5 0.5 -0.5 0.5 $(arg base_link_frame_id) openni_rgb_optical_frame 10" />
<node pkg="tf" type="static_transform_publisher" name="tf_odom" args="0 0 0 0 0 0 1 $(arg base_link_frame_id) $(arg odom_frame_id) 10" />
<node pkg="tf" type="static_transform_publisher" name="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 localization_ground_truth_frame_id) $(arg map_ground_truth_frame_id) 10" />
<arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization_tests)/launch/environments/asl/filters_3d_tof_slam.yaml" if="$(arg use_slam)" />
<arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization_tests)/launch/environments/asl/filters_3d_tof.yaml" unless="$(arg use_slam)" />
<include file="$(find dynamic_robot_localization_tests)/launch/localization_tests.launch" >
<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="ground_truth_tf_time_offset" default="$(arg ground_truth_tf_time_offset)" />
<arg name="ambient_pointcloud_topic" default="$(arg ambient_pointcloud_topic)" />
<arg name="use_dynamic_robot_localization" default="$(arg use_dynamic_robot_localization)" />
<arg name="use_ethzasl_icp_mapper" default="$(arg use_ethzasl_icp_mapper)" />
<arg name="ethzasl_icp_mapper_min_registration_overlap" default="$(arg ethzasl_icp_mapper_min_registration_overlap)" />
<arg name="ethzasl_icp_mapper_icp_config_filename" default="$(arg ethzasl_icp_mapper_icp_config_filename)" />
<arg name="use_6_dof" default="1" />
<arg name="use_tof" default="1" />
<arg name="use_laser_assembler" default="0" />
<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_preprocessed_save_filename" default="$(find dynamic_robot_localization_tests)/maps/asl/ethzasl_kinect_dataset/high-complexity-environment_icp-point-point_0.0025-voxel-grid_mls_0.005-rosbag-speed_preprocessed.ply" if="$(arg save_preprocessed_reference_cloud)" />
<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_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="map_ground_truth_frame_id" default="$(arg map_ground_truth_frame_id)" />
<arg name="localization_ground_truth_frame_id" default="$(arg localization_ground_truth_frame_id)" />
<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="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="world_rosbag_folder" default="$(arg world_rosbag_folder)" />
<arg name="world_rosbag_filename" default="$(arg world_rosbag_filename)" />
<arg name="world_name" default="ethzasl_kinect_dataset" />
<arg name="move_robot_on_path" default="0" />
<arg name="robot_path" default="" />
<arg name="rviz_folder" default="$(arg rviz_folder)" />
</include>
</launch>