Updated slam for ship interior tests in deltamatic.

niusha-tw · Jul 3, 2015 · 693e287 · 693e287
1 parent 32cc46f
commit 693e287
Show file tree

Hide file tree

Showing 5 changed files with 57 additions and 116 deletions.
diff --git a/...terior_corner/rosbags/2014-12-23-18-46-41_climbing_up_and_down_palette_facing_windows.bag b/...terior_corner/rosbags/2014-12-23-18-46-41_climbing_up_and_down_palette_facing_windows.bag
diff --git a/launch/environments/crob_lab/guardian/lab_with_windows.launch b/launch/environments/crob_lab/guardian/lab_with_windows.launch
@@ -10,6 +10,7 @@
 		<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_with_windows.yaml" />
 		<arg name="world_name" default="lab_with_windows" />
 		<arg name="move_robot_on_path" default="0" />
+		<arg name="use_gazebo" default="1" />
 		<arg name="play_rosbags" default="0" />
 		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/crob_lab/guardian" />
 		<arg name="rviz_folder" default="$(find dynamic_robot_localization_tests)/rviz/guardian/crob_lab" />

diff --git a/launch/environments/crob_lab/guardian/lab_without_windows.launch b/launch/environments/crob_lab/guardian/lab_without_windows.launch
@@ -10,6 +10,7 @@
 		<arg name="map_file" default="$(find dynamic_robot_localization_tests)/maps/crob_lab/planar/lab_without_windows.yaml" />
 		<arg name="world_name" default="lab_without_windows" />
 		<arg name="move_robot_on_path" default="0" />
+		<arg name="use_gazebo" default="1" />
 		<arg name="play_rosbags" default="0" />
 		<arg name="world_rosbag_folder" default="$(find dynamic_robot_localization_tests)/datasets/crob_lab/guardian" />
 		<arg name="rviz_folder" default="$(find dynamic_robot_localization_tests)/rviz/guardian/crob_lab" />

diff --git a/launch/environments/ship_interior_corner/ship_interior_corner_bags.launch b/launch/environments/ship_interior_corner/ship_interior_corner_bags.launch
@@ -54,8 +54,9 @@
 		<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_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_filters_filename" default="$(find dynamic_robot_localization)/yaml/configs/filters/filters_3d_slam.yaml" if="$(arg use_slam)" />
+		<arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization)/yaml/configs/filters/filters_3d.yaml" unless="$(arg use_slam)" />
+<!-- 		<arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/empty.yaml" if="$(arg use_slam)" /> -->
 		<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_lasers_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)" />
@@ -77,7 +78,7 @@
 		<arg name="reference_pointcloud_descriptors_filename" default="$(find dynamic_robot_localization_tests)/maps/ship_interior_corner/tridimensional/cache/ship_interior_corner_fpfh_descriptors_25mm.pcd" unless="$(arg recompute_point_features)" />
 		<arg name="reference_pointcloud_descriptors_save_filename" default="$(find dynamic_robot_localization_tests)/maps/ship_interior_corner/tridimensional/cache/ship_interior_corner_fpfh_descriptors_25mm.pcd" if="$(arg recompute_point_features)" />
 
-		<arg name="publish_ground_truth" default="1" />
+		<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" />
@@ -93,9 +94,9 @@
 
 <!-- 		<arg name="odometry_topic" default="/guardian/odom" /> -->
 		<arg name="odometry_topic" default="" />
-		<arg name="number_of_scans_to_assemble_per_cloud" default="6" />
+		<arg name="number_of_scans_to_assemble_per_cloud" default="2" />
 		<arg name="timeout_for_cloud_assembly" default="1.0" />
-		<arg name="min_number_of_scans_to_assemble_per_cloud" default="6" />
+		<arg name="min_number_of_scans_to_assemble_per_cloud" default="2" />
 		<arg name="max_number_of_scans_to_assemble_per_cloud" default="10" />
 		<arg name="min_timeout_seconds_for_cloud_assembly" default="0.3" />
 		<arg name="max_timeout_seconds_for_cloud_assembly" default="2.1" />

diff --git a/rviz/guardian/ship_interior/dynamic_robot_localization_tridimensional.rviz b/rviz/guardian/ship_interior/dynamic_robot_localization_tridimensional.rviz
@@ -5,9 +5,14 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /Grid1
+        - /Grid small1
         - /TF1/Frames1
         - /Sensors1
         - /Cloud registration1
+        - /Cloud registration1/PointCloud2 from map1
+        - /Cloud registration1/PointCloud2 inliers1
+        - /Cloud registration1/PointCloud2 outliers1
         - /Localization1
       Splitter Ratio: 0.699074
     Tree Height: 843
@@ -43,7 +48,7 @@ Visualization Manager:
     - Alpha: 0.5
       Cell Size: 1
       Class: rviz/Grid
-      Color: 54; 54; 54
+      Color: 0; 0; 0
       Enabled: true
       Line Style:
         Line Width: 0.03
@@ -55,13 +60,13 @@ Visualization Manager:
         Y: 0
         Z: 0
       Plane: XY
-      Plane Cell Count: 100
+      Plane Cell Count: 1000
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 0.5
-      Cell Size: 0.05
+      Cell Size: 0.1
       Class: rviz/Grid
-      Color: 160; 160; 164
+      Color: 87; 87; 89
       Enabled: true
       Line Style:
         Line Width: 0.03
@@ -73,105 +78,24 @@ Visualization Manager:
         Y: 0
         Z: 0
       Plane: XY
-      Plane Cell Count: 1000
+      Plane Cell Count: 100000
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
-      Enabled: true
+      Enabled: false
       Links:
         All Links Enabled: true
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        back_left_wheel:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        back_right_wheel:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_footprint:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        camera_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera_link_support_axis:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        front_left_wheel:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        front_right_wheel:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        hokuyo_back_laser_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        hokuyo_front_laser_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        hokuyo_front_laser_tilt_axis_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        hokuyo_front_laser_tilt_support_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_mpu_9150_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        left_track:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        right_track:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        support_bottom_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        support_top_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
       Update Interval: 0
-      Value: true
+      Value: false
       Visual Enabled: true
     - Class: rviz/TF
       Enabled: false
@@ -190,13 +114,13 @@ Visualization Manager:
     - Angle Tolerance: 0.1
       Class: rviz/Odometry
       Color: 85; 170; 0
-      Enabled: true
+      Enabled: false
       Keep: 1
       Length: 1
       Name: Odometry
       Position Tolerance: 0.001
       Topic: /guardian/odom
-      Value: true
+      Value: false
     - Arrow Length: 0.3
       Class: rviz/PoseArray
       Color: 255; 85; 0
@@ -373,17 +297,17 @@ Visualization Manager:
       Name: Paths
     - Class: rviz/Group
       Displays:
-        - Alpha: 0.5
+        - Alpha: 0.75
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 10
-            Min Value: -10
+            Max Value: 1.40662
+            Min Value: 0.00679513
             Value: true
           Axis: Z
           Channel Name: intensity
           Class: rviz/PointCloud2
           Color: 0; 85; 255
-          Color Transformer: FlatColor
+          Color Transformer: AxisColor
           Decay Time: 0
           Enabled: true
           Invert Rainbow: false
@@ -396,7 +320,7 @@ Visualization Manager:
           Queue Size: 1
           Selectable: true
           Size (Pixels): 1
-          Size (m): 0.01
+          Size (m): 0.015
           Style: Spheres
           Topic: /dynamic_robot_localization/reference_pointcloud
           Use Fixed Frame: true
@@ -541,8 +465,8 @@ Visualization Manager:
           Queue Size: 1
           Selectable: true
           Size (Pixels): 5
-          Size (m): 0.01
-          Style: Points
+          Size (m): 0.035
+          Style: Spheres
           Topic: /dynamic_robot_localization/aligned_pointcloud_inliers
           Use Fixed Frame: true
           Use rainbow: true
@@ -570,8 +494,8 @@ Visualization Manager:
           Queue Size: 1
           Selectable: true
           Size (Pixels): 8
-          Size (m): 0.01
-          Style: Points
+          Size (m): 0.035
+          Style: Spheres
           Topic: /dynamic_robot_localization/aligned_pointcloud_outliers
           Use Fixed Frame: true
           Use rainbow: true
@@ -674,6 +598,20 @@ Visualization Manager:
       Name: Cloud integration
     - Class: rviz/Group
       Displays:
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 25; 255; 0
+          Enabled: false
+          Line Style: Lines
+          Line Width: 0.03
+          Name: Path localization
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Topic: /localization_trajectory
+          Value: false
         - Arrow Length: 0.05
           Class: rviz/PoseArray
           Color: 255; 25; 0
@@ -688,9 +626,9 @@ Visualization Manager:
           Color: 0; 0; 255
           Enabled: true
           Head Length: 0.2
-          Head Radius: 0.25
+          Head Radius: 0.1
           Name: Localization Pose
-          Shaft Length: 0.8
+          Shaft Length: 0.3
           Shaft Radius: 0.02
           Shape: Arrow
           Topic: /dynamic_robot_localization/localization_pose
@@ -735,30 +673,30 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 8.98372
+      Distance: 4.77245
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 1.52524
-        Y: -2.33369
-        Z: -0.307846
+        X: 3.24056
+        Y: 0.0810108
+        Z: 0.305017
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.624796
+      Pitch: 0.404795
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.54317
+      Yaw: 4.20643
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
+    collapsed: true
   Height: 1056
-  Hide Left Dock: false
+  Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001b2000003dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003da000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065000000000000000780000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005c8000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001b2000003dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000003da000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065000000000000000780000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000780000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -768,5 +706,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1920
-  X: 0
+  X: 1920
   Y: 24