add mapas destinaciones brl y fm y launchs de tracker y detector a parte de los del companion

ea5eb0c7 · Ely Repiso Polo · 662e04ad · ea5eb0c7 · ea5eb0c7 · ea5eb0c7
Commit ea5eb0c7 authored 4 years ago by Ely Repiso Polo
--- a/launch/iri_mht_and_ASPSI_test_bags_laser_Terrinet.launch
+++ b/launch/iri_mht_and_ASPSI_test_bags_laser_Terrinet.launch
+<!-- -->
+<launch>
+
+  <param name="use_sim_time" value="true"/>
+
+  <node pkg="rosbag"
+        type="play"
+        name="rosbag"
+        args="--pause -l -q --clock  -r 1 $(find iri_people_tracking_mht)/bags_terrinet_de_Luca/DIAG-corr-pt_1.bag"
+        > 
+        
+     <!-- -s (segundos inicio) pause : rosbags starts in pause-->   
+     
+	<!-- rosbags to use: -->
+  <!-- DIAG-corr-static-p1_01.bag
+			 DIAG-corr-static-p1_02.bag	
+			DIAG-room-static-p1_01.bag 
+			DIAG-room-static-p1_02.bag 
+			DIAG-corr-moving-p1_01.bag
+			DIAG-corr-moving-p1_02.bag
+
+rosbags with tf:
+Home-moving-tf_01.bag
+Home-moving-tf_02.bag
+Home-moving-tf_03.bag
+
+Readme from Luca:
+
+Bags with people moving in the DIAG corridor
+
+DIAG-corr-pt_1.bag: /odom /scam /cmd_vel /tf
+DIAG-corr-pt_2.bag /odom /scam /cmd_vel /tf
+
+Maps (they are actually the same, 1st one seems good)
+
+DIAG-corr-map_1.yaml / DIAG-corr-map_1.pgm
+DIAG-corr-map_2.yaml / DIAG-corr-map_2.pgm
+
+The bags above starts from the origin of the map. So if you want to run a localizer, just start it with above map and use default initial pose (0,0,0).
+
+Bags with all topics and tf including localization
+
+DIAG-corr-pt_1_all.bag
+DIAG-corr-pt_2_all.bag
+
+
+
+-->
+		
+  </node>
+   
+     
+  
+  <!-- People Tracking MHT (remap) -->
+    <!-- published topics:  /$(env ROBOT)/mht/tracks -->
+    <!-- subscribed topics: /$(env ROBOT)/detections -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+  <node pkg ="iri_people_tracking_mht"
+        type="iri_people_tracking_mht"
+        name="mht_2"      
+        output="screen"> 
+    <remap from="/mht_2/detections"
+              to="/lpd_2/peopleMapFiltered" />         
+    <remap from="/mht_2/odom"
+              to="/odom" /> <!-- /topic-odometry-of-the-robot -->
+    <remap from="/mht_2/odom_for_medium_velocity"
+              to="/odom" /> <!-- /topic-odometry-of-the-robot -->
+              
+ <!-- People Tracking MHT parameters for outdoor environments with few false alarms-->
+  	<param name="~/frame_id"  type="string" value="/map" /> <!-- tf of the map for the tracker -->
+    <param name="local_tracker"            type="bool"   value="false"/>
+    <param name="~/association_threshold"  type="double" value="1.0" />
+    <param name="~/confirmation_threshold" type="double" value="0.9" />
+    <param name="~/deletion_threshold"     type="double" value="0.4" />
+    <param name="~/laser_probability"      type="double" value="0.9" />
+    <param name="~/laser_false_alarm"      type="double" value="0.1" />
+    <param name="~/laser_new_track"        type="double" value="0.11" />
+    <param name="~/laser_no_detection"     type="double" value="0.97" />
+    <param name="~/laser_no_confirmed"     type="double" value="0.02" />
+		<param name="~/laser_no_confirmed_iteration" type="double" value="0.01" />
+ 		<param name="~/augment_covariance_track" type="double" value="0.5" />
+    <param name="~/mht_velocity_margin" type="double" value="1.0" />
+    <param name="~/covariance_no_track" type="double" value="5.0" />
+  </node>
+
+  <!-- Laser People Detector (remap) -->
+    <!-- published topics:  /$(env ROBOT)/people -->
+    <!-- subscribed topics: /$(env ROBOT)/sensors/front_laser_scan -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+  <node pkg ="iri_laser_people_detection"
+        type="iri_laser_people_detection"
+        name="lpd_2"
+        output="screen"
+       	>
+
+      <param name="~/rangeThreshold"     type="double" value="0.0" />
+      <param name="~/detectionThreshold" type="double" value="-0.07" />
+			<param name="~/personRadius"        type="double" value="0.7" />
+      <!-- load different trained detectors (to switch at rangeThreshold) -->
+      <param name="~/boostFilePath"      type="string" value="/usr/local/include/iridrivers/boostData/boostFile.txt"  />
+      <param name="~/boostFilePath2"     type="string" value="/usr/local/include/iridrivers/boostData/boostFile2.txt" />
+      <!-- transform detections to base_link frame -->
+      <param name="~/selectPosesFrame"   type="bool"   value="False" />
+      <param name="~/posesFrame"         type="string" value="/scan" />  <!-- frame where you want the poses -->
+      <param name="~/selectScanFrame"    type="bool"   value="False" />
+      <param name="~/scanFrame"          type="string" value="/scan" /> <!-- frame of the scan -->
+      <!-- visualization -->
+      <param name="~/markerWidth"        type="double" value="0.7" />
+      <param name="~/markerHeight"       type="double" value="0.4" />
+      <param name="~/markerR"            type="double" value="0.0" />
+      <param name="~/markerG"            type="double" value="0.0" />
+      <param name="~/markerB"            type="double" value="0.5" />
+      <param name="~/markerA"            type="double" value="0.5" />
+      <!-- filter detections by range/x/y -->
+      <param name="~/filterPosesMode"    type="bool"   value="False" />
+      <param name="~/filterR"            type="double" value="0.0" />
+      <param name="~/filterX"            type="double" value="0.0" />
+      <param name="~/filterY"            type="double" value="0.0" />
+
+    <remap from="/lpd_2/scan"
+             to="/scan" /> <!-- topic: scan del robot -->
+    <remap from="/lpd_2/people"
+             to="/lpd_2/people" />
+  </node>
+  
+   
+  
+   <!-- Laser People map Filter -->
+    <!-- published topics:  /$(env ROBOT)/peopleFiltered -->
+    <!-- subscribed topics: /$(env ROBOT)/people -->
+    <!--                    /$(env ROBOT)map -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+     <node pkg ="iri_laser_people_map_filter"
+          type="iri_laser_people_map_filter"
+          name="laser_people_map_filter">
+
+ 			<param name="~/filter"   type="bool" value="True" /> <!-- true to enable filtering when we have a map -->
+      <param name="~/neighborRadius"   type="double" value="0.3" />
+      <param name="~/markerWidth"      type="double" value="0.7" />
+      <param name="~/markerHeight"     type="double" value="1.5" />
+      <param name="~/markerR"          type="double" value="1.0" />
+      <param name="~/markerG"          type="double" value="1.0" />
+      <param name="~/markerB"          type="double" value="0.0" />
+      <param name="~/markerA"          type="double" value="0.9" />
+      <param name="~/markerLifeTime"   type="double" value="0.1" />
+      
+
+      <remap from="/laser_people_map_filter/people"
+               to="/lpd_2/people" />
+      <remap from="/laser_people_map_filter/peopleFiltered"
+               to="/lpd_2/peopleMapFiltered" />
+      <remap from="/laser_people_map_filter/map"
+               to="/map" /> <!-- topic of the map -->
+                          
+    </node> 
+
+
+<!--map_server-->
+ <include file="$(find tibi_dabo_rosnav)/launch/tibi_dabo_amcl_terrinet.launch">
+    <arg name="x" value="0.0"/>
+    <arg name="y" value="0.0"/>
+    <arg name="a" value="0.0"/>
+  </include> 
+   <include file="$(find tibi_dabo_rosnav)/launch/tibi_dabo_map_server_terrinet.launch">
+    <arg name="map" value="DIAG-corr-map_1"/> 
+  </include>
+
+  <!--/nav-Companion-node:-->
+  <!--output="screen"-->
+    <node pkg ="move_base"
+          type="move_base"
+          name="move_base"
+	  output="screen">
+      <remap from="/cmd_vel"
+              to="/cmd_vel" />
+      <remap from="/odom"
+              to="/odom" />
+      <remap from="/front_scan"
+              to="/scan" />
+      <remap from="/rear_scan"
+              to="/scan" />
+      <remap from="/tracks"
+               to="/mht_2/tracks" />
+		<remap from="/map"
+               to="/map" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/move_base_params.yaml"      command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/costmap_common_params_terrinet.yaml" command="load" ns="global_costmap" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/local_costmap_params_terrinet.yaml"  command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/global_costmap_params_terrinet.yaml" command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/akp_local_planner_params.yaml" command="load" />
+     
+ <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/terrinet_corridor_dest.txt" />
+<param name="AkpLocalPlanner/results_filename_Zanlungo" value="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/local_lib_ATR_7_junio/1_data_results_Vform/results_person_companion_Vform.txt" /> 
+      <param name="AkpLocalPlanner/robot" value="$(optenv ROBOT tibi)"/>
+      <param name="AkpLocalPlanner/person_goal_id"  value="200"/> 
+      <param name="AkpLocalPlanner/speed_k"  value="1"/>  
+	 
+			 <param name="AkpLocalPlanner/radi_to_detect_other_people_no_companions"  value="4.0"/> <!-- radi to detect people like obstacles-->
+ 			<param name="AkpLocalPlanner/detection_laser_obstacle_distances"  value="2.0"/> <!-- radi to detect static obstacles-->
+<param name="AkpLocalPlanner/in_change_dynamically_final_dest"  value="False"/> <!-- To true if you use the  distance_to_dynamic_goal_Vform-->
+			 <param name="AkpLocalPlanner/distance_to_dynamic_goal_Vform"  value="0.0"/> <!-- dynamic goal from static goals of the environment to obtain a good sid-by-side formation (In narrow corridor 0.0, to do not collide with the walls at the sides of the two final static obstacles of the environment.-->
+     <param name="AkpLocalPlanner/person_radi_amp"  value="0.2"/> 
+     <param name="AkpLocalPlanner/change_treshold_distance_between_steps"  value="0.3"/> 
+ <param name="AkpLocalPlanner/proximity_distance_between_robot_and_person" value="0.7"/> <!-- accompaniment distance-->
+	<!-- for SIM-->
+ 	<param name="AkpLocalPlanner/change_sim"  value="False"/> 
+ 	<param name="AkpLocalPlanner/sim_target_per"  value="False"/> 
+	<param name="AkpLocalPlanner/frame_map"  type="string" value="/map" /> 
+	<param name="AkpLocalPlanner/frame_robot_footprint"  type="string" value="/base_frame" /> 
+    </node>
+
+ <!--to do not collide in any case-->
+  <!-- <node pkg="iri_safe_cmd"
+          type="iri_safe_cmd"
+          name="iri_safe_cmd_nav"
+          >
+      <param name="unsafe"          type="bool"   value="false"/>
+      <param name="min_dist"        type="double" value="0.5"/>
+      <param name="collision_time"  type="double" value="1.5"/>
+      <param name="limit_vel_front" type="double" value="1.0"/>
+      <param name="limit_vel_rear"  type="double" value="-1.0"/>
+      <param name="front_laser_watchdog_time" type="double" value="1.0"/>
+      <param name="rear_laser_watchdog_time"  type="double" value="1.0"/>-->
+      <!--<param name="base_frame"  type="str_t" value="tibi/base_link"/>-->
+     <!-- <remap from="~cmd_vel_safe" to="/$(env ROBOT)/segway/cmd_vel"/>
+      <remap from="~cmd_vel"      to="/$(env ROBOT)/navigation/cmd_vel_unsafe"/>
+      <remap from="~rear_laser"   to="/scan"/>
+      <remap from="~front_laser"  to="/scan"/>
+      </node> -->
+
+ <!--hri-->
+ <!-- <include file="$(find tibi_dabo_hri_node)/launch/tibi_dabo_hri_node.launch"/>
+  <include file="$(find tibi_dabo_hri_database)/launch/tibi_dabo_hri_database_ely.launch">
+     <arg name="demo" default="Ely_side_Vform"/>
+  </include>
+  <include file="$(find tibi_dabo_hri_teleop_companion)/launch/tibi_dabo_hri_teleop_ely.launch"/> -->
+
+  <!-- node states companion -->
+  <!-- <include file="$(find iri_state_machine_companion)/launch/tibi_dabo_state_machine_companion.launch"/>-->
+
+
+
+  <node name="rviz"
+        pkg ="rviz"
+        type="rviz"
+        args="-d $(find iri_people_tracking_mht)/config/test_terrinet.rviz" />/>
+<!-- tibi_local.vcg-->
+  <node name="reconfigure_gui"
+        pkg ="rqt_reconfigure"
+        type="rqt_reconfigure" />
+
+
+</launch>
--- a/launch/sim_gazebo/tibi_dabo_akp_local_ASPSI.launch
+++ b/launch/sim_gazebo/tibi_dabo_akp_local_ASPSI.launch
+<!-- -->
+<launch>
+
+  <arg name="map" default="fme_open2" />
+
+  <rosparam command="delete" ns="/$(optenv ROBOT tibi)/move_base" />
+
+  <group ns="$(optenv ROBOT tibi)"> 
+
+    <!--launch-prefix="xterm -e ddd -args" -->
+  <!--output="screen"-->
+    <node pkg ="move_base"
+          type="move_base"
+          name="move_base"
+	  output="screen">
+      <remap from="/$(optenv ROBOT tibi)/cmd_vel"
+              to="/$(optenv ROBOT tibi)/navigation/cmd_vel_unsafe" />
+      <remap from="/$(optenv ROBOT tibi)/odom"
+              to="/$(optenv ROBOT tibi)/segway/odom" />
+      <remap from="/$(optenv ROBOT tibi)/front_scan"
+              to="/$(optenv ROBOT tibi)/sensors/front_laser_scan" />
+      <remap from="/$(optenv ROBOT tibi)/rear_scan"
+              to="/$(optenv ROBOT tibi)/sensors/rear_laser_scan" />
+      <remap from="/$(optenv ROBOT tibi)/tracks"
+               to="/$(optenv ROBOT tibi)/mht/tracks" />
+      <remap from="/$(optenv ROBOT tibi)/cmd_vel_stop" 
+               to="/turtlebot_teleop_keyboard/cmd_vel2"/> 
+
+      <remap from="/$(optenv ROBOT tibi)/tibi_pose_for_sim" 
+               to="/$(optenv ROBOT tibi)/initialpose"/> 
+
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/move_base_params.yaml"      command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/costmap_common_params.yaml" command="load" ns="global_costmap" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/costmap_common_params.yaml" command="load" ns="local_costmap" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/local_costmap_params.yaml"  command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/global_costmap_params.yaml" command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/$(optenv ROBOT tibi)/akp_local_planner_params.yaml" command="load" />
+      <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/master_big_destinations_Gazebo_sim2.txt" />
+      <param name="AkpLocalPlanner/robot" value="$(optenv ROBOT tibi)"/>
+      <param name="AkpLocalPlanner/speed_k"  value="1"/>  <!--MUY importante: en sim, speed_k=1.0; en robot real,  speed_k=1.5; is due to our robot-velocity controller that divides the velocity by 2.-->        
+	<!-- for SIM-->
+ 	<param name="AkpLocalPlanner/change_sim"  value="False"/> 
+ 	<param name="AkpLocalPlanner/sim_target_per"  value="False"/> 
+    </node>
+
+
+   <node pkg="iri_safe_cmd"
+          type="iri_safe_cmd"
+          name="iri_safe_cmd_nav"
+          >
+      <param name="unsafe"          type="bool"   value="false"/>
+      <param name="min_dist"        type="double" value="0.5"/>
+      <param name="collision_time"  type="double" value="1.5"/>
+      <param name="limit_vel_front" type="double" value="1.0"/>
+      <param name="limit_vel_rear"  type="double" value="-1.0"/>
+      <param name="front_laser_watchdog_time" type="double" value="1.0"/>
+      <param name="rear_laser_watchdog_time"  type="double" value="1.0"/>
+      <param name="base_frame"  type="string" value="tibi/base_link"/>
+      <remap from="~cmd_vel_safe" to="/$(env ROBOT)/segway/cmd_vel"/>
+      <remap from="~cmd_vel"      to="/$(env ROBOT)/navigation/cmd_vel_unsafe"/>
+      <remap from="~rear_laser"   to="/$(env ROBOT)/sensors/rear_laser_scan"/>
+      <remap from="~front_laser"  to="/$(env ROBOT)/sensors/front_laser_scan"/>
+      </node> 
+
+  
+
+  </group>
+
+</launch>
--- a/launch/tracker_nav_and_detection_for_ASPSI.launch
+++ b/launch/tracker_nav_and_detection_for_ASPSI.launch
+<!-- -->
+<launch>
+
+  <arg name="rviz"                    default="true"/>
+  <arg name="rviz_file"               default="$(find iri_ana_rosnav)/rviz/ana.rviz"/>
+  <arg name="move_base_params"        default="move_base_params.yaml"/>
+  <arg name="costmap_common_params"   default="common_params.yaml"/>
+  <arg name="costmap_local_params"    default="local_params.yaml"/>
+  <arg name="costmap_global_params"   default="global_params.yaml"/>
+  <arg name="map_name"                default="fme"/>
+  <arg name="initial_x"               default="-1.5"/>
+  <arg name="initial_y"               default="6.0"/>
+  <arg name="initial_yaw"             default="-1.57"/>
+  <arg name="local_planner"           default="dwa"/>
+  <arg name="global_planner"          default="global_planner"/>
+  <arg name="output"                  default="screen" />
+  <arg name="launch_prefix"           default="" />
+
+  <include file="$(find iri_rosnav)/launch/nav.launch">
+    <arg name="ns"                    value="ana"/>
+    <arg name="path"                  value="$(find iri_ana_rosnav)/params"/>
+    <arg name="move_base_params"      value="$(arg move_base_params)"/>
+    <arg name="costmap_common_params" value="$(arg costmap_common_params)"/>
+    <arg name="costmap_local_params"  value="$(arg costmap_local_params)"/>
+    <arg name="costmap_global_params" value="$(arg costmap_global_params)"/>
+    <arg name="map_frame_id"          value="map"/>
+    <arg name="odom_frame_id"         value="ana/odom"/>
+    <arg name="base_frame_id"         value="ana/base_footprint"/>
+    <arg name="map_topic"             value="/ana/map"/>
+    <arg name="map_service"           value="/ana/static_map"/>
+    <arg name="odom_topic"            value="/ana/odom"/>
+    <arg name="cmd_vel_topic"         value="/ana/navigation/cmd_vel"/>
+    <arg name="scan_topic"            value="/ana/sensors/scan"/>
+    <arg name="use_map"               value="true"/>
+    <arg name="use_map_server"        value="true"/> 
+    <arg name="map_name"              value="$(arg map_name)"/>
+    <arg name="use_amcl"              value="true"/>
+    <arg name="amcl_config"           value="$(find iri_ana_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml"/>
+    <arg name="initial_x"             value="$(arg initial_x)"/>
+    <arg name="initial_y"             value="$(arg initial_y)"/>
+    <arg name="initial_yaw"           value="$(arg initial_yaw)"/>
+    <arg name="use_fake_loc"          value="false"/>
+    <arg name="use_gmapping"          value="false"/>
+    <arg name="local_planner"         value="$(arg local_planner)"/>
+    <arg name="global_planner"        value="$(arg global_planner)"/>
+    <arg name="output"                value="$(arg output)" />
+    <arg name="launch_prefix"         value="$(arg launch_prefix)" />
+  </include>
+  
+  <include file="$(find iri_rosnav)/launch/include/pointcloud_to_laserscan.launch">
+    <arg name="ns"             value="ana"/>
+    <arg name="node_name"      value="pcl_to_laserscan"/>
+    <arg name="scan_topic"     value="/ana/sensors/scan"/>
+    <arg name="cloud_topic"    value="/ana/sensors/velodyne_points"/>
+    <arg name="config_file"    value="$(find iri_ana_rosnav)/params/pointcloud_to_laserscan.yaml" />
+    <arg name="output"         value="$(arg output)" />
+    <arg name="launch_prefix"  value="$(arg launch_prefix)" />
+  </include>
+
+<group ns="$(optenv ROBOT ana)"> 
+ <!-- People Tracking MHT (remap) -->
+    <!-- published topics:  /$(env ROBOT)/mht/tracks -->
+    <!-- subscribed topics: /$(env ROBOT)/detections -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+  <node pkg ="iri_people_tracking_mht"
+        type="iri_people_tracking_mht"
+        name="mht"      
+        output="screen"> 
+    <remap from="/ana/mht/detections"
+              to="/ana/lpd/peopleMapFiltered" />         
+    <remap from="/ana/mht/odom"
+              to="/ana/odom" /> <!-- /topic-odometry-of-the-robot -->
+    <remap from="/ana/mht/odom_for_medium_velocity"
+              to="/ana/odom" /> <!-- /topic-odometry-of-the-robot -->
+              
+ <!-- People Tracking MHT parameters for outdoor environments with few false alarms-->
+  	<param name="~/frame_id"  type="string" value="map" /> <!-- tf of the map for the tracker -->
+    <param name="local_tracker"            type="bool"   value="false"/>
+    <param name="~/association_threshold"  type="double" value="1.0" />
+    <param name="~/confirmation_threshold" type="double" value="0.9" />
+    <param name="~/deletion_threshold"     type="double" value="0.4" />
+    <param name="~/laser_probability"      type="double" value="0.9" />
+    <param name="~/laser_false_alarm"      type="double" value="0.1" />
+    <param name="~/laser_new_track"        type="double" value="0.11" />
+    <param name="~/laser_no_detection"     type="double" value="0.97" />
+    <param name="~/laser_no_confirmed"     type="double" value="0.02" />
+   <param name="~/laser_no_confirmed_iteration" type="double" value="0.01" />
+   <param name="~/augment_covariance_track" type="double" value="0.5" />
+    <param name="~/mht_velocity_margin" type="double" value="1.0" />
+    <param name="~/covariance_no_track" type="double" value="5.0" />
+  </node>
+
+  <!-- Laser People Detector (remap) -->
+    <!-- published topics:  /$(env ROBOT)/people -->
+    <!-- subscribed topics: /$(env ROBOT)/sensors/front_laser_scan -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+  <node pkg ="iri_laser_people_detection"
+        type="iri_laser_people_detection"
+        name="lpd"
+        output="screen"
+       	>
+
+      <param name="~/rangeThreshold"     type="double" value="0.0" />
+      <param name="~/detectionThreshold" type="double" value="-0.07" />
+			<param name="~/personRadius"        type="double" value="0.7" />
+      <!-- load different trained detectors (to switch at rangeThreshold) -->
+      <param name="~/boostFilePath"      type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt"  />
+      <param name="~/boostFilePath2"     type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" />
+      <!-- transform detections to base_link frame -->
+      <param name="~/selectPosesFrame"   type="bool"   value="False" />
+      <param name="~/posesFrame"         type="string" value="/scan" />  <!-- frame where you want the poses -->
+      <param name="~/selectScanFrame"    type="bool"   value="False" />
+      <param name="~/scanFrame"          type="string" value="/scan" /> <!-- frame of the scan -->
+      <!-- visualization -->
+      <param name="~/markerWidth"        type="double" value="0.7" />
+      <param name="~/markerHeight"       type="double" value="0.4" />
+      <param name="~/markerR"            type="double" value="0.0" />
+      <param name="~/markerG"            type="double" value="0.0" />
+      <param name="~/markerB"            type="double" value="0.5" />
+      <param name="~/markerA"            type="double" value="0.5" />
+      <!-- filter detections by range/x/y -->
+      <param name="~/filterPosesMode"    type="bool"   value="False" />
+      <param name="~/filterR"            type="double" value="0.0" />
+      <param name="~/filterX"            type="double" value="0.0" />
+      <param name="~/filterY"            type="double" value="0.0" />
+
+    <remap from="/ana/lpd/scan"
+             to="/ana/sensors/scan" /> <!-- topic: scan del robot -->
+    <remap from="/ana/lpd/people"
+             to="/ana/lpd/people" />
+  </node>
+  
+   
+  
+   <!-- Laser People map Filter -->
+    <!-- published topics:  /$(env ROBOT)/peopleFiltered -->
+    <!-- subscribed topics: /$(env ROBOT)/people -->
+    <!--                    /$(env ROBOT)map -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+     <node pkg ="iri_laser_people_map_filter"
+          type="iri_laser_people_map_filter"
+          name="lpmf">
+
+ 	<param name="~/filter"   type="bool" value="True" /> <!-- true to enable filtering when we have a map -->
+      <param name="~/neighborRadius"   type="double" value="0.3" />
+      <param name="~/markerWidth"      type="double" value="0.7" />
+      <param name="~/markerHeight"     type="double" value="1.5" />
+      <param name="~/markerR"          type="double" value="1.0" />
+      <param name="~/markerG"          type="double" value="1.0" />
+      <param name="~/markerB"          type="double" value="0.0" />
+      <param name="~/markerA"          type="double" value="0.9" />
+      <param name="~/markerLifeTime"   type="double" value="0.1" />
+      
+
+      <remap from="/ana/lpmf/people"
+               to="/ana/lpd/people" />
+      <remap from="/ana/lpmf/people_out"
+               to="/ana/lpd/peopleMapFiltered" />
+      <remap from="/ana/lpmf/map"
+               to="/ana/map" /> <!-- topic of the map -->
+                          
+    </node> 
+ </group>
+
+   <!-- Ini Navigation Companion Nodes -->
+
+ 
+
+ <!--  <node name="rviz"
+        pkg="rviz"
+        type="rviz"
+        if="$(arg rviz)"
+        args="-d $(arg rviz_file)">
+  </node>-->
+
+<node name="rviz"
+        pkg="rviz"
+        type="rviz"
+        if="$(arg rviz)"
+         args="-d $(find iri_robot_aspsi)/rviz/ana.rviz">
+  </node>
+</launch>
--- a/launch/tracker_nav_and_detection_for_ASPSI_brl.launch
+++ b/launch/tracker_nav_and_detection_for_ASPSI_brl.launch
+<!-- -->
+<launch>
+
+  <arg name="rviz"                    default="true"/>
+  <arg name="rviz_file"               default="$(find iri_ana_rosnav)/rviz/ana.rviz"/>
+  <arg name="move_base_params"        default="move_base_params.yaml"/>
+  <arg name="costmap_common_params"   default="common_params.yaml"/>
+  <arg name="costmap_local_params"    default="local_params.yaml"/>
+  <arg name="costmap_global_params"   default="global_params.yaml"/>
+  <arg name="map_name"                default="master_big"/>
+  <arg name="initial_x"               default="20.5"/>
+  <arg name="initial_y"               default="39.0"/>
+  <arg name="initial_yaw"             default="0.0"/>
+  <arg name="local_planner"           default="dwa"/>
+  <arg name="global_planner"          default="global_planner"/>
+  <arg name="output"                  default="screen" />
+  <arg name="launch_prefix"           default="" />
+
+  <include file="$(find iri_rosnav)/launch/nav.launch">
+    <arg name="ns"                    value="ana"/>
+    <arg name="path"                  value="$(find iri_ana_rosnav)/params"/>
+    <arg name="move_base_params"      value="$(arg move_base_params)"/>
+    <arg name="costmap_common_params" value="$(arg costmap_common_params)"/>
+    <arg name="costmap_local_params"  value="$(arg costmap_local_params)"/>
+    <arg name="costmap_global_params" value="$(arg costmap_global_params)"/>
+    <arg name="map_frame_id"          value="map"/>
+    <arg name="odom_frame_id"         value="ana/odom"/>
+    <arg name="base_frame_id"         value="ana/base_footprint"/>
+    <arg name="map_topic"             value="/ana/map"/>
+    <arg name="map_service"           value="/ana/static_map"/>
+    <arg name="odom_topic"            value="/ana/odom"/>
+    <arg name="cmd_vel_topic"         value="/ana/navigation/cmd_vel"/>
+    <arg name="scan_topic"            value="/ana/sensors/scan"/>
+    <arg name="use_map"               value="true"/>
+    <arg name="use_map_server"        value="true"/> 
+    <arg name="map_name"              value="$(arg map_name)"/>
+    <arg name="use_amcl"              value="true"/>
+    <arg name="amcl_config"           value="$(find iri_ana_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml"/>
+    <arg name="initial_x"             value="$(arg initial_x)"/>
+    <arg name="initial_y"             value="$(arg initial_y)"/>
+    <arg name="initial_yaw"           value="$(arg initial_yaw)"/>
+    <arg name="use_fake_loc"          value="false"/>
+    <arg name="use_gmapping"          value="false"/>
+    <arg name="local_planner"         value="$(arg local_planner)"/>
+    <arg name="global_planner"        value="$(arg global_planner)"/>
+    <arg name="output"                value="$(arg output)" />
+    <arg name="launch_prefix"         value="$(arg launch_prefix)" />
+  </include>
+  
+  <include file="$(find iri_rosnav)/launch/include/pointcloud_to_laserscan.launch">
+    <arg name="ns"             value="ana"/>
+    <arg name="node_name"      value="pcl_to_laserscan"/>
+    <arg name="scan_topic"     value="/ana/sensors/scan"/>
+    <arg name="cloud_topic"    value="/ana/sensors/velodyne_points"/>
+    <arg name="config_file"    value="$(find iri_ana_rosnav)/params/pointcloud_to_laserscan.yaml" />
+    <arg name="output"         value="$(arg output)" />
+    <arg name="launch_prefix"  value="$(arg launch_prefix)" />
+  </include>
+
+<group ns="$(optenv ROBOT ana)"> 
+ <!-- People Tracking MHT (remap) -->
+    <!-- published topics:  /$(env ROBOT)/mht/tracks -->
+    <!-- subscribed topics: /$(env ROBOT)/detections -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+  <node pkg ="iri_people_tracking_mht"
+        type="iri_people_tracking_mht"
+        name="mht"      
+        output="screen"> 
+    <remap from="/ana/mht/detections"
+              to="/ana/lpd/peopleMapFiltered" />         
+    <remap from="/ana/mht/odom"
+              to="/ana/odom" /> <!-- /topic-odometry-of-the-robot -->
+    <remap from="/ana/mht/odom_for_medium_velocity"
+              to="/ana/odom" /> <!-- /topic-odometry-of-the-robot -->
+              
+ <!-- People Tracking MHT parameters for outdoor environments with few false alarms-->
+  	<param name="~/frame_id"  type="string" value="map" /> <!-- tf of the map for the tracker -->
+    <param name="local_tracker"            type="bool"   value="false"/>
+    <param name="~/association_threshold"  type="double" value="1.0" />
+    <param name="~/confirmation_threshold" type="double" value="0.9" />
+    <param name="~/deletion_threshold"     type="double" value="0.4" />
+    <param name="~/laser_probability"      type="double" value="0.9" />
+    <param name="~/laser_false_alarm"      type="double" value="0.1" />
+    <param name="~/laser_new_track"        type="double" value="0.11" />
+    <param name="~/laser_no_detection"     type="double" value="0.97" />
+    <param name="~/laser_no_confirmed"     type="double" value="0.02" />
+   <param name="~/laser_no_confirmed_iteration" type="double" value="0.01" />
+   <param name="~/augment_covariance_track" type="double" value="0.5" />
+    <param name="~/mht_velocity_margin" type="double" value="1.0" />
+    <param name="~/covariance_no_track" type="double" value="5.0" />
+  </node>
+
+  <!-- Laser People Detector (remap) -->
+    <!-- published topics:  /$(env ROBOT)/people -->
+    <!-- subscribed topics: /$(env ROBOT)/sensors/front_laser_scan -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+  <node pkg ="iri_laser_people_detection"
+        type="iri_laser_people_detection"
+        name="lpd"
+        output="screen"
+       	>
+
+      <param name="~/rangeThreshold"     type="double" value="0.0" />
+      <param name="~/detectionThreshold" type="double" value="-0.07" />
+			<param name="~/personRadius"        type="double" value="0.7" />
+      <!-- load different trained detectors (to switch at rangeThreshold) -->
+      <param name="~/boostFilePath"      type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt"  />
+      <param name="~/boostFilePath2"     type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" />
+      <!-- transform detections to base_link frame -->
+      <param name="~/selectPosesFrame"   type="bool"   value="False" />
+      <param name="~/posesFrame"         type="string" value="/scan" />  <!-- frame where you want the poses -->
+      <param name="~/selectScanFrame"    type="bool"   value="False" />
+      <param name="~/scanFrame"          type="string" value="/scan" /> <!-- frame of the scan -->
+      <!-- visualization -->
+      <param name="~/markerWidth"        type="double" value="0.7" />
+      <param name="~/markerHeight"       type="double" value="0.4" />
+      <param name="~/markerR"            type="double" value="0.0" />
+      <param name="~/markerG"            type="double" value="0.0" />
+      <param name="~/markerB"            type="double" value="0.5" />
+      <param name="~/markerA"            type="double" value="0.5" />
+      <!-- filter detections by range/x/y -->
+      <param name="~/filterPosesMode"    type="bool"   value="False" />
+      <param name="~/filterR"            type="double" value="0.0" />
+      <param name="~/filterX"            type="double" value="0.0" />
+      <param name="~/filterY"            type="double" value="0.0" />
+
+    <remap from="/ana/lpd/scan"
+             to="/ana/sensors/scan" /> <!-- topic: scan del robot -->
+    <remap from="/ana/lpd/people"
+             to="/ana/lpd/people" />
+  </node>
+  
+   
+  
+   <!-- Laser People map Filter -->
+    <!-- published topics:  /$(env ROBOT)/peopleFiltered -->
+    <!-- subscribed topics: /$(env ROBOT)/people -->
+    <!--                    /$(env ROBOT)map -->
+    <!-- service clients: -->
+    <!-- service servers: -->
+    <!-- action clients: -->
+    <!-- action servers: -->
+     <node pkg ="iri_laser_people_map_filter"
+          type="iri_laser_people_map_filter"
+          name="lpmf">
+
+ 			<param name="~/filter"   type="bool" value="True" /> <!-- true to enable filtering when we have a map -->
+      <param name="~/neighborRadius"   type="double" value="0.3" />
+      <param name="~/markerWidth"      type="double" value="0.7" />
+      <param name="~/markerHeight"     type="double" value="1.5" />
+      <param name="~/markerR"          type="double" value="1.0" />
+      <param name="~/markerG"          type="double" value="1.0" />
+      <param name="~/markerB"          type="double" value="0.0" />
+      <param name="~/markerA"          type="double" value="0.9" />
+      <param name="~/markerLifeTime"   type="double" value="0.1" />
+      
+
+      <remap from="/ana/lpmf/people"
+               to="/ana/lpd/people" />
+      <remap from="/ana/lpmf/people_out"
+               to="/ana/lpd/peopleMapFiltered" />
+      <remap from="/ana/lpmf/map"
+               to="/ana/map" /> <!-- topic of the map -->
+                          
+    </node> 
+ </group>
+
+   <!-- Ini Navigation Companion Nodes -->
+
+ 
+
+ <!--  <node name="rviz"
+        pkg="rviz"
+        type="rviz"
+        if="$(arg rviz)"
+        args="-d $(arg rviz_file)">
+  </node>-->
+
+<node name="rviz"
+        pkg="rviz"
+        type="rviz"
+        if="$(arg rviz)"
+         args="-d $(find iri_robot_aspsi)/rviz/ana.rviz">
+  </node>
+</launch>
--- a/maps/destinaciones_ana_FME_ubuntu18_ros_melodic.txt
+++ b/maps/destinaciones_ana_FME_ubuntu18_ros_melodic.txt
+2
+0, 0.5, 8.0, 0.25, 1, 1 
+1, 0.5, -6.0, 0.25, 1, 0
+
+
+
+
+
+
+
+
--- a/maps/master_big_destinations_Gazebo_sim_more_dest.txt
+++ b/maps/master_big_destinations_Gazebo_sim_more_dest.txt
+4
+0, 55.0, 37.5, 0.25, 3, 1, 2, 3
+1, 19.0, 37.5, 0.25, 3, 0, 2, 3
+2, 35.5, 13.5, 0.25, 3, 0, 1, 3
+3, 33.0, 50.5, 0.25, 3, 0, 1, 2
+
+
+
--- a/rviz/ana.rviz
+++ b/rviz/ana.rviz
--- a/rviz/dabo_companion_ok.rviz
+++ b/rviz/dabo_companion_ok.rviz
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /Sensors1
+        - /MarkerArray4
+      Splitter Ratio: 0.5936599373817444
+    Tree Height: 800
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+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.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        dabo/base_footprint:
+          Value: true
+        dabo/base_link:
+          Value: true
+        dabo/body:
+          Value: true
+        dabo/caster_support:
+          Value: true
+        dabo/front_caster_wheel:
+          Value: true
+        dabo/front_hokuyo_base:
+          Value: true
+        dabo/front_hokuyo_scan_frame:
+          Value: true
+        dabo/front_support:
+          Value: true
+        dabo/head:
+          Value: true
+        dabo/head_servo1:
+          Value: true
+        dabo/head_servo2:
+          Value: true
+        dabo/head_servo3:
+          Value: true
+        dabo/left_arm:
+          Value: true
+        dabo/left_arm_servo1:
+          Value: true
+        dabo/left_arm_servo2:
+          Value: true
+        dabo/left_wheel_link:
+          Value: true
+        dabo/odom:
+          Value: true
+        dabo/rear_caster_wheel:
+          Value: true
+        dabo/rear_hokuyo_base:
+          Value: true
+        dabo/rear_hokuyo_scan_frame:
+          Value: true
+        dabo/rear_support:
+          Value: true
+        dabo/right_arm:
+          Value: true
+        dabo/right_arm_servo1:
+          Value: true
+        dabo/right_arm_servo2:
+          Value: true
+        dabo/right_wheel_link:
+          Value: true
+        dabo/top_plate:
+          Value: true
+        map:
+          Value: true
+        world:
+          Value: true
+        world_body:
+          Value: true
+      Marker Scale: 0.25
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      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
+      Name: RobotModel
+      Robot Description: dabo/robot_description
+      TF Prefix: dabo
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/Group
+      Displays:
+        - Angle Tolerance: 0.30000001192092896
+          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.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: false
+          Enabled: true
+          Keep: 100
+          Name: Odometry
+          Position Tolerance: 0.30000001192092896
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.10000000149011612
+            Head Radius: 0.10000000149011612
+            Shaft Length: 0.5
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic: /dabo/odom
+          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/LaserScan
+          Color: 85; 0; 255
+          Color Transformer: FlatColor
+          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: FrontLaserScan
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /dabo/sensors/front_scan
+          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: 85; 170; 255
+          Color Transformer: FlatColor
+          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: RearLaserScan
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /dabo/sensors/rear_scan
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz/Group
+          Displays:
+            - Class: rviz/Image
+              Enabled: true
+              Image Topic: /dabo/bumblebee2_left/image_raw
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: LeftImage
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: compressed
+              Unreliable: false
+              Value: true
+            - Class: rviz/Image
+              Enabled: true
+              Image Topic: /dabo/bumblebee2_right/image_raw
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: RightImage
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: compressed
+              Unreliable: false
+              Value: true
+            - Class: rviz/Camera
+              Enabled: false
+              Image Rendering: background and overlay
+              Image Topic: /dabo/bumblebee2_right/image_raw
+              Name: RightCamera
+              Overlay Alpha: 0.5
+              Queue Size: 2
+              Transport Hint: compressed
+              Unreliable: false
+              Value: false
+              Visibility:
+                "": true
+                Grid: true
+                RobotModel: true
+                Sensors:
+                  Camera:
+                    LeftCamera: true
+                    LeftImage: true
+                    RightImage: true
+                    Value: true
+                  FrontLaserScan: true
+                  Odometry: true
+                  RearLaserScan: true
+                  Value: true
+                TF: true
+                Value: true
+              Zoom Factor: 1
+            - Class: rviz/Camera
+              Enabled: false
+              Image Rendering: background and overlay
+              Image Topic: /dabo/bumblebee2_left/image_raw
+              Name: LeftCamera
+              Overlay Alpha: 0.5
+              Queue Size: 2
+              Transport Hint: compressed
+              Unreliable: false
+              Value: false
+              Visibility:
+                "": true
+                Grid: true
+                RobotModel: true
+                Sensors:
+                  Camera:
+                    LeftImage: true
+                    RightCamera: true
+                    RightImage: true
+                    Value: true
+                  FrontLaserScan: true
+                  Odometry: true
+                  RearLaserScan: true
+                  Value: true
+                TF: true
+                Value: true
+              Zoom Factor: 1
+          Enabled: false
+          Name: Camera
+      Enabled: true
+      Name: Sensors
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /lpd/markers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /lpd_2/markers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mht_2/tracksMarkers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /dabo/planner_markers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: dabo/odom
+    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
+      Theta std deviation: 0.2617993950843811
+      Topic: /dabo/initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /dabo/move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 85.43057250976562
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        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.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7903981804847717
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.7853981852531433
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1029
+  Hide Left Dock: false
+  Hide Right Dock: false
+  LeftCamera:
+    collapsed: false
+  LeftImage:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003ab000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000035f000002b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000308000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055d0000015bfc0100000006fb00000012004c0065006600740049006d0061006700650000000000000003070000006e00fffffffb00000014005200690067006800740049006d006100670065000000030d000002500000007600fffffffb00000014004c00650066007400430061006d00650072006100000002b1000002ac0000007600fffffffb000000160052006900670068007400430061006d0065007200610000000498000000c50000007e00fffffffb0000000800540069006d00650000000000000004b00000025600fffffffb0000000800540069006d006501000000000000045000000000000000000000060e000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RightCamera:
+    collapsed: false
+  RightImage:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1918
+  X: 0
+  Y: 25
--- a/rviz/dabo_companion_ok2.rviz
+++ b/rviz/dabo_companion_ok2.rviz
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /Sensors1
+        - /MarkerArray4
+        - /Map1
+      Splitter Ratio: 0.5936599373817444
+    Tree Height: 800
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+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.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        dabo/base_footprint:
+          Value: true
+        dabo/base_link:
+          Value: true
+        dabo/body:
+          Value: true
+        dabo/caster_support:
+          Value: true
+        dabo/front_caster_wheel:
+          Value: true
+        dabo/front_hokuyo_base:
+          Value: true
+        dabo/front_hokuyo_scan_frame:
+          Value: true
+        dabo/front_support:
+          Value: true
+        dabo/head:
+          Value: true
+        dabo/head_servo1:
+          Value: true
+        dabo/head_servo2:
+          Value: true
+        dabo/head_servo3:
+          Value: true
+        dabo/left_arm:
+          Value: true
+        dabo/left_arm_servo1:
+          Value: true
+        dabo/left_arm_servo2:
+          Value: true
+        dabo/left_wheel_link:
+          Value: true
+        dabo/odom:
+          Value: true
+        dabo/rear_caster_wheel:
+          Value: true
+        dabo/rear_hokuyo_base:
+          Value: true
+        dabo/rear_hokuyo_scan_frame:
+          Value: true
+        dabo/rear_support:
+          Value: true
+        dabo/right_arm:
+          Value: true
+        dabo/right_arm_servo1:
+          Value: true
+        dabo/right_arm_servo2:
+          Value: true
+        dabo/right_wheel_link:
+          Value: true
+        dabo/top_plate:
+          Value: true
+        map:
+          Value: true
+        world:
+          Value: true
+        world_body:
+          Value: true
+      Marker Scale: 0.25
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      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
+      Name: RobotModel
+      Robot Description: dabo/robot_description
+      TF Prefix: dabo
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/Group
+      Displays:
+        - Angle Tolerance: 0.30000001192092896
+          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.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: false
+          Enabled: true
+          Keep: 100
+          Name: Odometry
+          Position Tolerance: 0.30000001192092896
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.10000000149011612
+            Head Radius: 0.10000000149011612
+            Shaft Length: 0.5
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic: /dabo/odom
+          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/LaserScan
+          Color: 85; 0; 255
+          Color Transformer: FlatColor
+          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: FrontLaserScan
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /dabo/sensors/front_scan
+          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: 85; 170; 255
+          Color Transformer: FlatColor
+          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: RearLaserScan
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /dabo/sensors/rear_scan
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz/Group
+          Displays:
+            - Class: rviz/Image
+              Enabled: true
+              Image Topic: /dabo/bumblebee2_left/image_raw
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: LeftImage
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: compressed
+              Unreliable: false
+              Value: true
+            - Class: rviz/Image
+              Enabled: true
+              Image Topic: /dabo/bumblebee2_right/image_raw
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: RightImage
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: compressed
+              Unreliable: false
+              Value: true
+            - Class: rviz/Camera
+              Enabled: false
+              Image Rendering: background and overlay
+              Image Topic: /dabo/bumblebee2_right/image_raw
+              Name: RightCamera
+              Overlay Alpha: 0.5
+              Queue Size: 2
+              Transport Hint: compressed
+              Unreliable: false
+              Value: false
+              Visibility:
+                Grid: true
+                Map: true
+                MarkerArray: true
+                RobotModel: true
+                Sensors:
+                  Camera:
+                    LeftCamera: true
+                    LeftImage: true
+                    RightImage: true
+                    Value: true
+                  FrontLaserScan: true
+                  Odometry: true
+                  RearLaserScan: true
+                  Value: true
+                TF: true
+                Value: true
+              Zoom Factor: 1
+            - Class: rviz/Camera
+              Enabled: false
+              Image Rendering: background and overlay
+              Image Topic: /dabo/bumblebee2_left/image_raw
+              Name: LeftCamera
+              Overlay Alpha: 0.5
+              Queue Size: 2
+              Transport Hint: compressed
+              Unreliable: false
+              Value: false
+              Visibility:
+                Grid: true
+                Map: true
+                MarkerArray: true
+                RobotModel: true
+                Sensors:
+                  Camera:
+                    LeftImage: true
+                    RightCamera: true
+                    RightImage: true
+                    Value: true
+                  FrontLaserScan: true
+                  Odometry: true
+                  RearLaserScan: true
+                  Value: true
+                TF: true
+                Value: true
+              Zoom Factor: 1
+          Enabled: false
+          Name: Camera
+      Enabled: true
+      Name: Sensors
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /lpd/markers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /lpd_2/markers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mht_2/tracksMarkers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /dabo/planner_markers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /dabo/map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: dabo/odom
+    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
+      Theta std deviation: 0.2617993950843811
+      Topic: /dabo/initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /dabo/move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 85.43057250976562
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        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.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7953981757164001
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.7853981852531433
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1029
+  Hide Left Dock: false
+  Hide Right Dock: false
+  LeftCamera:
+    collapsed: false
+  LeftImage:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003ab000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000035f000002b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000308000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055d0000015bfc0100000006fb00000012004c0065006600740049006d0061006700650000000000000003070000006e00fffffffb00000014005200690067006800740049006d006100670065000000030d000002500000007600fffffffb00000014004c00650066007400430061006d00650072006100000002b1000002ac0000007600fffffffb000000160052006900670068007400430061006d0065007200610000000498000000c50000007e00fffffffb0000000800540069006d00650000000000000004b00000025600fffffffb0000000800540069006d006501000000000000045000000000000000000000060e000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RightCamera:
+    collapsed: false
+  RightImage:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1918
+  X: 0
+  Y: 25
--- a/rviz/tibi_companion.rviz
+++ b/rviz/tibi_companion.rviz
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /Person11
+        - /Person11/Person1Model1
+        - /Obstacles1
+      Splitter Ratio: 0.542553
+    Tree Height: 609
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: FrontLaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 200
+      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
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        body_mesh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bumblebee:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bumblebee_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        bumblebee_left_sim:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        bumblebee_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        bumblebee_right_sim:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        caster_support:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_caster_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_support:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_arm:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_shoulder1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_shoulder2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        neck1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        neck2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        neck3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_caster_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_support:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_arm:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_shoulder1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_shoulder2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: tibi/robot_description
+      TF Prefix: tibi
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Angle Tolerance: 0.2
+      Class: rviz/Odometry
+      Color: 255; 170; 0
+      Enabled: true
+      Keep: 100
+      Length: 0.3
+      Name: Odometry
+      Position Tolerance: 0.2
+      Topic: /tibi/segway/odom
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /tibi/map
+      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/LaserScan
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      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: FrontLaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /tibi/sensors/front_laser_scan
+      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; 170; 0
+      Color Transformer: FlatColor
+      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: RearLaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /tibi/sensors/rear_laser_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: false
+      Name: Footprint Polygon
+      Topic: /tibi/move_base/local_costmap/footprint
+      Unreliable: false
+      Value: false
+    - Alpha: 0.23
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: false
+      Name: LocalMap
+      Topic: /tibi/move_base/local_costmap/costmap
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 0; 0
+      Enabled: false
+      Head Diameter: 0.3
+      Head Length: 0.2
+      Length: 0.3
+      Line Style: Lines
+      Line Width: 0.03
+      Name: LocalPath
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.03
+      Shaft Diameter: 0.1
+      Shaft Length: 0.1
+      Topic: /tibi/move_base/AkpLocalPlanner/local_plan
+      Unreliable: false
+      Value: false
+    - Alpha: 0.2
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: false
+      Name: GlobalMap
+      Topic: /tibi/move_base/global_costmap/costmap
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 0; 0; 255
+      Enabled: false
+      Head Diameter: 0.3
+      Head Length: 0.2
+      Length: 0.3
+      Line Style: Lines
+      Line Width: 0.03
+      Name: GlobalPath
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.03
+      Shaft Diameter: 0.1
+      Shaft Length: 0.1
+      Topic: /tibi/move_base/AkpLocalPlanner/global_plan
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: false
+      Head Diameter: 0.3
+      Head Length: 0.2
+      Length: 0.3
+      Line Style: Lines
+      Line Width: 0.03
+      Name: PlanPath
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.03
+      Shaft Diameter: 0.1
+      Shaft Length: 0.1
+      Topic: /tibi/move_base/NavfnROS/plan
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.1
+      Class: rviz/Pose
+      Color: 255; 255; 0
+      Enabled: true
+      Head Length: 0.3
+      Head Radius: 0.1
+      Name: GoalPose
+      Shaft Length: 1
+      Shaft Radius: 0.05
+      Shape: Arrow
+      Topic: /tibi/move_base/current_goal
+      Unreliable: false
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Angle Tolerance: 0.2
+          Class: rviz/Odometry
+          Color: 0; 170; 0
+          Enabled: true
+          Keep: 100
+          Length: 0.3
+          Name: Person1Odometry
+          Position Tolerance: 0.2
+          Topic: /person1/odom
+          Value: true
+        - Alpha: 0.5
+          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
+            base_footprint:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            base_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            head:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+          Name: Person1Model
+          Robot Description: person1_description
+          TF Prefix: person1
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+      Enabled: true
+      Name: Person1
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /tibi/mht/tracksMarkers
+      Name: PeopleTracks
+      Namespaces:
+        cov_track: true
+        id: true
+        vel: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /tibi/planner_markers
+      Name: PlannerMarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.5
+          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
+          Name: Box1Model
+          Robot Description: box1_description
+          TF Prefix: box1
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+        - Alpha: 0.5
+          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
+          Name: Box2Model
+          Robot Description: box2_description
+          TF Prefix: box2
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+      Enabled: true
+      Name: Obstacles
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /tibi/lpmf/peopleMarkers
+      Name: MarkerArray
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /tibi/lpd_front/peopleMarkers_array
+      Name: MarkerArray
+      Namespaces:
+        people: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /tibi/lpd_rear/peopleMarkers_array
+      Name: MarkerArray
+      Namespaces:
+        people: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/SetInitialPose
+      Topic: /tibi/initialpose
+    - Class: rviz/SetGoal
+      Topic: /tibi/move_base_simple/goal
+    - Class: rviz/CancelAction
+      Topic: /tibi/move_base/cancel
+  Value: true
+  Views:
+    Current:
+      Class: rviz/ThirdPersonFollower
+      Distance: 8.43049
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 10.6797
+        Y: 19.7547
+        Z: -4.21554e-05
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.2598
+      Target Frame: map
+      Value: ThirdPersonFollower (rviz)
+      Yaw: 3.2104
+    Saved:
+      - Class: rviz/ThirdPersonFollower
+        Distance: 18.1534
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.06
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 0.0519981
+          Y: -0.0570698
+          Z: -0.240009
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.01
+        Pitch: 1.2598
+        Target Frame: tibi/base_link
+        Value: ThirdPersonFollower (rviz)
+        Yaw: -3.07279
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 744
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002a2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df000001af000000effb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002a2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001810000014900000000000000000000000100000215000002a2fc0200000007fb0000000a0049006d0061006700650000000028000002a20000000000000000fb0000000c00430061006d00650072006100000000280000029f0000000000000000fb0000000c00430061006d006500720061000000016d0000015a0000000000000000fb0000000c00430061006d00650072006101000001890000013e0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002a2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000024f000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 959
+  X: 65
+  Y: 126