diff --git a/config/ana/common_params.yaml b/config/ana/common_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f2b77404912104474d0f6f11a0e83adcc2434c38
--- /dev/null
+++ b/config/ana/common_params.yaml
@@ -0,0 +1,59 @@
+# 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
diff --git a/config/ana/costmap_common_params.yaml b/config/ana/costmap_common_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f2b77404912104474d0f6f11a0e83adcc2434c38
--- /dev/null
+++ b/config/ana/costmap_common_params.yaml
@@ -0,0 +1,59 @@
+# 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
diff --git a/config/ana/global_costmap_params.yaml b/config/ana/global_costmap_params.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..a1ad9eefc92c4d223b7703452ac5747ac4722f7c
--- /dev/null
+++ b/config/ana/global_costmap_params.yaml
@@ -0,0 +1,34 @@
+#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
diff --git a/config/ana2/akp_local_planner_params.yaml b/config/ana2/akp_local_planner_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..758cf17abeb4c97392993ed4fd0e94bd8fc7b9c8
--- /dev/null
+++ b/config/ana2/akp_local_planner_params.yaml
@@ -0,0 +1,122 @@
+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
+#
+#
+
+
diff --git a/config/ana2/akp_local_planner_params_BUENOS_ZanlungoVform.yaml b/config/ana2/akp_local_planner_params_BUENOS_ZanlungoVform.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fa71716c0e798027a2372a288e72cd63c0032729
--- /dev/null
+++ b/config/ana2/akp_local_planner_params_BUENOS_ZanlungoVform.yaml
@@ -0,0 +1,113 @@
+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
+#
+#
+#
+
+
diff --git a/config/ana2/akp_local_planner_params_SIDE_By_SIDE_demo2019.yaml b/config/ana2/akp_local_planner_params_SIDE_By_SIDE_demo2019.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f7800a4eaae8cca97b68790228c5f760833b3c25
--- /dev/null
+++ b/config/ana2/akp_local_planner_params_SIDE_By_SIDE_demo2019.yaml
@@ -0,0 +1,114 @@
+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
+#
+#
+
+
diff --git a/config/ana2/ana/common_params.yaml b/config/ana2/ana/common_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..46ee9f80b418242f41429711c7fbe1d658a2716f
--- /dev/null
+++ b/config/ana2/ana/common_params.yaml
@@ -0,0 +1,88 @@
+# 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
diff --git a/config/ana2/ana/costmap_common_params.yaml b/config/ana2/ana/costmap_common_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3e7471dc767e69ed1558a431163ba69ad6c53a43
--- /dev/null
+++ b/config/ana2/ana/costmap_common_params.yaml
@@ -0,0 +1,62 @@
+# 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
diff --git a/config/ana2/ana/global_costmap_params.yaml b/config/ana2/ana/global_costmap_params.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..1d925688d4c0c6dae7f8eacfc7a52f731658f7cb
--- /dev/null
+++ b/config/ana2/ana/global_costmap_params.yaml
@@ -0,0 +1,34 @@
+#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
diff --git a/config/ana2/costmap_common_params.yaml b/config/ana2/costmap_common_params.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..4e717ecce97029f640a25f323adca6df93e29da4
--- /dev/null
+++ b/config/ana2/costmap_common_params.yaml
@@ -0,0 +1,41 @@
+#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
+ }
diff --git a/config/ana2/costmap_common_params_terrinet.yaml b/config/ana2/costmap_common_params_terrinet.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8dcb51ada74780283e559ea83fccd4d1e281edca
--- /dev/null
+++ b/config/ana2/costmap_common_params_terrinet.yaml
@@ -0,0 +1,41 @@
+#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
+ }
diff --git a/config/ana2/global_costmap_params.yaml b/config/ana2/global_costmap_params.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..4b3de12f1157e1309608dbe089b4673bd8ac58a9
--- /dev/null
+++ b/config/ana2/global_costmap_params.yaml
@@ -0,0 +1,40 @@
+#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"}
diff --git a/config/ana2/global_costmap_params_terrinet.yaml b/config/ana2/global_costmap_params_terrinet.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f49035f893dd9c4bdc4cdd3954f6a436cb557030
--- /dev/null
+++ b/config/ana2/global_costmap_params_terrinet.yaml
@@ -0,0 +1,16 @@
+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"}
diff --git a/config/ana2/local_costmap_params.yaml b/config/ana2/local_costmap_params.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..1961789f1a6daa3da2075d69267d3dd13003115a
--- /dev/null
+++ b/config/ana2/local_costmap_params.yaml
@@ -0,0 +1,20 @@
+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"}
diff --git a/config/ana2/local_costmap_params_terrinet.yaml b/config/ana2/local_costmap_params_terrinet.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6eb1eadc39a7d2efd097766189d9baa0f2ef2ff2
--- /dev/null
+++ b/config/ana2/local_costmap_params_terrinet.yaml
@@ -0,0 +1,20 @@
+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"}
diff --git a/config/ana2/move_base_params.yaml b/config/ana2/move_base_params.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..2e536ebf6bc3c4371033742e8dfc364af7d924dd
--- /dev/null
+++ b/config/ana2/move_base_params.yaml
@@ -0,0 +1,19 @@
+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
+
diff --git a/config/ana2/move_base_params_aspsi.yaml b/config/ana2/move_base_params_aspsi.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..e4609eb2f578fbe65264ef1fe4164d354ce258e1
--- /dev/null
+++ b/config/ana2/move_base_params_aspsi.yaml
@@ -0,0 +1,19 @@
+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
+
diff --git a/launch/gazebo_ASPSI_BRL.launch b/launch/gazebo_ASPSI_BRL.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e9a9d7abcbc8e1566844ed3638fdd2758a8dc8b2
--- /dev/null
+++ b/launch/gazebo_ASPSI_BRL.launch
@@ -0,0 +1,103 @@
+<!-- -->
+<!--
+ 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>
diff --git a/launch/gazebo_ASPSI_BRL_OK.launch b/launch/gazebo_ASPSI_BRL_OK.launch
new file mode 100644
index 0000000000000000000000000000000000000000..30a1915d1c8eb3daf29056216c92c60c2868a477
--- /dev/null
+++ b/launch/gazebo_ASPSI_BRL_OK.launch
@@ -0,0 +1,50 @@
+<!-- -->
+<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>
diff --git a/launch/gazebo_ASPSI_BRL_OK_more_dest.launch b/launch/gazebo_ASPSI_BRL_OK_more_dest.launch
new file mode 100644
index 0000000000000000000000000000000000000000..ecc4606c25ab73e2dda0ae399f665afe5fa9c37a
--- /dev/null
+++ b/launch/gazebo_ASPSI_BRL_OK_more_dest.launch
@@ -0,0 +1,50 @@
+<!-- -->
+<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>
diff --git a/launch/gazebo_ASPSI_OK_ana.launch b/launch/gazebo_ASPSI_OK_ana.launch
new file mode 100644
index 0000000000000000000000000000000000000000..ba1860aec5b9d1a99ebb41a7a9d5b500c3e87c5c
--- /dev/null
+++ b/launch/gazebo_ASPSI_OK_ana.launch
@@ -0,0 +1,51 @@
+<!-- -->
+<launch>
+
+
+ <!-- Ini Navigation Companion Nodes -->
+ <!--launch-prefix="xterm -e ddd -args" -->
+
+ <group ns="$(optenv ROBOT ana)">
+ <rosparam command="delete" ns="/ana/move_base" />
+
+ <!--launch-prefix="xterm -e ddd -args" machine="visio"-->
+ <node pkg ="move_base"
+ type="move_base"
+ name="move_base"
+ output="screen">
+ <remap from="/ana/move_base_ana/cmd_vel"
+ to="/ana/navigation/cmd_vel" />
+ <remap from="/ana/move_base_ana/odom"
+ to="/ana/odom" />
+ <remap from="/ana/front_scan"
+ to="/ana/sensors/scan" />
+ <remap from="/ana/rear_scan"
+ to="/ana/sensors/scan" />
+ <remap from="/ana/tracks"
+ to="/ana/mht/tracks" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/move_base_params_aspsi.yaml" command="load" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/ana/costmap_common_params.yaml" command="load" ns="global_costmap" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/costmap_common_params.yaml" command="load" ns="local_costmap" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/local_costmap_params.yaml" command="load" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/ana/global_costmap_params.yaml" command="load" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/akp_local_planner_params.yaml" command="load" />
+<rosparam file="$(find iri_ana_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/destinaciones_ana_FME_ubuntu18_ros_melodic.txt" />
+ <param name="AkpLocalPlanner/results_filename_Zanlungo" value="name_file.txt" />
+ <param name="AkpLocalPlanner/robot" value="ana"/>
+ <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="ana/base_link"/>
+ </node>
+ </group>
+
+ <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" />
+
+</launch>
diff --git a/launch/gazebo_ASPSI_OK_ana_brl.launch b/launch/gazebo_ASPSI_OK_ana_brl.launch
new file mode 100644
index 0000000000000000000000000000000000000000..8361d8f89701ab3d48b383ae25640c3b247c7122
--- /dev/null
+++ b/launch/gazebo_ASPSI_OK_ana_brl.launch
@@ -0,0 +1,51 @@
+<!-- -->
+<launch>
+
+
+ <!-- Ini Navigation Companion Nodes -->
+ <!--launch-prefix="xterm -e ddd -args" -->
+
+ <group ns="$(optenv ROBOT ana)">
+ <rosparam command="delete" ns="/ana/move_base" />
+
+ <!--launch-prefix="xterm -e ddd -args" machine="visio"-->
+ <node pkg ="move_base"
+ type="move_base"
+ name="move_base"
+ output="screen">
+ <remap from="/ana/move_base_ana/cmd_vel"
+ to="/ana/navigation/cmd_vel" />
+ <remap from="/ana/move_base_ana/odom"
+ to="/ana/odom" />
+ <remap from="/ana/front_scan"
+ to="/ana/sensors/scan" />
+ <remap from="/ana/rear_scan"
+ to="/ana/sensors/scan" />
+ <remap from="/ana/tracks"
+ to="/ana/mht/tracks" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/move_base_params_aspsi.yaml" command="load" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/ana/costmap_common_params.yaml" command="load" ns="global_costmap" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/costmap_common_params.yaml" command="load" ns="local_costmap" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/local_costmap_params.yaml" command="load" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/ana/global_costmap_params.yaml" command="load" />
+ <rosparam file="$(find iri_robot_aspsi)/config/ana2/akp_local_planner_params.yaml" command="load" />
+<rosparam file="$(find iri_ana_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="ana"/>
+ <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="ana/base_link"/>
+ </node>
+ </group>
+
+ <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" />
+
+</launch>