diff --git a/launch/demo.launch b/launch/demo.launch
new file mode 100644
index 0000000000000000000000000000000000000000..9f94fe01df64611756774cf13a9948b918daf16b
--- /dev/null
+++ b/launch/demo.launch
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <arg name="name"               default="ana" />
+  <arg name="model"              default="$(arg name)"/>
+  <arg name="output"             default="screen" />
+  <arg name="launch_prefix"      default="" />
+
+  <include file="$(find iri_ana_bringup)/launch/bringup.launch">
+    <arg name="ns"                 value="$(arg name)"/>
+    <arg name="output"             value="$(arg output)"/>
+    <arg name="launch_prefix"      value="$(arg launch_prefix)"/>
+  </include>
+
+  <include file="$(find iri_ana_description)/launch/description.launch">
+    <arg name="ns"            value="$(arg name)"/>
+    <arg name="model"         value="$(arg model)"/>
+    <arg name="static_wheels" value="true"/>
+  </include>
+
+ <include file="$(find iri_ana_rosnav)/launch/nav_map.launch">
+    <arg name="map_name"      value="master_big"/>
+    <arg name="rviz"          value="false"/>
+  </include>
+
+ <include file="$(find iri_ana_launch)/launch/ps3_teleop.launch">
+ </include>
+
+ <include file="$(find iri_ana_launch)/launch/people.launch">
+ </include>
+
+</launch>
diff --git a/launch/people.launch b/launch/people.launch
new file mode 100644
index 0000000000000000000000000000000000000000..ecb61a23d09343217ccc466035e6a691d2f8c286
--- /dev/null
+++ b/launch/people.launch
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <arg name="ns"                 default="ana" />
+  <arg name="output"             default="screen" />
+  <arg name="scan_topic"         default="/$(arg ns)/sensors/scan"/>
+  <arg name="map_topic"          default="/$(arg ns)/map"/>
+  <arg name="tracks_topic"       default="/$(arg ns)/mht/tracks"/>
+  <arg name="launch_prefix"      default="" />
+
+  <include file="$(find iri_people_tracking_mht)/launch/test_robot.launch">
+    <arg name="ns"            value="$(arg ns)" />
+    <arg name="output"        value="$(arg output)" />
+    <arg name="launch_prefix" value="$(arg launch_prefix)" />
+    <arg name="scan_topic"    value="$(arg scan_topic)"/>
+    <arg name="map_topic"     value="$(arg map_topic)"/>
+    <arg name="tracks_topic"  value="$(arg tracks_topic)"/>
+  </include>
+</launch>
diff --git a/launch/ps3_teleop.launch b/launch/ps3_teleop.launch
index b49e9004dd73097ab249c4507e65580966a32b16..e68aa103a32160106b8f2f8692551d5364f09535 100644
--- a/launch/ps3_teleop.launch
+++ b/launch/ps3_teleop.launch
@@ -3,19 +3,19 @@
 <launch>
 
   <arg name="ns"            default="ana" />
-  <arg name="path"          default="$(find iri_teleop_launch)"/>
-  <arg name="config_file"   default="$(arg path)/config/ps3_bt_teleop.yaml" />
+  <arg name="path"          default="$(find iri_ana_bringup)"/>
+  <arg name="config_file"   default="$(arg path)/config/ana_ps3_bt_teleop.yaml" />
   <arg name="twist_topic"   default="/$(arg ns)/teleop/cmd_vel"/>
   <arg name="joy_topic"     default="/$(arg ns)/joy"/>
   <arg name="run_joy"       default="true"/>
-    <arg name="device"          default="/dev/input/js0" />
-    <arg name="joy_config_file" default="$(arg path)/config/joy.yaml" />
+    <arg name="device"          default="/dev/input/js1" />
+    <arg name="joy_config_file" default="$(find iri_teleop_launch)/config/joy.yaml" />
   <arg name="output"        default="screen" />
   <arg name="launch_prefix" default="" />
 
   <include file="$(find iri_teleop_launch)/launch/joystick_teleop.launch">
     <arg name="ns"               value="$(arg ns)"/>
-    <arg name="path"             value="$(arg path)"/>
+    <arg name="path"             value="$(find iri_teleop_launch)"/>
     <arg name="config_file"      value="$(arg config_file)"/>
     <arg name="twist_topic"      value="$(arg twist_topic)"/>
     <arg name="joy_topic"        value="$(arg joy_topic)"/>
diff --git a/rviz/ana.rviz b/rviz/ana.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..2895ab5526167b55b195f470d2f8ef41095d0aaa
--- /dev/null
+++ b/rviz/ana.rviz
@@ -0,0 +1,1022 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /TF1/Frames1
+        - /Sensors1
+        - /3dnav_detection1/camera1
+        - /People_detection1
+      Splitter Ratio: 0.504000008
+    Tree Height: 787
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+  - Class: rviz_plugin_tutorials/Teleop
+    Name: Teleop
+    Topic: ""
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 1000
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        ana_box:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ana_realsense_support:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_axle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_hub:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_axle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_hub:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_sonar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_bno055:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_bno055_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_left_axle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_hub:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_right_axle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_right_hub:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_sonar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        top_plate:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        velodyne:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        velodyne_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: ana/robot_description
+      TF Prefix: ana
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        ana/ana_box:
+          Value: true
+        ana/ana_realsense_support:
+          Value: true
+        ana/base_footprint:
+          Value: true
+        ana/base_footprint_robot:
+          Value: true
+        ana/base_link:
+          Value: true
+        ana/camera_aligned_depth_to_color_frame:
+          Value: true
+        ana/camera_aligned_depth_to_infra1_frame:
+          Value: true
+        ana/camera_bottom_screw_frame:
+          Value: true
+        ana/camera_color_frame:
+          Value: true
+        ana/camera_color_optical_frame:
+          Value: true
+        ana/camera_depth_frame:
+          Value: true
+        ana/camera_depth_optical_frame:
+          Value: true
+        ana/camera_infra1_frame:
+          Value: true
+        ana/camera_infra1_optical_frame:
+          Value: true
+        ana/camera_infra2_frame:
+          Value: true
+        ana/camera_infra2_optical_frame:
+          Value: true
+        ana/camera_link:
+          Value: true
+        ana/front_left_axle:
+          Value: true
+        ana/front_left_hub:
+          Value: true
+        ana/front_left_wheel:
+          Value: true
+        ana/front_right_axle:
+          Value: true
+        ana/front_right_hub:
+          Value: true
+        ana/front_right_wheel:
+          Value: true
+        ana/front_sonar:
+          Value: true
+        ana/imu_bno055:
+          Value: true
+        ana/imu_bno055_base:
+          Value: true
+        ana/odom:
+          Value: true
+        ana/rear_left_axle:
+          Value: true
+        ana/rear_left_hub:
+          Value: true
+        ana/rear_left_wheel:
+          Value: true
+        ana/rear_right_axle:
+          Value: true
+        ana/rear_right_hub:
+          Value: true
+        ana/rear_right_wheel:
+          Value: true
+        ana/rear_sonar:
+          Value: true
+        ana/top_plate:
+          Value: true
+        ana/velodyne:
+          Value: true
+        ana/velodyne_base:
+          Value: true
+        camera_accel_frame:
+          Value: true
+        camera_accel_optical_frame:
+          Value: true
+        camera_gyro_frame:
+          Value: true
+        camera_gyro_optical_frame:
+          Value: true
+        map:
+          Value: true
+      Marker Scale: 0.100000001
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          ana/odom:
+            ana/base_footprint:
+              ana/base_link:
+                ana/front_left_axle:
+                  ana/front_left_hub:
+                    ana/front_left_wheel:
+                      {}
+                ana/front_right_axle:
+                  ana/front_right_hub:
+                    ana/front_right_wheel:
+                      {}
+                ana/front_sonar:
+                  {}
+                ana/rear_left_axle:
+                  ana/rear_left_hub:
+                    ana/rear_left_wheel:
+                      {}
+                ana/rear_right_axle:
+                  ana/rear_right_hub:
+                    ana/rear_right_wheel:
+                      {}
+                ana/rear_sonar:
+                  {}
+                ana/top_plate:
+                  ana/ana_box:
+                    {}
+                  ana/ana_realsense_support:
+                    ana/camera_bottom_screw_frame:
+                      ana/camera_link:
+                        ana/camera_aligned_depth_to_color_frame:
+                          ana/camera_color_optical_frame:
+                            {}
+                        ana/camera_aligned_depth_to_infra1_frame:
+                          ana/camera_infra1_optical_frame:
+                            {}
+                        ana/camera_color_frame:
+                          {}
+                        ana/camera_depth_frame:
+                          ana/camera_depth_optical_frame:
+                            {}
+                        ana/camera_infra1_frame:
+                          {}
+                        ana/camera_infra2_frame:
+                          ana/camera_infra2_optical_frame:
+                            {}
+                        camera_accel_frame:
+                          camera_accel_optical_frame:
+                            {}
+                        camera_gyro_frame:
+                          camera_gyro_optical_frame:
+                            {}
+                  ana/velodyne_base:
+                    ana/velodyne:
+                      {}
+              ana/imu_bno055_base:
+                ana/imu_bno055:
+                  {}
+            ana/base_footprint_robot:
+              {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Angle Tolerance: 0.100000001
+          Class: rviz/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.300000012
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: false
+          Enabled: true
+          Keep: 100
+          Name: Odom
+          Position Tolerance: 0.100000001
+          Shape:
+            Alpha: 0.75
+            Axes Length: 1
+            Axes Radius: 0.100000001
+            Color: 255; 25; 0
+            Head Length: 0.300000012
+            Head Radius: 0.100000001
+            Shaft Length: 1
+            Shaft Radius: 0.0500000007
+            Value: Arrow
+          Topic: /ana/odom
+          Unreliable: false
+          Value: true
+        - Alpha: 0.75
+          Class: rviz_plugin_tutorials/Imu
+          Color: 204; 51; 204
+          Enabled: false
+          History Length: 1
+          Name: Imu
+          Topic: /ana/sensors/imu
+          Unreliable: false
+          Value: false
+        - Class: rviz/Image
+          Enabled: false
+          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: false
+        - Alpha: 0.5
+          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: false
+          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: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 3.40824103
+            Min Value: -0.157667398
+            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: 0
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: Velodyne
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.00999999978
+          Style: Points
+          Topic: /ana/sensors/velodyne_points
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          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/LaserScan
+          Color: 255; 0; 0
+          Color Transformer: FlatColor
+          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: LaserScan
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.00999999978
+          Style: Points
+          Topic: /ana/sensors/scan
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Sensors
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.699999988
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: true
+          Enabled: true
+          Name: Map
+          Topic: /ana/map
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Class: rviz/Polygon
+          Color: 25; 255; 0
+          Enabled: true
+          Name: footprint
+          Topic: /ana/move_base/global_costmap/footprint
+          Unreliable: false
+          Value: true
+        - Alpha: 0.5
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: true
+          Enabled: false
+          Name: local_costmap
+          Topic: /ana/move_base/local_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: false
+        - Alpha: 0.699999988
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: true
+          Enabled: false
+          Name: global_costmap
+          Topic: /ana/move_base/global_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: false
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 85; 255; 0
+          Enabled: true
+          Head Diameter: 0.300000012
+          Head Length: 0.200000003
+          Length: 0.300000012
+          Line Style: Lines
+          Line Width: 0.0299999993
+          Name: GlobalPlanner_global_plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.0299999993
+          Shaft Diameter: 0.100000001
+          Shaft Length: 0.100000001
+          Topic: /ana/move_base/GlobalPlanner/plan
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 0; 0; 255
+          Enabled: true
+          Head Diameter: 0.300000012
+          Head Length: 0.200000003
+          Length: 0.300000012
+          Line Style: Lines
+          Line Width: 0.0299999993
+          Name: DWA_global_plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.0299999993
+          Shaft Diameter: 0.100000001
+          Shaft Length: 0.100000001
+          Topic: /ana/move_base/DWAPlannerROS/global_plan
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 255; 0; 0
+          Enabled: true
+          Head Diameter: 0.300000012
+          Head Length: 0.200000003
+          Length: 0.300000012
+          Line Style: Lines
+          Line Width: 0.0299999993
+          Name: DWA_local_plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.0299999993
+          Shaft Diameter: 0.100000001
+          Shaft Length: 0.100000001
+          Topic: /ana/move_base/DWAPlannerROS/local_plan
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Axes Length: 1
+          Axes Radius: 0.100000001
+          Class: rviz/Pose
+          Color: 255; 255; 0
+          Enabled: true
+          Head Length: 0.300000012
+          Head Radius: 0.100000001
+          Name: CurrentGoal
+          Shaft Length: 1
+          Shaft Radius: 0.0500000007
+          Shape: Arrow
+          Topic: /ana/move_base/current_goal
+          Unreliable: false
+          Value: true
+      Enabled: true
+      Name: Nav
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Group
+          Displays:
+            - 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: false
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: obstacle_vision
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.00999999978
+              Style: Squares
+              Topic: /ana/obstacle_detection_normals/all_rg
+              Unreliable: false
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - Alpha: 1
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 10
+                Min Value: -10
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz/PointCloud
+              Color: 255; 255; 255
+              Color Transformer: RGB8
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: obstacle_normals
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 10
+              Size (m): 0.00999999978
+              Style: Points
+              Topic: /ana/obstacle_detection_normals/obstacles
+              Unreliable: false
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - 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: Intensity
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: far_hole_obstacle
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.00999999978
+              Style: Flat Squares
+              Topic: /ana/far_hole_detection/hole_obstacle
+              Unreliable: false
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - 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: Intensity
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: near_hole_obstacle
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.00999999978
+              Style: Flat Squares
+              Topic: /ana/near_hole_detection/hole_obstacle
+              Unreliable: false
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - 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: near_hole_vision
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.00999999978
+              Style: Flat Squares
+              Topic: /ana/near_hole_detection/hole_zone
+              Unreliable: false
+              Use Fixed Frame: true
+              Use rainbow: true
+              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
+              Enabled: true
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: far_hole_vision
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.00999999978
+              Style: Flat Squares
+              Topic: /ana/far_hole_detection/hole_zone
+              Unreliable: false
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: true
+          Enabled: true
+          Name: camera
+        - Class: rviz/Group
+          Displays:
+            - Alpha: 1
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 1.41061962e-08
+                Min Value: -1.07652482e-08
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz/LaserScan
+              Color: 255; 0; 0
+              Color Transformer: FlatColor
+              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: lidar_obstacle
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 10
+              Size (m): 0.00999999978
+              Style: Points
+              Topic: /ana/lidar_detector/scan_out
+              Unreliable: false
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: true
+            - Alpha: 1
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 0.997381926
+                Min Value: -0.279256642
+                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: 111
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: lidar_zone
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 2
+              Size (m): 0.00999999978
+              Style: Points
+              Topic: /ana/lidar_filter/cloud_out
+              Unreliable: false
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: true
+            - Class: rviz/Image
+              Enabled: false
+              Image Topic: /ana/lidar_detector/obstacles_img/image_raw
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: LidarDetectorImage
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: raw
+              Unreliable: false
+              Value: false
+          Enabled: true
+          Name: lidar
+      Enabled: true
+      Name: 3dnav_detection
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/MarkerArray
+          Enabled: false
+          Marker Topic: /ana/lpd/markers
+          Name: lpd
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: false
+        - Class: rviz/MarkerArray
+          Enabled: false
+          Marker Topic: /ana/lpmf/markers
+          Name: lpmf
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: false
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /ana/mht/markers
+          Name: mht
+          Namespaces:
+            cov_track: true
+            id: true
+            vel: true
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: People_detection
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /ana/initialpose
+    - Class: rviz/SetGoal
+      Topic: /ana/move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.785398185
+      Target Frame: ana/base_footprint
+      Value: Orbit (rviz)
+      Yaw: 0.785398185
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1000
+  Hide Left Dock: false
+  Hide Right Dock: false
+  LidarDetectorImage:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003a2fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003a2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f0072000000020f0000011b0000001600fffffffb0000000c00540065006c0065006f00700000000315000000b30000004900fffffffb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002f9000001090000000000000000000000010000010f00000302fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000302000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000007efc0100000003fb00000024004c0069006400610072004400650074006500630074006f00720049006d0061006700650000000000000004bf000000bb00fffffffb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000034f000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Teleop:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1215
+  X: 65
+  Y: 24
+  realsense_color:
+    collapsed: false