diff --git a/launch/sim_sample.launch b/launch/sim_sample.launch
index 1a68b2bf0303a2787ff5e72d1c1d626c3e5ec78a..5c40cb805c8cdb15e40f6cc455d6fe6e39e03190 100644
--- a/launch/sim_sample.launch
+++ b/launch/sim_sample.launch
@@ -4,24 +4,15 @@
   <arg name="ns"    default="ana"/>
   <arg name="world_name" default="empty"/>
   <arg name="world"      default="$(find iri_gazebo_worlds)/worlds/$(arg world_name).world"/>
+  <arg name="world_description" default="true"/>
   <arg name="rviz"  default="false"/>
   <arg name="gui"   default="true"/>
 
-  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="paused"              value="false"/>
-    <arg name="use_sim_time"        value="true"/>
-    <arg name="extra_gazebo_args"   value=""/>
-    <arg name="gui"                 value="$(arg gui)"/>
-    <arg name="recording"           value="false"/>
-    <arg name="headless"            value="false"/>
-    <arg name="debug"               value="false"/>
-    <arg name="physics"             value="ode"/>
-    <arg name="verbose"             value="false"/>
-    <arg name="world_name"          value="$(arg world)"/> 
-    <arg name="respawn_gazebo"      value="false"/>
-    <arg name="use_clock_frequency" value="false"/>
-    <arg name="pub_clock_frequency" value="100"/>
+  <include file="$(find iri_gazebo_worlds)/launch/test.launch">
+    <arg name="gui"         value="$(arg gui)"/>
+    <arg name="world_name"  value="$(arg world_name)"/> 
+    <arg name="description" value="$(arg world_description)"/> 
+    <arg name="rviz"        value="false"/> 
   </include>
 
   <include file="$(find iri_ana_gazebo)/launch/spawn.launch">
diff --git a/rviz/ana.rviz b/rviz/ana.rviz
index 7b3d927e125cd14f0356f98aa1fe0801f9161086..3c0dbdb5af7874b5e8ef7d132e442b6cc56bd920 100644
--- a/rviz/ana.rviz
+++ b/rviz/ana.rviz
@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /Status1
+        - /Sensors1
       Splitter Ratio: 0.5
     Tree Height: 594
   - Class: rviz/Selection
@@ -27,6 +27,8 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: Velodyne
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -290,6 +292,10 @@ Visualization Manager:
           Value: true
         ana/velodyne_base:
           Value: true
+        map:
+          Value: true
+        world:
+          Value: true
       Marker Scale: 0.100000001
       Name: TF
       Show Arrows: true
@@ -345,87 +351,114 @@ Visualization Manager:
                 {}
       Update Interval: 0
       Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 0.00564962626
-        Min Value: -0.00647342205
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 999999
-      Min Color: 0; 0; 0
-      Min Intensity: 999999
-      Name: Velodyne
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.00999999978
-      Style: Points
-      Topic: /ana/sensors/velodyne_points
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 0.5
-      Class: rviz_plugin_tutorials/Imu
-      Color: 204; 51; 204
-      Enabled: true
-      History Length: 1
-      Name: Imu
-      Topic: /ana/sensors/imu
-      Unreliable: false
-      Value: true
-    - Class: rviz/Image
-      Enabled: true
-      Image Topic: /ana/sensors/nav_cam/color/image_raw
-      Max Value: 1
-      Median window: 5
-      Min Value: 0
-      Name: realsense_color
-      Normalize Range: true
-      Queue Size: 2
-      Transport Hint: raw
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 1.46991944
+            Min Value: -0.123292208
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: AxisColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 999999
+          Min Color: 0; 0; 0
+          Min Intensity: 999999
+          Name: Velodyne
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.00999999978
+          Style: Points
+          Topic: /ana/sensors/velodyne_points
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz/Image
+          Enabled: true
+          Image Topic: /ana/sensors/nav_cam/color/image_raw
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: realsense_color
+          Normalize Range: true
+          Queue Size: 2
+          Transport Hint: raw
+          Unreliable: false
+          Value: true
+        - Class: rviz/Camera
+          Enabled: true
+          Image Rendering: background and overlay
+          Image Topic: /ana/sensors/nav_cam/color/image_raw
+          Name: Camera
+          Overlay Alpha: 0.100000001
+          Queue Size: 2
+          Transport Hint: compressed
+          Unreliable: false
+          Value: true
+          Visibility:
+            Grid: true
+            RobotModel: false
+            Sensors:
+              Camera: true
+              Imu: false
+              Value: true
+              Velodyne: true
+              realsense_color: true
+              realsense_pointcloud: true
+            TF: false
+            Value: true
+          Zoom Factor: 1
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: realsense_pointcloud
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.00999999978
+          Style: Points
+          Topic: /ana/sensors/nav_cam/depth_registered/points
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.5
+          Class: rviz_plugin_tutorials/Imu
+          Color: 204; 51; 204
+          Enabled: true
+          History Length: 1
+          Name: Imu
+          Topic: /ana/sensors/imu
+          Unreliable: false
+          Value: true
       Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: realsense_pointcloud
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.00999999978
-      Style: Points
-      Topic: /ana/sensors/nav_cam/depth_registered/points
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
+      Name: Sensors
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -472,12 +505,14 @@ Visualization Manager:
       Yaw: 0.785398185
     Saved: ~
 Window Geometry:
+  Camera:
+    collapsed: false
   Displays:
     collapsed: false
   Height: 807
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e1000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000019f000002e1fc0200000004fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f00720100000028000002e10000001600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002fd000002e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e1000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000019f000002e1fc0200000005fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007201000000280000018a0000001600fffffffb0000000c00430061006d00650072006101000001b8000001510000001600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002fd000002e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time: