Skip to content

Commit

Permalink
Updated 6 DoF tests. Added labiomep tests. Improved slam 3 DoF.
Browse files Browse the repository at this point in the history
  • Loading branch information
@carlosmccosta
carlosmccosta committed Jun 9, 2015
1 parent 142442c commit 3646b02
Show file tree
Hide file tree
Showing 35 changed files with 808 additions and 243 deletions.
16 changes: 8 additions & 8 deletions launch/environments/asl/ethzasl_kinect_dataset.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
<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="1.0" unless="$(arg use_slam)" />
<arg name="play_rosbags_rate" default="0.01" if="$(arg use_slam)" />
<arg name="play_rosbags_rate" default="0.1" 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" />

Expand Down Expand Up @@ -38,15 +38,15 @@
<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="tf2_ros" type="static_transform_publisher" name="tf_pub1" args="0 0 0 0.70711 0.70711 0 0 $(arg map_ground_truth_frame_id) ned" />
<node pkg="tf2_ros" 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" />
<node pkg="tf2_ros" 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)" />
<node pkg="tf2_ros" 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" />
<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="tf2_ros" type="static_transform_publisher" name="tf_odom" args="0 0 0 0 0 0 1 base_link $(arg odom_frame_id)" />
<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)/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/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" >
Expand Down
137 changes: 137 additions & 0 deletions launch/environments/asl/filters_3d_tof_slam.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
localization_point_type: 'PointXYZRGBNormal'

filters:
reference_pointcloud:
1_voxel_grid:
leaf_size_x: 0.0025
leaf_size_y: 0.0025
leaf_size_z: 0.0025
filter_limit_field_name: 'z'
filter_limit_min: -5.0
filter_limit_max: 5.0
filtered_cloud_publish_topic: ''
downsample_all_data: true
save_leaf_layout: false
# 2_covariance_sampling:
# number_of_samples: 10000
# filtered_cloud_publish_topic: ''
# 3_random_sample:
# number_of_random_samples: 6000
# invert_sampling: false
# filtered_cloud_publish_topic: ''
ambient_pointcloud_integration_filters:
1_voxel_grid:
leaf_size_x: 0.0025
leaf_size_y: 0.0025
leaf_size_z: 0.0025
filter_limit_field_name: 'z'
filter_limit_min: -5.0
filter_limit_max: 5.0
filtered_cloud_publish_topic: ''
downsample_all_data: true
save_leaf_layout: false
4_crop_box:
box_min_x: -2.0
box_min_y: -2.0
box_min_z: 0.4
box_max_x: 2.0
box_max_y: 2.0
box_max_z: 3.5
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: ''
5_random_sample:
number_of_random_samples: 10000
invert_sampling: false
filtered_cloud_publish_topic: ''
# ambient_pointcloud_integration_filters_map_frame:
# 4_crop_box:
# box_min_x: 1.5
# box_min_y: -2.0
# box_min_z: -0.2
# box_max_x: 3.5
# box_max_y: 1.0
# box_max_z: 1.5
# 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: ''
ambient_pointcloud:
1_voxel_grid:
leaf_size_x: 0.0025
leaf_size_y: 0.0025
leaf_size_z: 0.0025
filter_limit_field_name: 'z'
filter_limit_min: -5.0
filter_limit_max: 5.0
filtered_cloud_publish_topic: ''
downsample_all_data: true
save_leaf_layout: false
# 2_approximate_voxel_grid:
# leaf_size_x: 0.025
# leaf_size_y: 0.025
# leaf_size_z: 0.025
# filtered_cloud_publish_topic: ''
# downsample_all_data: false
# 3_radius_outlier_removal:
# radius_search: 2.0
# min_neighbors_in_radius: 6
# invert_removal: false
# filtered_cloud_publish_topic: ''
4_crop_box:
box_min_x: -2.0
box_min_y: -2.0
box_min_z: 0.4
box_max_x: 2.0
box_max_y: 2.0
box_max_z: 3.5
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: ''
5_random_sample:
number_of_random_samples: 2500
invert_sampling: false
filtered_cloud_publish_topic: ''
# 6_statistical_outlier_removal:
# number_of_neighbors_for_mean_distance_estimation: 6
# standard_deviation_multiplier: 2.0
# invert_selection: false
# filtered_cloud_publish_topic: ''
# 7_covariance_sampling:
# number_of_samples: 1000
# filtered_cloud_publish_topic: ''
# 8_radius_outlier_removal:
# radius_search: 1.0
# min_neighbors_in_radius: 2
# invert_removal: false
# filtered_cloud_publish_topic: ''
ambient_pointcloud_map_frame:
4_crop_box:
box_min_x: 1.5
box_min_y: -2.0
box_min_z: -0.2
box_max_x: 3.5
box_max_y: 1.0
box_max_z: 1.5
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: 'ambient_pointcloud_filtered'
3 changes: 2 additions & 1 deletion launch/environments/crob_lab/guardian/bags/test_bag_guardian_controller_1.launch
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<include file="$(find dynamic_robot_localization_tests)/launch/environments/crob_lab/guardian/lab_slam_guardian.launch" >
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam.yaml" />
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_10mm.yaml" />
<!-- <arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam.yaml" /> -->
<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/crob_lab/guardian/guardian_controller_1" />
<arg name="robot_initial_pose_in_base_to_map" default="true" />
<arg name="robot_initial_x" default="5.3" />
Expand Down
4 changes: 3 additions & 1 deletion launch/environments/crob_lab/guardian/bags/test_bag_guardian_controller_2.launch
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<include file="$(find dynamic_robot_localization_tests)/launch/environments/crob_lab/guardian/lab_slam_guardian.launch" >
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam.yaml" />
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_10mm.yaml" />
<!-- <arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam.yaml" /> -->
<!-- <arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam_corner_drl_2.yaml" /> -->
<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/crob_lab/guardian/guardian_controller_2" />
<arg name="robot_initial_pose_in_base_to_map" default="true" />
<arg name="robot_initial_x" default="-3.35" />
Expand Down
3 changes: 2 additions & 1 deletion launch/environments/crob_lab/guardian/bags/test_bag_guardian_controller_3.launch
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<include file="$(find dynamic_robot_localization_tests)/launch/environments/crob_lab/guardian/lab_slam_guardian.launch" >
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam.yaml" />
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_10mm.yaml" />
<!-- <arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam.yaml" /> -->
<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/crob_lab/guardian/guardian_controller_3" />
<arg name="robot_initial_pose_in_base_to_map" default="true" />
<arg name="robot_initial_x" default="8.65" />
Expand Down
5 changes: 3 additions & 2 deletions launch/environments/crob_lab/guardian/lab_slam_guardian.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,10 @@
<launch>
<arg name="play_rosbags" default="1" />
<arg name="use_map_server" default="1" />
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam_corner_drl.yaml" />
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam.yaml" />
<!-- <arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_slam_corner_drl_2.yaml" /> -->
<!-- <arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_10mm.yaml" /> -->
<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/crob_lab/guardian/lab_2015-04-02-14-44-24" />
<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/crob_lab/guardian/guardian_1" />
<arg name="robot_initial_pose_in_base_to_map" default="true" />
<arg name="robot_initial_x" default="-1.34" />
<arg name="robot_initial_y" default="-0.98" />
Expand Down
28 changes: 28 additions & 0 deletions launch/environments/labiomep/labiomep_robot_tfs.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_1" args="0.0 0.0 0.0635 0.0 0.0 0.0 base_footprint base_link" />

<!-- front laser -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_2" args="0.46 -0.0025 0.2535 0.0 0.0 0.0 base_link hokuyo_front_laser_tilt_support_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_3" args="0.0 0.0 0.0 0.0 0.0 0.0 hokuyo_front_laser_tilt_support_link hokuyo_front_laser_tilt_axis_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_4" args="0.026 -0.0015 0.0565 -0.023 0.0 -0.013 hokuyo_front_laser_tilt_axis_link hokuyo_front_laser_link" />

<!-- back laser -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_5" args="-0.433 -0.0085 0.3065 3.14159265359 0.0 0.0 base_link hokuyo_back_laser_link" />

<!-- IMUs -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_6" args="0.0775 0.0 0.1555 0.0 0.0 0.0 base_link imu_mpu_9150_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_7" args="0.0560 0.0 0.1355 0.0 0.0 0.0 base_link imu_um7_link" />

<!-- support -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_8" args="0.185 0 0.2425 0.0 0.0 0.0 base_link support_bottom_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_9" args="0.33 0.0 0.265 0.0 0.0 0.0 support_bottom_link support_top_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_10" args="-0.02 -0.015 0.0 0.015 0.63 1.58 support_top_link camera_link_support_axis" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_11" args="0.045 0.06 0.017 0.0 0.0 0.0 camera_link_support_axis camera_link" />

<!-- structure io -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_12" args="0.0 -0.045 0.0 0.0 0.0 0.0 /camera_link /camera_rgb_frame" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_13" args="0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 /camera_rgb_frame /camera_rgb_optical_frame" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_14" args="0.0 -0.02 0.0 0.0 0.0 0.0 /camera_link /camera_depth_frame" />
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_robot_labiomep_15" args="0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 /camera_depth_frame /camera_depth_optical_frame" />
</launch>
1 change: 1 addition & 0 deletions ...test_bag_guardian_ship_interior_corner_climbing_up_and_down_palette_facing_windows.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
<arg name="world_rosbag_filename" default="$(find dynamic_robot_localization_tests)/datasets/ship_interior_corner/rosbags/2014-12-23-18-46-41_climbing_up_and_down_palette_facing_windows" />
<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/ship_interior_corner/planar/ship_interior_corner_1mm.yaml" />
<arg name="rviz_folder" default="$(find dynamic_robot_localization_tests)/rviz/guardian/ship_interior" />
<arg name="laser_scan_topics" default="/guardian/laser_tilt_front" />
<arg name="robot_initial_pose_in_base_to_map" default="true" />
<arg name="robot_initial_x" default="3.0" />
<arg name="robot_initial_y" default="-2.3" />
Expand Down
14 changes: 11 additions & 3 deletions launch/environments/ship_interior_corner/ship_interior_corner_bags.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
<arg name="recompute_point_features" default="0" />
<arg name="load_point_features" default="0" />
<arg name="save_point_features" default="0" />
<arg name="laser_scan_topics" default="/guardian/laser_tilt_front+/guardian/laser_horizontal_back" />

<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 map_ground_truth_frame_id) $(arg localization_ground_truth_frame_id) 50" />

Expand All @@ -47,7 +48,12 @@
<arg name="load_point_features" default="$(arg load_point_features)" />
<arg name="save_point_features" default="$(arg save_point_features)" />
<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="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization)/yaml/configs/filters/filters_3d.yaml" />
<!-- <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_3d.yaml" />
<arg name="yaml_configuration_recovery_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_recovery/recovery_3d.yaml" />

<arg name="reference_pointcloud_filename" default="$(find dynamic_robot_localization_tests)/maps/ship_interior_corner/tridimensional/pcd/ship_interior_corner_35mm.pcd" />
<!-- <arg name="reference_pointcloud_filename" default="$(find dynamic_robot_localization_tests)/maps/ship_interior_corner/tridimensional/pcd/ship_interior_corner_25mm.pcd" if="$(arg process_reference_cloud)" />
Expand Down Expand Up @@ -80,8 +86,10 @@
<arg name="min_number_of_scans_to_assemble_per_cloud" default="6" />
<arg name="max_number_of_scans_to_assemble_per_cloud" default="20" />
<arg name="min_timeout_seconds_for_cloud_assembly" default="0.3" />
<arg name="max_timeout_seconds_for_cloud_assembly" default="1.1" />
<arg name="laser_scan_topics" default="/guardian/laser_tilt_front+/guardian/laser_horizontal_back" />
<arg name="max_timeout_seconds_for_cloud_assembly" default="2.1" />
<arg name="max_linear_velocity" default="0.05" />
<arg name="max_angular_velocity" default="0.174532925" />
<arg name="laser_scan_topics" default="$(arg laser_scan_topics)" />
<!-- <arg name="laser_scan_topics" default="/guardian/laser_tilt_front" /> -->
<arg name="laser_scan_topic_amcl" default="/guardian/laser_tilt_front" />

Expand Down
Loading

0 comments on commit 3646b02

Please sign in to comment.