Changed 6 DoF tests in ship_interior to use both kinect and lasers.

niusha-tw · Mar 8, 2015 · 2281217 · 2281217
1 parent 2ab1165
commit 2281217
Show file tree

Hide file tree

Showing 27 changed files with 253 additions and 112 deletions.
diff --git a/..._freiburg2_pioneer_slam_accelerometer.txt → ..._freiburg2_pioneer_slam_accelerometer.txt b/..._freiburg2_pioneer_slam_accelerometer.txt → ..._freiburg2_pioneer_slam_accelerometer.txt
diff --git a/..._dataset_freiburg2_pioneer_slam_depth.flv → ..._dataset_freiburg2_pioneer_slam_depth.flv b/..._dataset_freiburg2_pioneer_slam_depth.flv → ..._dataset_freiburg2_pioneer_slam_depth.flv
diff --git a/...et_freiburg2_pioneer_slam_groundtruth.txt → ...et_freiburg2_pioneer_slam_groundtruth.txt b/...et_freiburg2_pioneer_slam_groundtruth.txt → ...et_freiburg2_pioneer_slam_groundtruth.txt
diff --git a/...d_dataset_freiburg2_pioneer_slam_info.txt → ...d_dataset_freiburg2_pioneer_slam_info.txt b/...d_dataset_freiburg2_pioneer_slam_info.txt → ...d_dataset_freiburg2_pioneer_slam_info.txt
diff --git a/...rg2_pioneer_slam_lasers_only_fixed_tf.bag → ...rg2_pioneer_slam_lasers_only_fixed_tf.bag b/...rg2_pioneer_slam_lasers_only_fixed_tf.bag → ...rg2_pioneer_slam_lasers_only_fixed_tf.bag
diff --git a/...y_fixed_tf_without_base_link_to_laser.bag → ...y_fixed_tf_without_base_link_to_laser.bag b/...y_fixed_tf_without_base_link_to_laser.bag → ...y_fixed_tf_without_base_link_to_laser.bag
diff --git a/...bd_dataset_freiburg2_pioneer_slam_rgb.flv → ...bd_dataset_freiburg2_pioneer_slam_rgb.flv b/...bd_dataset_freiburg2_pioneer_slam_rgb.flv → ...bd_dataset_freiburg2_pioneer_slam_rgb.flv
diff --git a/...environment_dynamic_move_polaris_robot.sh → ...ments/ship_interior/move_polaris_robot.sh b/...environment_dynamic_move_polaris_robot.sh → ...ments/ship_interior/move_polaris_robot.sh
diff --git a/launch/environments/ship_interior/ship_interior.launch b/launch/environments/ship_interior/ship_interior.launch
@@ -1,14 +1,21 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
+	<arg name="use_fast_path" default="0" />
+	<arg name="yaml_configuration_base_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/static" />
+	<arg name="world_name" default="ship_interior" />
+
 	<include file="$(find dynamic_robot_localization_tests)/launch/localization_tests.launch" >
+		<arg name="use_fast_path" default="$(arg use_fast_path)" />
+		<arg name="yaml_configuration_base_filename" default="$(arg yaml_configuration_base_filename)" />
+		<arg name="world_name" default="$(arg world_name)" />
 		<arg name="robot_initial_pose_in_base_to_map" default="true" />
 		<arg name="robot_initial_x" default="-4.5" />
 		<arg name="robot_initial_y" default="2.25" />
 		<arg name="robot_initial_z" default="0.004665" />
 		<arg name="robot_initial_yaw" default="-1.57079633" />
-		<arg name="yaml_configuration_base_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/static" />
-		<arg name="world_name" default="ship_interior" />
-		<arg name="move_robot_on_path" default="1" />
+		<arg name="play_rosbags" default="1" />
+		<arg name="move_robot_on_path" default="0" />
+		<arg name="robot_path" default="$(find dynamic_robot_localization_tests)/maps/ship_interior/path" />
 		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/ship_interior" />
 	</include>
 </launch>
diff --git a/launch/environments/ship_interior/ship_interior_cluttered.launch b/launch/environments/ship_interior/ship_interior_cluttered.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+	<arg name="use_fast_path" default="0" />
+
+	<include file="$(find dynamic_robot_localization_tests)/launch/environments/ship_interior/ship_interior.launch" >
+		<arg name="use_fast_path" default="$(arg use_fast_path)" />
+		<arg name="yaml_configuration_base_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/cluttered_environments" />
+		<arg name="world_name" default="ship_interior_with_cluttered_environment" />
+	</include>
+</launch>
diff --git a/...with_cluttered_environment_dynamic.launch → ...or/ship_interior_cluttered_dynamic.launch b/...with_cluttered_environment_dynamic.launch → ...or/ship_interior_cluttered_dynamic.launch
@@ -1,15 +1,11 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-	<include file="$(find dynamic_robot_localization_tests)/launch/localization_tests.launch" >
-		<arg name="robot_initial_pose_in_base_to_map" default="true" />
-		<arg name="robot_initial_x" default="-4.5" />
-		<arg name="robot_initial_y" default="2.25" />
-		<arg name="robot_initial_z" default="0.004665" />
-		<arg name="robot_initial_yaw" default="-1.57079633" />
+	<arg name="use_fast_path" default="0" />
+
+	<include file="$(find dynamic_robot_localization_tests)/launch/environments/ship_interior/ship_interior.launch" >
+		<arg name="use_fast_path" default="$(arg use_fast_path)" />
 		<arg name="yaml_configuration_base_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/cluttered_environments_dynamic" />
 		<arg name="world_name" default="ship_interior_with_cluttered_environment_dynamic" />
-		<arg name="move_robot_on_path" default="1" />
-		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/ship_interior" />
 	</include>
 
 	<node pkg="rostopic" type="rostopic" name="move_polaris_robot" output="screen"

diff --git a/launch/environments/ship_interior/ship_interior_filters_unstable_ground.yaml b/launch/environments/ship_interior/ship_interior_filters_unstable_ground.yaml
@@ -0,0 +1,41 @@
+message_management:
+    maximum_number_points_ambient_pointcloud_circular_buffer: 275
+    max_seconds_ambient_pointcloud_offset_to_last_estimated_pose: 0.100
+
+filters:
+    reference_pointcloud:
+        1_crop_box:
+            box_min_x: -5.39
+            box_min_y: -4.184
+            box_min_z: 0.07
+            box_max_x: 3.004
+            box_max_y: 8.21
+            box_max_z: 2.594
+            box_translation_x: 0.0
+            box_translation_y: 0.0
+            box_translation_z: 0.0
+            box_rotation_roll: 0.0
+            box_rotation_pitch: 0.0
+            box_rotation_yaw: 0.0
+            invert_selection: false
+            filtered_cloud_publish_topic: ''
+        2_random_sample:
+            number_of_random_samples: 300000
+            invert_sampling: false
+            filtered_cloud_publish_topic: ''
+    ambient_pointcloud:
+        filter_ambient_cloud_in_map_frame: false
+        1_voxel_grid:
+            leaf_size_x: 0.070
+            leaf_size_y: 0.070
+            leaf_size_z: 0.070
+            filter_limit_field_name: 'z'
+            filter_limit_min: -10.0
+            filter_limit_max: 10.0
+            filtered_cloud_publish_topic: ''
+            downsample_all_data: true
+            save_leaf_layout: false
+        2_random_sample:
+            number_of_random_samples: 150
+            invert_sampling: false
+            filtered_cloud_publish_topic: 'ambient_pointcloud_filtered'
diff --git a/launch/environments/ship_interior/ship_interior_tof.launch b/launch/environments/ship_interior/ship_interior_tof.launch
diff --git a/launch/environments/ship_interior/ship_interior_tof_and_lasers.launch b/launch/environments/ship_interior/ship_interior_tof_and_lasers.launch
@@ -0,0 +1,42 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+	<arg name="robot_initial_pose_in_base_to_map" default="true" />
+	<arg name="robot_initial_x" default="-4.5" />
+	<arg name="robot_initial_y" default="2.25" />
+	<arg name="robot_initial_z" default="0.004665" />
+	<arg name="robot_initial_yaw" default="-1.57079633" />
+	<arg name="world_name" default="ship_interior" />
+	<arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/unstable_ground_3d_tof.yaml" />
+	<arg name="robot_path" default="$(find dynamic_robot_localization_tests)/maps/ship_interior/path" />
+	<arg name="use_fast_path" default="0" />
+
+	<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_yaw" default="$(arg robot_initial_yaw)" />
+		<arg name="world_name" default="$(arg world_name)" />
+		<arg name="robot_path" default="$(arg robot_path)" />
+		<arg name="use_fast_path" default="$(arg use_fast_path)" />
+		<arg name="process_reference_cloud" default="0" />
+		<arg name="load_point_features" default="1" />
+		<arg name="reference_pointcloud_filename" default="$(find dynamic_robot_localization_tests)/maps/ship_interior/tridimensional/pcd/ship_interior_70mm_n30.pcd" />
+		<arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization_tests)/launch/environments/ship_interior/ship_interior_filters_unstable_ground.yaml" />
+		<arg name="yaml_configuration_tracking_filename" default="$(arg yaml_configuration_pose_estimation_filename)" />
+		<arg name="yaml_configuration_recovery_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_recovery/recovery_3d_large_map.yaml" />
+		<arg name="play_rosbags" default="0" />
+		<arg name="ambient_pointcloud_topic" default="ambient_pointcloud+/camera/depth/points" />
+		<arg name="use_tof" default="1" />
+		<arg name="use_6_dof" default="1" />
+		<arg name="use_laser_assembler" default="1" />
+		<arg name="assemble_lasers_on_map_frame" default="1" />
+		<arg name="number_of_scans_to_assemble_per_cloud" default="20" />
+		<arg name="min_number_of_scans_to_assemble_per_cloud" default="2" />
+		<arg name="max_number_of_scans_to_assemble_per_cloud" default="60" />
+		<arg name="min_timeout_seconds_for_cloud_assembly" default="0.5" />
+		<arg name="max_timeout_seconds_for_cloud_assembly" default="3.0" />
+		<arg name="move_robot_on_path" default="1" />
+		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/ship_interior" />
+	</include>
+</launch>
diff --git a/...ship_interior_with_unstable_ground.launch → ...rior/ship_interior_unstable_ground.launch b/...ship_interior_with_unstable_ground.launch → ...rior/ship_interior_unstable_ground.launch
@@ -1,16 +1,15 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-	<include file="$(find dynamic_robot_localization_tests)/launch/localization_tests.launch" >
+	<arg name="use_fast_path" default="0" />
+
+	<include file="$(find dynamic_robot_localization_tests)/launch/environments/ship_interior/ship_interior_tof_and_lasers.launch" >
+		<arg name="use_fast_path" default="$(arg use_fast_path)" />
 		<arg name="robot_initial_pose_in_base_to_map" default="true" />
 		<arg name="robot_initial_x" default="-1.25" />
 		<arg name="robot_initial_y" default="-3.445" />
 		<arg name="robot_initial_z" default="0.004665" />
 		<arg name="robot_initial_yaw" default="0" />
-		<arg name="yaml_configuration_base_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/unstable_ground" />
-		<arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/empty.yaml" />
-		<arg name="world_name" default="ship_interior_with_unstable_ground" />
-		<arg name="move_robot_on_path" default="1" />
+		<arg name="world_name" default="ship_interior_with_unstable_ground" />>
 		<arg name="robot_path" default="$(find dynamic_robot_localization_tests)/maps/ship_interior/path_for_unstable_ground" />
-		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/ship_interior" />
 	</include>
 </launch>
diff --git a/...ed_environment_and_unstable_ground.launch → ...interior_unstable_ground_cluttered.launch b/...ed_environment_and_unstable_ground.launch → ...interior_unstable_ground_cluttered.launch
@@ -1,15 +1,15 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-	<include file="$(find dynamic_robot_localization_tests)/launch/localization_tests.launch" >
+	<arg name="use_fast_path" default="0" />
+
+	<include file="$(find dynamic_robot_localization_tests)/launch/environments/ship_interior/ship_interior_tof_and_lasers.launch" >
+		<arg name="use_fast_path" default="$(arg use_fast_path)" />
 		<arg name="robot_initial_pose_in_base_to_map" default="true" />
 		<arg name="robot_initial_x" default="-4.5" />
 		<arg name="robot_initial_y" default="2.25" />
 		<arg name="robot_initial_z" default="0.004665" />
 		<arg name="robot_initial_yaw" default="-1.57079633" />
-		<arg name="yaml_configuration_base_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/unstable_ground" />
 		<arg name="world_name" default="ship_interior_with_cluttered_environment_and_unstable_ground" />
-		<arg name="move_robot_on_path" default="1" />
 		<arg name="robot_path" default="$(find dynamic_robot_localization_tests)/maps/ship_interior/path_for_unstable_ground_complex" />
-		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/ship_interior" />
 	</include>
 </launch>
diff --git a/...onment_dynamic_and_unstable_ground.launch → ..._unstable_ground_cluttered_dynamic.launch b/...onment_dynamic_and_unstable_ground.launch → ..._unstable_ground_cluttered_dynamic.launch
@@ -1,16 +1,16 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-	<include file="$(find dynamic_robot_localization_tests)/launch/localization_tests.launch" >
+	<arg name="use_fast_path" default="0" />
+
+	<include file="$(find dynamic_robot_localization_tests)/launch/environments/ship_interior/ship_interior_tof_and_lasers.launch" >
+		<arg name="use_fast_path" default="$(arg use_fast_path)" />
 		<arg name="robot_initial_pose_in_base_to_map" default="true" />
 		<arg name="robot_initial_x" default="-4.5" />
 		<arg name="robot_initial_y" default="2.25" />
 		<arg name="robot_initial_z" default="0.004665" />
 		<arg name="robot_initial_yaw" default="-1.57079633" />
-		<arg name="yaml_configuration_base_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/unstable_ground" />
 		<arg name="world_name" default="ship_interior_with_cluttered_environment_dynamic_and_unstable_ground" />
-		<arg name="move_robot_on_path" default="1" />
 		<arg name="robot_path" default="$(find dynamic_robot_localization_tests)/maps/ship_interior/path_for_unstable_ground_complex" />
-		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/ship_interior" />
 	</include>
 
 	<node pkg="rostopic" type="rostopic" name="move_polaris_robot" output="screen"

diff --git a/...erior_with_unstable_ground_complex.launch → ...p_interior_unstable_ground_complex.launch b/...erior_with_unstable_ground_complex.launch → ...p_interior_unstable_ground_complex.launch
@@ -1,16 +1,15 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-	<include file="$(find dynamic_robot_localization_tests)/launch/localization_tests.launch" >
+	<arg name="use_fast_path" default="0" />
+
+	<include file="$(find dynamic_robot_localization_tests)/launch/environments/ship_interior/ship_interior_tof_and_lasers.launch" >
+		<arg name="use_fast_path" default="$(arg use_fast_path)" />
 		<arg name="robot_initial_pose_in_base_to_map" default="true" />
 		<arg name="robot_initial_x" default="-4.5" />
 		<arg name="robot_initial_y" default="2.25" />
 		<arg name="robot_initial_z" default="0.004665" />
 		<arg name="robot_initial_yaw" default="-1.57079633" />
-		<arg name="yaml_configuration_base_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/unstable_ground" />
-		<arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/empty.yaml" />
 		<arg name="world_name" default="ship_interior_with_unstable_ground_complex" />
-		<arg name="move_robot_on_path" default="1" />
 		<arg name="robot_path" default="$(find dynamic_robot_localization_tests)/maps/ship_interior/path_for_unstable_ground_complex" />
-		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/ship_interior" />
 	</include>
 </launch>
diff --git a/launch/environments/ship_interior/ship_interior_with_cluttered_environment.launch b/launch/environments/ship_interior/ship_interior_with_cluttered_environment.launch
diff --git a/launch/environments/ship_interior_corner/ship_interior_corner_slam.launch b/launch/environments/ship_interior_corner/ship_interior_corner_slam.launch
@@ -24,7 +24,7 @@
 		<arg name="target_frame" default="base_link" />
 	</include>
 
-	<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan" name="pointcloud_to_laserscan" if="$(arg use_laser_assembler)" >
+	<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan" if="$(arg use_laser_assembler)" >
 		<param name="target_frame" value="base_link" />
 		<remap from="cloud_in" to="ambient_pointcloud" />
 		<remap from="scan" to="/scan" />

diff --git a/...ts/tum/bags/freiburg2_pioneer_slam.launch → ...s/tum/bags/freiburg2_pioneer_slam1.launch b/...ts/tum/bags/freiburg2_pioneer_slam.launch → ...s/tum/bags/freiburg2_pioneer_slam1.launch
@@ -1,8 +1,8 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
 	<include file="$(find dynamic_robot_localization_tests)/launch/environments/tum/freiburg2_pioneer.launch" >
-		<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/tum/freiburg2_pioneer_slam/rgbd_dataset_freiburg2_pioneer_slam_lasers_only_fixed_tf_without_base_link_to_laser" />
-		<arg name="ground_truth_filename" default="$(find dynamic_robot_localization_tests)/datasets/tum/freiburg2_pioneer_slam/rgbd_dataset_freiburg2_pioneer_slam_groundtruth.txt" />
+		<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/tum/freiburg2_pioneer_slam1/rgbd_dataset_freiburg2_pioneer_slam_lasers_only_fixed_tf_without_base_link_to_laser" />
+		<arg name="ground_truth_filename" default="$(find dynamic_robot_localization_tests)/datasets/tum/freiburg2_pioneer_slam1/rgbd_dataset_freiburg2_pioneer_slam_groundtruth.txt" />
 
 
 		<arg name="robot_initial_pose_in_base_to_map" default="true" />

diff --git a/launch/environments/tum/freiburg2_pioneer.launch b/launch/environments/tum/freiburg2_pioneer.launch
@@ -2,8 +2,8 @@
 <launch>
 	<arg name="play_rosbags" default="1" />
 	<arg name="play_rosbags_rate" default="1.0" />
-	<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/tum/freiburg2_pioneer_slam/rgbd_dataset_freiburg2_pioneer_slam_lasers_only_fixed_tf" />
-	<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/tum/freiburg2_pioneer_slam" />
+	<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/tum/freiburg2_pioneer_slam1/rgbd_dataset_freiburg2_pioneer_slam_lasers_only_fixed_tf" />
+	<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/tum/freiburg2_pioneer_slam1" />
 
 	<arg name="use_map_server" default="1" />
 	<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/tum/freiburg2_pioneer_slam/drl_corrected.yaml" />

diff --git a/launch/localization_tests.launch b/launch/localization_tests.launch
@@ -153,7 +153,9 @@
 	<arg name="use_tilt_on_front_laser" default="1" if="$(arg use_6_dof)" />
 	<arg name="use_laser_assembler" default="1" unless="$(arg use_tof)" />
 	<arg name="use_laser_assembler" default="0" if="$(arg use_tof)" />
-	<arg name="laser_assembly_frame" default="$(arg odom_frame_id)" />
+	<arg name="assemble_lasers_on_map_frame" default="0" /> <!-- if false -> assembles lasers on odom frame -->
+	<arg name="laser_assembly_frame" default="$(arg map_frame_id)" if="$(arg assemble_lasers_on_map_frame)" />
+	<arg name="laser_assembly_frame" default="$(arg odom_frame_id)" unless="$(arg assemble_lasers_on_map_frame)" />
 	<arg name="number_of_tf_queries_for_spherical_interpolation" default="4" />
 	<arg name="min_range_cutoff_percentage_offset" default="2.00" />
 	<arg name="max_range_cutoff_percentage_offset" default="0.95" />

diff --git a/rviz/asl/dynamic_robot_localization_tridimensional.rviz b/rviz/asl/dynamic_robot_localization_tridimensional.rviz
@@ -12,7 +12,7 @@ Panels:
         - /Cloud registration1/PointCloud2 after registration1
         - /Cloud integration1
         - /Localization1
-      Splitter Ratio: 0.613248
+      Splitter Ratio: 0.707386
     Tree Height: 745
   - Class: rviz/Selection
     Name: Selection
@@ -206,7 +206,7 @@ Visualization Manager:
           Color: 140; 70; 0
           Color Transformer: RGB8
           Decay Time: 0
-          Enabled: false
+          Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Max Intensity: 4096
@@ -222,7 +222,7 @@ Visualization Manager:
           Topic: /dynamic_robot_localization/ambient_pointcloud_filtered
           Use Fixed Frame: true
           Use rainbow: true
-          Value: false
+          Value: true
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -503,5 +503,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1580
-  X: 2171
+  X: 2164
   Y: 24