add launchs y configs nuevos para ana y dabo

config/ana/common_params.yaml

+# footprint parameters
+#footprint: [[0.35, 0.35], [0.35, -0.35], [-0.35,-0.35 ], [-0.35, 0.35]]
+robot_radius: 0.5
+footprint_padding: 0.01
+footprint_topic: /dabo/move_base/local_costmap/footprint
+published_footprint_topic: false
+
+robot_base_frame: dabo/base_footprint
+track_unknown_space: false
+always_send_full_costmap: false
+transform_tolerance: 1.0
+width: 10.0
+height: 10.0
+resolution: 0.1
+origin_x: 0.0
+origin_y: 0.0
+
+obstacle_layer:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  max_obstacle_height: 1.0
+  footprint_clearing_enabled: false
+  combination_method: 1
+  observation_sources: front_laser rear_laser
+  front_laser:  {
+    sensor_frame: dabo/front_hokuyo_scan_frame,
+    data_type: LaserScan,
+    topic: /dabo/sensors/front_scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 1.8,
+    expected_update_rate: 10,
+    obstacle_range: 30.0,
+    raytrace_range: 40.0
+  }
+  
+  rear_laser:  {
+    sensor_frame: dabo/rear_hokuyo_scan_frame,
+    data_type: LaserScan,
+    topic: /dabo/sensors/rear_scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 1.8,
+    expected_update_rate: 10,
+    obstacle_range: 30.0,
+    raytrace_range: 40.0
+  }
+
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  inflation_radius: 2.0
+  cost_scaling_factor:  5.0
+  inflation_radius:     0.5

config/ana/costmap_common_params.yaml

+# footprint parameters
+#footprint: [[0.35, 0.35], [0.35, -0.35], [-0.35,-0.35 ], [-0.35, 0.35]]
+robot_radius: 0.5
+footprint_padding: 0.01
+footprint_topic: /dabo/move_base/local_costmap/footprint
+published_footprint_topic: false
+
+robot_base_frame: dabo/base_footprint
+track_unknown_space: false
+always_send_full_costmap: false
+transform_tolerance: 1.0
+width: 10.0
+height: 10.0
+resolution: 0.1
+origin_x: 0.0
+origin_y: 0.0
+
+obstacle_layer:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  max_obstacle_height: 1.0
+  footprint_clearing_enabled: false
+  combination_method: 1
+  observation_sources: front_laser rear_laser
+  front_laser:  {
+    sensor_frame: dabo/front_hokuyo_scan_frame,
+    data_type: LaserScan,
+    topic: /dabo/sensors/front_scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 1.8,
+    expected_update_rate: 10,
+    obstacle_range: 30.0,
+    raytrace_range: 40.0
+  }
+  
+  rear_laser:  {
+    sensor_frame: dabo/rear_hokuyo_scan_frame,
+    data_type: LaserScan,
+    topic: /dabo/sensors/rear_scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 1.8,
+    expected_update_rate: 10,
+    obstacle_range: 30.0,
+    raytrace_range: 40.0
+  }
+
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  inflation_radius: 2.0
+  cost_scaling_factor:  5.0
+  inflation_radius:     0.5

config/ana/global_costmap_params.yaml

+#global_costmap: ###namespace added when loading parameters in move_base.launch
+global_frame: map
+update_frequency: 1.0
+publish_frequency: 0.0
+rolling_window: false
+
+plugins:
+  - {name: static_layer,    type: "costmap_2d::StaticLayer"}
+  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+    
+static_layer:
+  enabled: true
+  map_topic: /dabo/map
+  first_map_only: false
+  subscribe_to_updates: false
+  unknown_cost_value: -1
+  use_maximum: false
+  lethal_cost_threshold: 100
+  track_unknown_space: true
+  trinary_costmap: true
+
+# version anterior, el 14.04ubunto y rosIndigo
+global_costmap:
+  publish_voxel_map: true
+  robot_base_frame:  dabo/base_footprint
+#  update_frequency:  0.2
+#  publish_frequency: 0.2
+  static_map:        true
+  rolling_window:    false
+  transform_tolerance: 1.0
+  inflation_layer:
+   inflation_radius: 2.0
+   cost_scaling_factor: 1.5

config/ana2/akp_local_planner_params.yaml

+AkpLocalPlanner:
+  move_base: True
+  plan_mode: 2
+  distance_mode: 1
+  global_mode: 3
+  vis_mode:    1
+  frozen_mode: False
+  number_vertex: 200
+  horizon_time: 5.0
+  cost_distance:    0.7
+  cost_orientation: 0.5
+  cost_w_robot:     0.4
+  cost_w_people:    0.2
+  cost_time:        0.25
+  cost_obs:         0.2
+  cost_old_path:    0.4
+  cost_l_minima:    0.0
+  v_max:  1.2
+  w_max:  1.0
+  av_max: 0.3
+  av_max_neg: 0.4
+  av_Vrobotzero: 0.6
+  lim_Vrobotzero: 0.1
+  av_break: 0.4
+  aw_max: 0.9
+  platform_radii: 0.5
+  xy_goal_tolerance: 0.3
+  distance_to_stop: 2.0
+  v_goal_tolerance: 0.1
+  esfm_to_person_A: 5.05
+  esfm_to_person_B: 1.2
+  esfm_to_person_lambda: 0.25
+  esfm_to_robot_A: 5.04
+  esfm_to_robot_B: 1.2
+  esfm_to_robot_lambda: 0.25
+  esfm_to_obstacle_A: 3.5
+  esfm_to_obstacle_B: 0.68
+  esfm_to_obstacle_lambda: 1.0
+  esfm_k: 2.3
+  esfm_d: 0.2
+  min_v_to_predict : 0.1
+  ppl_collision_mode : 0
+  pr_force_mode : 0
+  goal_providing_mode : 1
+  slicing_diff_orientation : 50.0
+  esfm_to_person_companion_A: 5.0252
+  esfm_to_person_companion_B: 0.71  
+  esfm_to_person_companion_lambda: 0.25
+  esfm_companion_d: 0.1
+  genome_Cr: 0.62
+  genome_Ct: 0.08
+  genome_eta: -0.43
+  genome_r0: 1.0
+  genome_S0: 0.5
+  genome_rmax: 3
+
+#############################
+#  av_max: 0.2  (OJO!, cambiados para que vaya mejor la simulacion, pero en real son estos!)
+#  av_max_neg: 0.2
+#  av_break: 0.2
+#  aw_max: 0.7
+#############################
+### Parameters of group model Francesco
+##############
+#
+# valores buenos side-by-side
+#  esfm_to_person_companion_A: 3.0252
+#  esfm_to_person_companion_B: 0.33  
+#  esfm_to_person_companion_lambda: 1.0
+#  esfm_companion_d: 0.1
+#  esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.13  
+#  esfm_to_person_companion_lambda: 1.0
+# (mejor opcion)y9= 1.5471* exp((0.4464-d) / (0.5710));
+#y9= 0.2471* exp((0.4464-d) / (0.1710)); y9= A* exp((d_0-d) / (B));
+##from launch file
+#  force_map_path: "/home/fherrero/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation_companion/map/force_map.txt"
+#  destination_map_path: "/home/fherrero/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation_companion/map/2_destinations.txt"
+#  robot: "tibi"
+# esfm_to_obstacle_A: 5.0
+# esfm_to_robot_B: 0.2 (antes)
+#  (antes) w_max:  0.8
+#  (antes) aw_max: 0.9
+# esfm_k: 2.3
+# esfm_to_robot_A: 7.05
+#  esfm_to_robot_B: 0.2
+#  esfm_to_robot_lambda: 0.05
+#  esfm_d: 0.2
+
+# parametro bueno mio, para companion y find target
+# cambiado en find target, porque no iba bien en las predicciones de personas.
+#  esfm_to_obstacle_A: 3.5
+#   esfm_to_obstacle_B: 0.5
+#  esfm_to_obstacle_lambda: 1.0
+# esfm_to_robot_A: 6.05
+#  esfm_to_robot_B: 0.45
+#  esfm_to_robot_lambda: 0.25
+# anteriores SIMULACION
+#  v_max:  0.9
+#  w_max:  0.785
+#  av_max: 0.4
+#  av_break: 0.4
+#  aw_max: 0.785
+# IDEAL para person companion
+#  esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.4963 => rebajado a 0.1963 para que NO choque nunca! pero que este a su lado bien puesta.
+#  esfm_to_person_companion_lambda: 1.0
+ # esfm_companion_d: 0.1
+#  esfm_to_person_A: 5.05 
+#  esfm_to_person_B: 0.91 Ideal, =>  rebajado a 0.71 para que no se aleje tanto de las personas!==SOLO ZANLUNGO (en atr para que no oscilara tube que rebajarla a 0.10 y lambda =1, d=0.25
+#  esfm_to_person_lambda: 0.25
+
+# Parametros companion buenos:
+#   esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.35  
+#  esfm_to_person_companion_lambda: 1.0
+#esfm_to_obstacle_A: 3.5
+#  esfm_to_obstacle_B: 0.65
+#
+#
+
+    

config/ana2/akp_local_planner_params_BUENOS_ZanlungoVform.yaml

+AkpLocalPlanner:
+  move_base: True
+  plan_mode: 2
+  distance_mode: 1
+  global_mode: 3
+  vis_mode:    1
+  frozen_mode: False
+  number_vertex: 500
+  horizon_time: 5.0
+  cost_distance:    0.7
+  cost_orientation: 0.5
+  cost_w_robot:     0.4
+  cost_w_people:    0.2
+  cost_time:        0.25
+  cost_obs:         0.2
+  cost_old_path:    0.4
+  cost_l_minima:    0.0
+  v_max:  1.0
+  w_max:  0.7
+  av_max: 0.35
+  av_max_neg: 0.1
+  av_break: 0.35
+  aw_max: 0.9
+  platform_radii: 0.5
+  xy_goal_tolerance: 0.3
+  distance_to_stop: 2.0
+  v_goal_tolerance: 0.1
+  esfm_to_person_A: 5.05
+  esfm_to_person_B: 0.71
+  esfm_to_person_lambda: 0.25
+  esfm_to_robot_A: 0.0
+  esfm_to_robot_B: 1.2
+  esfm_to_robot_lambda: 0.25
+  esfm_to_obstacle_A: 3.5
+  esfm_to_obstacle_B: 0.65
+  esfm_to_obstacle_lambda: 1.0
+  esfm_k: 1.52
+  esfm_d: 0.2
+  min_v_to_predict : 0.1
+  ppl_collision_mode : 0
+  pr_force_mode : 0
+  goal_providing_mode : 1
+  slicing_diff_orientation : 50.0
+  esfm_to_person_companion_A: 5.0252
+  esfm_to_person_companion_B: 0.4  
+  esfm_to_person_companion_lambda: 1.0
+  esfm_companion_d: 0.1
+  genome_Cr: 0.62
+  genome_Ct: 0.08
+  genome_eta: -0.43
+  genome_r0: 1.0
+  genome_S0: 0.5
+  genome_rmax: 3
+#############################
+### Parameters of group model Francesco
+##############
+#
+# valores buenos side-by-side
+#  esfm_to_person_companion_A: 3.0252
+#  esfm_to_person_companion_B: 0.33  
+#  esfm_to_person_companion_lambda: 1.0
+#  esfm_companion_d: 0.1
+#  esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.13  
+#  esfm_to_person_companion_lambda: 1.0
+# (mejor opcion)y9= 1.5471* exp((0.4464-d) / (0.5710));
+#y9= 0.2471* exp((0.4464-d) / (0.1710)); y9= A* exp((d_0-d) / (B));
+##from launch file
+#  force_map_path: "/home/fherrero/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation_companion/map/force_map.txt"
+#  destination_map_path: "/home/fherrero/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation_companion/map/2_destinations.txt"
+#  robot: "tibi"
+# esfm_to_obstacle_A: 5.0
+# esfm_to_robot_B: 0.2 (antes)
+#  (antes) w_max:  0.8
+#  (antes) aw_max: 0.9
+# esfm_k: 2.3
+# esfm_to_robot_A: 7.05
+#  esfm_to_robot_B: 0.2
+#  esfm_to_robot_lambda: 0.05
+#  esfm_d: 0.2
+
+# parametro bueno mio, para companion y find target
+# cambiado en find target, porque no iba bien en las predicciones de personas.
+#  esfm_to_obstacle_A: 3.5
+#   esfm_to_obstacle_B: 0.5
+#  esfm_to_obstacle_lambda: 1.0
+# esfm_to_robot_A: 6.05
+#  esfm_to_robot_B: 0.45
+#  esfm_to_robot_lambda: 0.25
+# anteriores SIMULACION
+#  v_max:  0.9
+#  w_max:  0.785
+#  av_max: 0.4
+#  av_break: 0.4
+#  aw_max: 0.785
+# IDEAL para person companion
+#  esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.4963 => rebajado a 0.1963 para que NO choque nunca! pero que este a su lado bien puesta.
+#  esfm_to_person_companion_lambda: 1.0
+ # esfm_companion_d: 0.1
+#  esfm_to_person_A: 5.05 
+#  esfm_to_person_B: 0.91 Ideal, =>  rebajado a 0.71 para que no se aleje tanto de las personas! (en atr para que no oscilara tube que rebajarla a 0.10 y lambda =1, d=0.25
+#  esfm_to_person_lambda: 0.25
+
+# Parametros companion buenos:
+#   esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.35  
+#  esfm_to_person_companion_lambda: 1.0
+#
+#
+#
+
+    

config/ana2/akp_local_planner_params_SIDE_By_SIDE_demo2019.yaml

+AkpLocalPlanner:
+  move_base: True
+  plan_mode: 2
+  distance_mode: 1
+  global_mode: 3
+  vis_mode:    1
+  frozen_mode: False
+  number_vertex: 500
+  horizon_time: 5.0
+  cost_distance:    0.7
+  cost_orientation: 0.5
+  cost_w_robot:     0.4
+  cost_w_people:    0.2
+  cost_time:        0.25
+  cost_obs:         0.2
+  cost_old_path:    0.4
+  cost_l_minima:    0.0
+  v_max:  1.0
+  w_max:  0.7
+  av_max: 0.2
+  av_max_neg: 0.3
+  av_break: 0.2
+  aw_max: 0.8
+  platform_radii: 0.5
+  xy_goal_tolerance: 0.3
+  distance_to_stop: 2.0
+  v_goal_tolerance: 0.1
+  esfm_to_person_A: 3.5
+  esfm_to_person_B: 0.81
+  esfm_to_person_lambda: 0.25
+  esfm_to_robot_A: 3.5
+  esfm_to_robot_B: 0.81
+  esfm_to_robot_lambda: 0.25
+  esfm_to_obstacle_A: 3.5
+  esfm_to_obstacle_B: 0.78
+  esfm_to_obstacle_lambda: 1.0
+  esfm_k: 1.52
+  esfm_d: 0.2
+  min_v_to_predict : 0.1
+  ppl_collision_mode : 0
+  pr_force_mode : 0
+  goal_providing_mode : 1
+  slicing_diff_orientation : 50.0
+  esfm_to_person_companion_A: 10.0252
+  esfm_to_person_companion_B: 0.4  
+  esfm_to_person_companion_lambda: 1.0
+  esfm_companion_d: 0.1
+  genome_Cr: 0.62
+  genome_Ct: 0.08
+  genome_eta: -0.43
+  genome_r0: 1.0
+  genome_S0: 0.5
+  genome_rmax: 3
+#############################
+### Parameters of group model Francesco
+##############
+#
+# valores buenos side-by-side
+#  esfm_to_person_companion_A: 3.0252
+#  esfm_to_person_companion_B: 0.33  
+#  esfm_to_person_companion_lambda: 1.0
+#  esfm_companion_d: 0.1
+#  esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.13  
+#  esfm_to_person_companion_lambda: 1.0
+# (mejor opcion)y9= 1.5471* exp((0.4464-d) / (0.5710));
+#y9= 0.2471* exp((0.4464-d) / (0.1710)); y9= A* exp((d_0-d) / (B));
+##from launch file
+#  force_map_path: "/home/fherrero/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation_companion/map/force_map.txt"
+#  destination_map_path: "/home/fherrero/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation_companion/map/2_destinations.txt"
+#  robot: "tibi"
+# esfm_to_obstacle_A: 5.0
+# esfm_to_robot_B: 0.2 (antes)
+#  (antes) w_max:  0.8
+#  (antes) aw_max: 0.9
+# esfm_k: 2.3
+# esfm_to_robot_A: 7.05
+#  esfm_to_robot_B: 0.2
+#  esfm_to_robot_lambda: 0.05
+#  esfm_d: 0.2
+
+# parametro bueno mio, para companion y find target
+# cambiado en find target, porque no iba bien en las predicciones de personas.
+#  esfm_to_obstacle_A: 3.5
+#   esfm_to_obstacle_B: 0.5
+#  esfm_to_obstacle_lambda: 1.0
+# esfm_to_robot_A: 6.05
+#  esfm_to_robot_B: 0.45
+#  esfm_to_robot_lambda: 0.25
+# anteriores SIMULACION
+#  v_max:  0.9
+#  w_max:  0.785
+#  av_max: 0.4
+#  av_break: 0.4
+#  aw_max: 0.785
+# IDEAL para person companion
+#  esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.4963 => rebajado a 0.1963 para que NO choque nunca! pero que este a su lado bien puesta.
+#  esfm_to_person_companion_lambda: 1.0
+ # esfm_companion_d: 0.1
+#  esfm_to_person_A: 5.05 
+#  esfm_to_person_B: 0.91 Ideal, =>  rebajado a 0.71 para que no se aleje tanto de las personas!==SOLO ZANLUNGO (en atr para que no oscilara tube que rebajarla a 0.10 y lambda =1, d=0.25
+#  esfm_to_person_lambda: 0.25
+
+# Parametros companion buenos:
+#   esfm_to_person_companion_A: 5.0252
+#  esfm_to_person_companion_B: 0.35  
+#  esfm_to_person_companion_lambda: 1.0
+#esfm_to_obstacle_A: 3.5
+#  esfm_to_obstacle_B: 0.65
+#
+#
+
+    

config/ana2/ana/common_params.yaml

+# footprint parameters
+#footprint: [[0.35, 0.35], [0.35, -0.35], [-0.35,-0.35 ], [-0.35, 0.35]]
+#footprint: [[0.3, 0.3], [0.3, -0.3], [-0.3,-0.3 ], [-0.3, 0.3]]
+footprint: [[0.25,0.25],[0.25,-0.25],[-0.25,-0.25],[-0.25,0.25]]
+#robot_radius: 0.35
+footprint_padding: 0.01
+footprint_topic: /ana/footprint
+published_footprint_topic: false
+
+robot_base_frame: ana/base_footprint
+robot_radius: 0.4
+track_unknown_space: false
+always_send_full_costmap: false
+transform_tolerance: 1.0
+width: 10.0
+height: 10.0
+resolution: 0.1
+origin_x: 0.0
+origin_y: 0.0
+   
+
+obstacle_layer:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  max_obstacle_height: 1.0
+  footprint_clearing_enabled: false
+  combination_method: 1
+  observation_sources: velodyne front_laser rear_laser #depth_cam
+
+  velodyne:  {
+    sensor_frame: ana/velodyne,
+    data_type: PointCloud2,
+    topic: /ana/sensors/velodyne_points,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 1.0,
+    expected_update_rate: 1,
+    obstacle_range: 30.0,
+    raytrace_range: 40.0
+  }
+
+ front_laser: {
+    sensor_frame: ana/sensors/scan,
+    data_type: LaserScan,
+    topic: /ana/sensors/scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.1,
+    max_obstacle_height: 2.0,
+    expected_update_rate: 0
+    }
+
+  rear_laser: {
+    sensor_frame: ana/sensors/scan,
+    data_type: LaserScan,
+    topic: /ana/sensors/scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.1,
+    max_obstacle_height: 2.0,
+    expected_update_rate: 0
+    }
+
+  depth_cam:  {
+    sensor_frame: ana/camera_color_optical_frame,
+    data_type: PointCloud2,
+    topic: /ana/sensors/nav_cam/depth_registered/points,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: false,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 1.0,
+    expected_update_rate: 1,
+    obstacle_range: 9.0,
+    raytrace_range: 10.0
+  }
+
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  inflation_radius: 2.0
+  cost_scaling_factor:  5.0
+  inflation_radius:     1.0

config/ana2/ana/costmap_common_params.yaml

+# footprint parameters
+#footprint: [[0.35, 0.35], [0.35, -0.35], [-0.35,-0.35 ], [-0.35, 0.35]]
+#footprint: [[0.3, 0.3], [0.3, -0.3], [-0.3,-0.3 ], [-0.3, 0.3]]
+footprint: [[0.25,0.25],[0.25,-0.25],[-0.25,-0.25],[-0.25,0.25]]
+#robot_radius: 0.35
+footprint_padding: 0.01
+footprint_topic: /ana/footprint
+published_footprint_topic: false
+
+robot_base_frame: ana/base_footprint
+track_unknown_space: false
+always_send_full_costmap: false
+transform_tolerance: 1.0
+width: 10.0
+height: 10.0
+resolution: 0.1
+origin_x: 0.0
+origin_y: 0.0
+   
+obstacle_layer:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  max_obstacle_height: 1.0
+  footprint_clearing_enabled: false
+  combination_method: 1
+  observation_sources: velodyne #depth_cam
+
+  velodyne:  {
+    sensor_frame: ana/velodyne,
+    data_type: PointCloud2,
+    topic: /ana/sensors/velodyne_points,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 1.0,
+    expected_update_rate: 1,
+    obstacle_range: 30.0,
+    raytrace_range: 40.0
+  }
+
+  depth_cam:  {
+    sensor_frame: ana/camera_color_optical_frame,
+    data_type: PointCloud2,
+    topic: /ana/sensors/nav_cam/depth_registered/points,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: false,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 1.0,
+    expected_update_rate: 1,
+    obstacle_range: 9.0,
+    raytrace_range: 10.0
+  }
+
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  inflation_radius: 2.0
+  cost_scaling_factor:  5.0
+  inflation_radius:     1.0

config/ana2/ana/global_costmap_params.yaml

+#global_costmap: ###namespace added when loading parameters in move_base.launch
+global_frame: map
+update_frequency: 1.0
+publish_frequency: 0.0
+rolling_window: false
+
+plugins:
+  - {name: static_layer,    type: "costmap_2d::StaticLayer"}
+  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+    
+static_layer:
+  enabled: true
+  map_topic: /ana/map
+  first_map_only: false
+  subscribe_to_updates: false
+  unknown_cost_value: -1
+  use_maximum: false
+  lethal_cost_threshold: 100
+  track_unknown_space: true
+  trinary_costmap: true
+
+# version anterior, el 14.04ubunto y rosIndigo
+global_costmap:
+  publish_voxel_map: true
+  robot_base_frame:  ana/base_footprint
+#  update_frequency:  0.2
+#  publish_frequency: 0.2
+  static_map:        true
+  rolling_window:    false
+  transform_tolerance: 1.0
+  inflation_layer:
+   inflation_radius: 2.0
+   cost_scaling_factor: 1.5

config/ana2/costmap_common_params.yaml

+#footprint: [[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]
+robot_radius: 0.4
+
+obstacle_layer:
+  observation_sources: front_laser rear_laser #fake
+
+  front_laser: {
+    sensor_frame: ana/sensors/scan,
+    data_type: LaserScan,
+    topic: /ana/sensors/scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.1,
+    max_obstacle_height: 2.0,
+    expected_update_rate: 0
+    }
+
+  rear_laser: {
+    sensor_frame: ana/sensors/scan,
+    data_type: LaserScan,
+    topic: /ana/sensors/scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.1,
+    max_obstacle_height: 2.0,
+    expected_update_rate: 0
+    }
+
+  fake_laser: {
+    sensor_frame: /fake_laser,
+    data_type: LaserScan,
+    topic: /fake_laser,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.2,
+    max_obstacle_height: 2.0,
+    expected_update_rate: 0 
+    }

config/ana2/costmap_common_params_terrinet.yaml

+#footprint: [[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]
+robot_radius: 0.4
+
+obstacle_layer:
+  observation_sources: front_laser rear_laser #fake
+
+  front_laser: {
+    sensor_frame: /laser_frame,
+    data_type: LaserScan,
+    topic: /scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.1,
+    max_obstacle_height: 2.0,
+    expected_update_rate: 0
+    }
+
+  rear_laser: {
+    sensor_frame: /laser_frame,
+    data_type: LaserScan,
+    topic: /scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.1,
+    max_obstacle_height: 2.0,
+    expected_update_rate: 0
+    }
+
+  fake_laser: {
+    sensor_frame: /fake_laser,
+    data_type: LaserScan,
+    topic: /fake_laser,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.2,
+    max_obstacle_height: 2.0,
+    expected_update_rate: 0 
+    }

config/ana2/global_costmap_params.yaml

+#global_costmap: ###namespace added when loading parameters in move_base.launch
+global_frame: /map
+update_frequency: 1.0
+publish_frequency: 0.0
+rolling_window: false
+robot_base_frame:  /ana/base_footprint
+
+plugins:
+  - {name: static_layer,    type: "costmap_2d::StaticLayer"}
+  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+    
+static_layer:
+  enabled: true
+  map_topic: /ana/map
+  first_map_only: false
+  subscribe_to_updates: false
+  unknown_cost_value: -1
+  use_maximum: false
+  lethal_cost_threshold: 100
+  track_unknown_space: true
+  trinary_costmap: true
+
+# version anterior, el 14.04ubunto y rosIndigo
+#global_costmap:
+#  publish_voxel_map: true
+#  global_frame:      map
+#  robot_base_frame:  ana/base_footprint
+#  update_frequency:  0.2
+#  publish_frequency: 0.2
+#  static_map:        true
+#  rolling_window:    false
+#  transform_tolerance: 1.0
+#  inflation_layer:
+#    inflation_radius: 2.0
+#    cost_scaling_factor: 1.5
+#  plugins:
+#  - {name: static_layer,    type: "costmap_2d::StaticLayer"}
+#  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+#  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

config/ana2/global_costmap_params_terrinet.yaml

+global_costmap:
+  publish_voxel_map: true
+  global_frame:      /map
+  robot_base_frame:  /base_frame
+  update_frequency:  0.2
+  publish_frequency: 0.2
+  static_map:        true
+  rolling_window:    false
+  transform_tolerance: 1.0
+  inflation_layer:
+    inflation_radius: 1.0
+    cost_scaling_factor: 1.5
+  plugins:
+  - {name: static_layer,    type: "costmap_2d::StaticLayer"}
+  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

config/ana2/local_costmap_params.yaml

+local_costmap:
+  publish_voxel_map: true
+  global_frame:      ana/odom
+  robot_base_frame:  ana/base_footprint
+  update_frequency:  1.0
+  publish_frequency: 1.0
+  static_map:        false
+  rolling_window:    true
+  obstacle_range: 4.0
+  raytrace_range: 4.0
+  width:     8.0
+  height:    8.0
+  resolution: 0.3
+  transform_tolerance: 1.0
+  inflation_layer:
+    inflation_radius: 1.0
+    cost_scaling_factor: 3.0
+  plugins:
+  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

config/ana2/local_costmap_params_terrinet.yaml

+local_costmap:
+  publish_voxel_map: true
+  global_frame:      /map
+  robot_base_frame:  /base_frame
+  update_frequency:  1.0
+  publish_frequency: 1.0
+  static_map:        false
+  rolling_window:    true
+  obstacle_range: 4.0
+  raytrace_range: 4.0
+  width:     8.0
+  height:    8.0
+  resolution: 0.3
+  transform_tolerance: 1.0
+  inflation_layer:
+    inflation_radius: 1.0
+    cost_scaling_factor: 3.0
+  plugins:
+  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

config/ana2/move_base_params.yaml

+shutdown_costmaps: false
+
+controller_frequency: 5.0
+controller_patience: 5.0
+
+planner_frequency: 0.0
+planner_patience: 30.0
+
+oscillation_timeout: 0.0 #infinite time
+oscillation_distance: 0.2
+
+recovery_behavior_enabled: false # true
+clearing_rotation_allowed: false # true
+
+base_local_planner: "iri_atr_akp_local_planner_companion/AkpLocalPlanner"
+#alternatives: base_local_planner/TrajectoryPlannerROS, dwa_local_planner/DWAPlannerROS
+base_global_planner: "navfn/NavfnROS"
+#alternatives: navfn/NavfnROS, global_planner/GlobalPlanner, carrot_planner/CarrotPlanner
+

config/ana2/move_base_params_aspsi.yaml

+shutdown_costmaps: false
+
+controller_frequency: 5.0
+controller_patience: 5.0
+
+planner_frequency: 0.0
+planner_patience: 30.0
+
+oscillation_timeout: 0.0 #infinite time
+oscillation_distance: 0.2
+
+recovery_behavior_enabled: false # true
+clearing_rotation_allowed: false # true
+
+base_local_planner: "iri_robot_aspsi/AkpLocalPlanner"
+#alternatives: base_local_planner/TrajectoryPlannerROS, dwa_local_planner/DWAPlannerROS
+base_global_planner: "navfn/NavfnROS"
+#alternatives: navfn/NavfnROS, global_planner/GlobalPlanner, carrot_planner/CarrotPlanner
+

launch/gazebo_ASPSI_BRL.launch

+<!-- -->
+<!--
+  Need to add gazebo PATHS to iri_gazebo_worlds in your ~/.bashrc
+    source /usr/share/gazebo/setup.sh
+    export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/iri-lab/iri_ws/src/iri_navigation/iri_gazebo_worlds/models
+    export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:~/iri-lab/iri_ws/src/iri_navigation/iri_gazebo_worlds/models
+-->
+<launch>
+
+  <arg name="world" default="master_big"/>   <!--Change the name: master_big with other gazebo's map name to change the map of the environemnt. -->
+  <arg name="robot" default="$(optenv ROBOT tibi)" />
+  <arg name="robot_x"     default="11.0"/> <!-- changes the x component of the initial robot's position inside the environment (it is in map coordinates)-->
+  <arg name="robot_y"     default="22.0"/> <!-- changes the y component of the initial robot's position inside the environment (it is in map coordinates)-->
+  <arg name="robot_yaw"   default="0.0"/> <!-- changes the initial robot's orientation inside the environment (it is in map coordinates)-->
+
+  <include file="$(find tibi_dabo_base)/machines/$(env ROBOT)_$(env ROS_MODE).machines" />
+
+
+  <include file="$(find tibi_dabo_gazebo)/launch/include/world.launch.xml">
+    <arg name="world" value="$(arg world)"/>
+  </include>
+  
+  <include file="$(find tibi_dabo_gazebo)/launch/include/robot.launch.xml">
+    <arg name="robot" value="$(arg robot)"/>
+    <arg name="model" value="$(arg robot)"/>
+    <arg name="x"     value="$(arg robot_x)"/>
+    <arg name="y"     value="$(arg robot_y)"/>
+    <arg name="yaw"   value="$(arg robot_yaw)"/>
+  </include>
+  
+<!-- person1 is the nearest person to the robot. This person will be the accompanied one, if you use exactly our instructions included in the read-me of the node. -->
+  <include file="$(find iri_objects_description)/launch/spawn_person.launch">
+    <arg name="name" value="person1"/>
+    <arg name="type" value="person"/>
+    <arg name="x"     value="11.5"/>  <!-- changes the x component of the initial person1's position inside the environment (it is in map coordinates).  -->
+    <arg name="y"     value="20.0"/> <!-- changes the y component of the initial person1's position inside the environment (it is in map coordinates)-->
+    <arg name="yaw"   value="0.0"/> <!-- changes the initial person1's orientation inside the environment (it is in map coordinates)-->
+  </include>
+
+  <include file="$(find iri_objects_description)/launch/spawn_person.launch">
+    <arg name="name" value="person2"/>
+    <arg name="type" value="person"/>
+    <arg name="x"     value="11.5"/> <!-- changes the x component of the initial person2's position inside the environment (it is in map coordinates).  -->
+    <arg name="y"     value="18.0"/> <!-- changes the y component of the initial person2's position inside the environment (it is in map coordinates)-->
+    <arg name="yaw"   value="0.0"/> <!-- changes the initial person2's orientation inside the environment (it is in map coordinates)-->
+
+  </include>
+
+
+
+<include file="$(find iri_objects_description)/launch/spawn_person.launch">
+    <arg name="name" value="person3"/>
+    <arg name="type" value="person"/>
+    <arg name="x"     value="40.5"/>
+    <arg name="y"     value="17.5"/>
+    <arg name="yaw"   value="1.57"/> 
+  </include>
+  
+<include file="$(find iri_objects_description)/launch/spawn_person.launch">
+    <arg name="name" value="person4"/>
+    <arg name="type" value="person"/>
+    <arg name="x"     value="40.0"/>
+    <arg name="y"     value="21.5"/>
+    <arg name="yaw"   value="1.57"/>
+  </include>
+
+<!-- You can include more people using the same structure-->
+
+  <!--nav-->
+  <!--  <include file="$(find iri_platform_teleop)/launch/tibi_dabo_teleop_safe_mux.launch"/>-->
+  <include file="$(find tibi_dabo_rosnav)/launch/tibi_dabo_cmd_vel_mux.launch"/>
+ <!-- <include file="$(find tibi_dabo_rosnav)/launch/tibi_dabo_velocity_smoother.launch"/> -->
+
+  <include file="$(find tibi_dabo_rosnav)/launch/tibi_dabo_amcl.launch">
+    <arg name="x" value="$(arg robot_x)"/>
+    <arg name="y" value="$(arg robot_y)"/>
+    <arg name="a" value="$(arg robot_yaw)"/>
+  </include>
+  <include file="$(find tibi_dabo_rosnav)/launch/tibi_dabo_map_server.launch">
+    <arg name="map" value="sim/$(arg world)"/>
+  </include>
+  <!--<include file="$(find tibi_dabo_rosnav)/launch/tibi_dabo_move_base_dwa_mux.launch"/>--> <!-- ROS navigation -->
+  <!-- ASPSI- gazebo only-->
+  <include file="$(find iri_robot_aspsi)/launch/sim_gazebo/tibi_dabo_akp_local_ASPSI.launch">
+    <arg name="map" value="sim/$(arg world)"/> 
+  </include>
+  <!--/nav-->
+
+ 
+  <!--people-->
+  <include file="$(find iri_laser_people_detection)/launch/tibi_dabo_lpd_map.launch"/>
+  <include file="$(find iri_laser_people_map_filter)/launch/tibi_dabo_laser_people_map_filter.launch"/>
+  <include file="$(find iri_people_tracking_mht)/launch/tibi_dabo_mht_map.launch"/>
+  <!--/people-->
+ 
+
+
+  <node name="rviz" pkg ="rviz" type="rviz" machine="monitor" 
+        args="-d $(find iri_robot_aspsi)/rviz/$(env ROBOT)_companion.rviz" />
+
+  <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" machine="monitor"/>
+
+</launch>

launch/gazebo_ASPSI_BRL_OK.launch

+<!-- -->
+<launch>
+
+
+    <!-- Ini Navigation Companion Nodes -->
+   <!--launch-prefix="xterm -e ddd -args"  -->
+
+ <group ns="dabo"> 
+  <rosparam command="delete" ns="/dabo/move_base" />
+
+   <node pkg ="move_base"
+          type="move_base"
+          name="move_base"      
+          output="screen">
+      <remap from="/dabo/move_base/cmd_vel"
+              to="/dabo/navigation/cmd_vel" />
+      <remap from="/dabo/move_base/odom"
+              to="/dabo/odom" />
+      <remap from="/dabo/front_scan"
+              to="/dabo/sensors/front_scan" />
+      <remap from="/dabo/rear_scan"
+              to="/dabo/sensors/rear_scan" />
+      <remap from="/dabo/tracks"
+               to="/mht_2/tracks" />
+      <rosparam file="$(find iri_robot_aspsi)/config/dabo/move_base_params.yaml"      command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/ana/costmap_common_params.yaml" command="load" ns="global_costmap" />
+      <rosparam file="$(find iri_robot_aspsi)/config/dabo/costmap_common_params.yaml" command="load" ns="local_costmap" />
+      <rosparam file="$(find iri_robot_aspsi)/config/dabo/local_costmap_params.yaml"  command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/ana/global_costmap_params.yaml" command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/dabo/akp_local_planner_params.yaml" command="load" />
+      <rosparam file="$(find iri_dabo_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml" command="load" />
+ <!--<rosparam file="$(find iri_dabo_rosnav)/params/global_planner/global_planner_params.yaml" command="load" />
+ <rosparam file="$(find iri_dabo_rosnav)/params/mux.yaml" command="load" />-->
+      <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/master_big_destinations_Gazebo_sim.txt" />
+	<param name="AkpLocalPlanner/results_filename_Zanlungo" value="name_file.txt" /> 
+      <param name="AkpLocalPlanner/robot" value="dabo"/>
+      <param name="AkpLocalPlanner/person_goal_id"  value="200"/> 
+      <param name="AkpLocalPlanner/speed_k"  value="1"/> 
+      <param name="AkpLocalPlanner/person_radi_amp"  value="0.2"/> 
+      <param name="AkpLocalPlanner/change_treshold_distance_between_steps"  value="0.3"/> 
+	<!-- for SIM-->
+ 	<param name="AkpLocalPlanner/change_sim"  value="False"/> 
+ 	<param name="AkpLocalPlanner/sim_target_per"  value="False"/> 
+        <param name="AkpLocalPlanner/frame_robot_footprint"  value="dabo/base_link"/> 
+    </node>
+ </group>
+
+  <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" />
+
+</launch>

launch/gazebo_ASPSI_BRL_OK_more_dest.launch

+<!-- -->
+<launch>
+
+
+    <!-- Ini Navigation Companion Nodes -->
+   <!--launch-prefix="xterm -e ddd -args"  -->
+
+ <group ns="dabo"> 
+  <rosparam command="delete" ns="/dabo/move_base" />
+
+   <node pkg ="move_base"
+          type="move_base"
+          name="move_base"      
+          output="screen">
+      <remap from="/dabo/move_base/cmd_vel"
+              to="/dabo/navigation/cmd_vel" />
+      <remap from="/dabo/move_base/odom"
+              to="/dabo/odom" />
+      <remap from="/dabo/front_scan"
+              to="/dabo/sensors/front_scan" />
+      <remap from="/dabo/rear_scan"
+              to="/dabo/sensors/rear_scan" />
+      <remap from="/dabo/tracks"
+               to="/mht_2/tracks" />
+      <rosparam file="$(find iri_robot_aspsi)/config/dabo/move_base_params.yaml"      command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/ana/costmap_common_params.yaml" command="load" ns="global_costmap" />
+      <rosparam file="$(find iri_robot_aspsi)/config/dabo/costmap_common_params.yaml" command="load" ns="local_costmap" />
+      <rosparam file="$(find iri_robot_aspsi)/config/dabo/local_costmap_params.yaml"  command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/ana/global_costmap_params.yaml" command="load" />
+      <rosparam file="$(find iri_robot_aspsi)/config/dabo/akp_local_planner_params.yaml" command="load" />
+      <rosparam file="$(find iri_dabo_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml" command="load" />
+ <!--<rosparam file="$(find iri_dabo_rosnav)/params/global_planner/global_planner_params.yaml" command="load" />
+ <rosparam file="$(find iri_dabo_rosnav)/params/mux.yaml" command="load" />-->
+      <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/master_big_destinations_Gazebo_sim_more_dest.txt" />
+	<param name="AkpLocalPlanner/results_filename_Zanlungo" value="name_file.txt" /> 
+      <param name="AkpLocalPlanner/robot" value="dabo"/>
+      <param name="AkpLocalPlanner/person_goal_id"  value="200"/> 
+      <param name="AkpLocalPlanner/speed_k"  value="1"/> 
+      <param name="AkpLocalPlanner/person_radi_amp"  value="0.2"/> 
+      <param name="AkpLocalPlanner/change_treshold_distance_between_steps"  value="0.3"/> 
+	<!-- for SIM-->
+ 	<param name="AkpLocalPlanner/change_sim"  value="False"/> 
+ 	<param name="AkpLocalPlanner/sim_target_per"  value="False"/> 
+        <param name="AkpLocalPlanner/frame_robot_footprint"  value="dabo/base_link"/> 
+    </node>
+ </group>
+
+  <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" />
+
+</launch>