diff --git a/cfg/AkpLocalPlanner.cfg b/cfg/AkpLocalPlanner.cfg
index f63df1c0ec9eeed6709aa90ee923470f02c67a9d..866c18247c0b22cad50d9e580835429d27bdbbca 100755
--- a/cfg/AkpLocalPlanner.cfg
+++ b/cfg/AkpLocalPlanner.cfg
@@ -56,6 +56,9 @@ gen.add("Block_parameters_division3" , str_t, 0, "Parameter
# visualization markers
gen.add("vis_mode", int_t, 0, "Visualization markers: #0 no markers, #1 min, #2 normal(2d), #3 super (3d), #4 SUPER!", 4, 0, 7)
+gen.add("Block_parameters_division50" , str_t, 0, " Iterative Simulation ", " Iterative Simulation ")
+gen.add("change_sim" , bool_t, 0, "FALSE FOR REAL; True for simulation", True)
+gen.add("change_sim_target_per" , bool_t, 0, "FALSE FOR REAL; True for simulation", True)
#### Parameters to adapt the robot the accompaniment, distance between people and angle of accompaniment ####
diff --git a/config/tibi/akp_local_planner_params.yaml b/config/tibi/akp_local_planner_params.yaml
index 52b966c67fc3881ec79a5863d9938864b288f026..758cf17abeb4c97392993ed4fd0e94bd8fc7b9c8 100644
--- a/config/tibi/akp_local_planner_params.yaml
+++ b/config/tibi/akp_local_planner_params.yaml
@@ -38,13 +38,13 @@ AkpLocalPlanner:
esfm_to_obstacle_lambda: 1.0
esfm_k: 2.3
esfm_d: 0.2
- min_v_to_predict : 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: 4.05
- esfm_to_person_companion_B: 0.61
+ 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
@@ -63,9 +63,16 @@ AkpLocalPlanner:
### Parameters of group model Francesco
##############
#
-# valores anteriores buenos side-by-side
+# 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.71
+# 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"
@@ -88,15 +95,25 @@ AkpLocalPlanner:
# esfm_to_robot_A: 6.05
# esfm_to_robot_B: 0.45
# esfm_to_robot_lambda: 0.25
-# anteriores SIMULACION (los de gonzalo. Pero los mios parece que van mejor.
+# 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/tibi/costmap_common_params.yaml b/config/tibi/costmap_common_params.yaml
index 11d5d721c631ca91a7895ed6f9fde07a5b95b3bd..d43282cc27f92d768fb73afce6c43acfb9d8419b 100755
--- a/config/tibi/costmap_common_params.yaml
+++ b/config/tibi/costmap_common_params.yaml
@@ -5,7 +5,7 @@ obstacle_layer:
observation_sources: front_laser rear_laser #fake
front_laser: {
- sensor_frame: /tibi/front_laser,
+ sensor_frame: tibi/front_laser,
data_type: LaserScan,
topic: /tibi/sensors/front_laser_scan,
observation_persistence: 0.0,
@@ -17,7 +17,7 @@ obstacle_layer:
}
rear_laser: {
- sensor_frame: /tibi/rear_laser,
+ sensor_frame: tibi/rear_laser,
data_type: LaserScan,
topic: /tibi/sensors/rear_laser_scan,
observation_persistence: 0.0,
@@ -29,7 +29,7 @@ obstacle_layer:
}
fake_laser: {
- sensor_frame: /fake_laser,
+ sensor_frame: fake_laser,
data_type: LaserScan,
topic: /fake_laser,
observation_persistence: 0.0,
diff --git a/config/tibi/global_costmap_params.yaml b/config/tibi/global_costmap_params.yaml
index d4022c6db81169f097d3d5d663851af4007a594e..5309990eadcc7f3dd24c337d18ba89141f9d7bdc 100755
--- a/config/tibi/global_costmap_params.yaml
+++ b/config/tibi/global_costmap_params.yaml
@@ -1,7 +1,7 @@
global_costmap:
publish_voxel_map: true
- global_frame: /map
- robot_base_frame: /tibi/base_footprint
+ global_frame: map
+ robot_base_frame: tibi/base_footprint
update_frequency: 0.2
publish_frequency: 0.2
static_map: true
diff --git a/config/tibi/local_costmap_params.yaml b/config/tibi/local_costmap_params.yaml
index 82ebe6ef4f8f6dcc558b7fde6e786f898a95a50e..17165f5f20f74480dc921255bccfdf769432029b 100755
--- a/config/tibi/local_costmap_params.yaml
+++ b/config/tibi/local_costmap_params.yaml
@@ -1,7 +1,7 @@
local_costmap:
publish_voxel_map: true
- global_frame: /tibi/odom
- robot_base_frame: /tibi/base_footprint
+ global_frame: tibi/odom
+ robot_base_frame: tibi/base_footprint
update_frequency: 1.0
publish_frequency: 1.0
static_map: false
diff --git a/docs/1_Install_instructions/instructions_for_IVO/.~lock.Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt# b/docs/1_Install_instructions/instructions_for_IVO/.~lock.Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt#
index cc9cd91c2258644f27995f810e75d6ead9c3f7a3..761f5cb30a1fc3d3433676f03e26b6c43e980413 100644
--- a/docs/1_Install_instructions/instructions_for_IVO/.~lock.Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt#
+++ b/docs/1_Install_instructions/instructions_for_IVO/.~lock.Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt#
@@ -1 +1 @@
-,life,life-GE63-Raider-RGB-8RE,23.06.2021 10:58,file:///home/life/.config/libreoffice/4;
\ No newline at end of file
+,life,life-ASUS-TUF-Gaming-F15-FX506LU-FX506LU,09.07.2021 14:25,file:///home/life/.config/libreoffice/4;
\ No newline at end of file
diff --git a/docs/1_Install_instructions/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt b/docs/1_Install_instructions/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt
index 18d0c03b5782345c85a7116eb23cbc01f8a5ec06..31789318fd02d57d8f73da7d6336a15babfe6991 100644
Binary files a/docs/1_Install_instructions/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt and b/docs/1_Install_instructions/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt differ
diff --git a/launch/launchs_iterative_simulation/internos/keyboard_teleop_ely.launch b/launch/launchs_iterative_simulation/internos/keyboard_teleop_ely.launch
new file mode 100755
index 0000000000000000000000000000000000000000..f11e9835222af2474a9b6b337b13b61b8343855a
--- /dev/null
+++ b/launch/launchs_iterative_simulation/internos/keyboard_teleop_ely.launch
@@ -0,0 +1,9 @@
+<launch>
+ <!-- turtlebot_teleop_key already has its own built in velocity smoother -->
+ <node pkg="turtlebot_teleop" type="teleop_v1" name="turtlebot_teleop_keyboard" output="screen">
+ <param name="scale_linear" value="0.5" type="double"/>
+ <param name="scale_angular" value="1.5" type="double"/>
+ <remap from="/turtlebot_teleop_keyboard/cmd_vel" to="/person1/cmd_vel"/>
+ <remap from="/turtlebot_teleop_keyboard/cmd_vel2" to="/person2/cmd_vel"/>
+ </node>
+</launch>
diff --git a/launch/launchs_iterative_simulation/internos/model.launch b/launch/launchs_iterative_simulation/internos/model.launch
new file mode 100755
index 0000000000000000000000000000000000000000..5ccff6bf83347fde6d86ab6739a4e092ac4c4011
--- /dev/null
+++ b/launch/launchs_iterative_simulation/internos/model.launch
@@ -0,0 +1,58 @@
+<!-- -->
+<launch>
+ <group ns="$(optenv ROBOT tibi)">
+
+ <!-- <arg name="model" default="$(optenv ROBOT tibi)"/> -->
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find tibi_dabo_description)/urdf/$(optenv ROBOT tibi).xacro'"/>
+ <node pkg ="robot_state_publisher"
+ type="state_publisher"
+ name="robot_state_publisher">
+ <param name="tf_prefix" value="$(optenv ROBOT tibi)" type="str" />
+ </node>
+ <arg name="x" default="0.0"/>
+ <arg name="y" default="0.0"/>
+ <arg name="yaw" default="0.0"/>
+ <arg name="model" default="$(optenv ROBOT tibi)"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="head_pan"
+ args="0.01722 0 0.025 0 0 0 /$(optenv ROBOT tibi)/neck1 /$(optenv ROBOT tibi)/neck2 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="head_tilt"
+ args="0 0 0.051 -1.5707963 0 -1.5707963 /$(optenv ROBOT tibi)/neck2 /$(optenv ROBOT tibi)/neck3 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="head_roll"
+ args="0 -0.045 0 -1.5707963 0 -1.5707963 /$(optenv ROBOT tibi)/neck3 /$(optenv ROBOT tibi)/head 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="left_arm_pan"
+ args="0.0 0.0 0.023 0.0 0.0 0.0 /$(optenv ROBOT tibi)/left_shoulder1 /$(optenv ROBOT tibi)/left_shoulder2 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="left_arm_tilt"
+ args="0.0 0.0 0.0435 0.0 -1.5707963 0.0 /$(optenv ROBOT tibi)/left_shoulder2 /$(optenv ROBOT tibi)/left_arm 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="right_arm_pan"
+ args="0.0 0.0 0.023 0.0 0.0 0.0 /$(optenv ROBOT tibi)/right_shoulder1 /$(optenv ROBOT tibi)/right_shoulder2 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="right_arm_tilt"
+ args="0.0 0.0 0.0435 0.0 -1.5707963 0.0 /$(optenv ROBOT tibi)/right_shoulder2 /$(optenv ROBOT tibi)/right_arm 100"/>
+
+ <node name="spawn_urdf_$(optenv ROBOT tibi)"
+ pkg ="gazebo_ros"
+ type="spawn_model"
+ args="-param /$(optenv ROBOT tibi)/robot_description -urdf -model $(optenv ROBOT tibi) -x $(arg x) -y $(arg y) -Y $(arg yaw)">
+ </node>
+ </group>
+</launch>
diff --git a/launch/launchs_iterative_simulation/internos/model2.launch b/launch/launchs_iterative_simulation/internos/model2.launch
new file mode 100755
index 0000000000000000000000000000000000000000..c21ecb40efc96e14d9ae4987c40b1e8427c432e4
--- /dev/null
+++ b/launch/launchs_iterative_simulation/internos/model2.launch
@@ -0,0 +1,58 @@
+<!-- -->
+<launch>
+ <group ns="dabo">
+
+ <!-- <arg name="model" default="$(optenv ROBOT dabo)"/> -->
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find tibi_dabo_description)/urdf/dabo.xacro'"/>
+ <node pkg ="robot_state_publisher"
+ type="state_publisher"
+ name="robot_state_publisher2">
+ <param name="tf_prefix" value="dabo" type="str" />
+ </node>
+ <arg name="x" default="0.0"/>
+ <arg name="y" default="0.0"/>
+ <arg name="yaw" default="0.0"/>
+ <arg name="model" default="dabo"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="head_pan2"
+ args="0.01722 0 0.025 0 0 0 /dabo/neck1 /dabo/neck2 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="head_tilt2"
+ args="0 0 0.051 -1.5707963 0 -1.5707963 /dabo/neck2 /dabo/neck3 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="head_roll2"
+ args="0 -0.045 0 -1.5707963 0 -1.5707963 /dabo/neck3 /dabo/head 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="left_arm_pan2"
+ args="0.0 0.0 0.023 0.0 0.0 0.0 /dabo/left_shoulder1 /dabo/left_shoulder2 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="left_arm_tilt2"
+ args="0.0 0.0 0.0435 0.0 -1.5707963 0.0 /dabo/left_shoulder2 /dabo/left_arm 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="right_arm_pan2"
+ args="0.0 0.0 0.023 0.0 0.0 0.0 /dabo/right_shoulder1 /dabo/right_shoulder2 100"/>
+
+ <node pkg ="tf"
+ type="static_transform_publisher"
+ name="right_arm_tilt2"
+ args="0.0 0.0 0.0435 0.0 -1.5707963 0.0 /dabo/right_shoulder2 /dabo/right_arm 100"/>
+
+ <node name="spawn_urdf_dabo"
+ pkg ="gazebo_ros"
+ type="spawn_model"
+ args="-param /dabo/robot_description -urdf -model dabo -x $(arg x) -y $(arg y) -Y $(arg yaw)">
+ </node>
+ </group>
+</launch>
diff --git a/launch/launchs_iterative_simulation/internos/simulation_test2.launch b/launch/launchs_iterative_simulation/internos/simulation_test2.launch
new file mode 100755
index 0000000000000000000000000000000000000000..ffad39ec50b1c6e66abd90b87f65ac80a775e0cb
--- /dev/null
+++ b/launch/launchs_iterative_simulation/internos/simulation_test2.launch
@@ -0,0 +1,44 @@
+<!-- -->
+<launch>
+
+ <include file="$(find iri_akp_local_planner_companion)/launch/model.launch" />
+
+<!-- linea de abajo, carga el modelo del segundo robot, el que acompanya, no el central de la trajectoria. -->
+<!-- <include file="$(find iri_akp_local_planner_companion)/launch/model_robot_company.launch" /> -->
+
+ <include file="$(find iri_akp_tools_companion)/launch/diff_platform_simulator.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/fake_laser_gen.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/fake_localization.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/map_server_local.launch">
+ <arg name="map" value="pasillo_simulation" /> <!-- 3 maps: empty, brl and fme_open , corridor -->
+ </include>
+ <include file="$(find iri_akp_local_planner_companion)/launch/tibi_dabo_akp_local_sim.launch">
+ <arg name="map" value="pasillo_simulation"/> <!-- 3 maps: empty, brl and fme_open -->
+ </include>
+
+ <include file="$(find iri_people_simulation_companion)/launch/people_simulation.launch">
+ <arg name="map" value="pasillo_simulation" />
+ </include>
+<!-- 3 maps: empty, brl and fme_open -->
+
+<!-- DABO -->
+ <include file="$(find iri_simulated_person_companion_akp_local_planner)/launch/model2.launch" />
+ <include file="$(find iri_akp_tools_companion)/launch/diff_platform_simulator2.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/fake_laser_gen2.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/fake_localization2.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/map_server_local2.launch">
+ <arg name="map" value="pasillo_simulation" /> <!-- 3 maps: empty, brl and fme_open , corridor -->
+ </include>
+ <include file="$(find iri_simulated_person_companion_akp_local_planner)/launch/tibi_dabo_akp_local_sim2.launch">
+ <arg name="map" value="pasillo_simulation"/> <!-- 3 maps: empty, brl and fme_open -->
+ </include>
+
+
+ <include file="$(find iri_people_simulation_companion)/launch/people_simulation2.launch">
+ <arg name="map" value="pasillo_simulation" />
+ </include>
+
+
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find iri_akp_local_planner_companion)/config/$(optenv ROBOT tibi).rviz" />
+
+</launch>
diff --git a/launch/launchs_iterative_simulation/internos/tibi_and_simulated_ElyTELEOP.launch b/launch/launchs_iterative_simulation/internos/tibi_and_simulated_ElyTELEOP.launch
new file mode 100755
index 0000000000000000000000000000000000000000..513ed9632daf68a5835084074bb2def06ef4ccf0
--- /dev/null
+++ b/launch/launchs_iterative_simulation/internos/tibi_and_simulated_ElyTELEOP.launch
@@ -0,0 +1,7 @@
+<!-- -->
+<launch>
+
+ <include file="$(find iri_akp_local_planner_companion)/launch/keyboard_teleop_ely.launch">
+ </include>
+
+</launch>
diff --git a/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim.launch b/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim.launch
new file mode 100755
index 0000000000000000000000000000000000000000..98b4d6fceefa7de25e756d44e44b755503a92cb9
--- /dev/null
+++ b/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim.launch
@@ -0,0 +1,60 @@
+<!-- -->
+<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)/segway/cmd_vel" />
+ <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/force_map_path" value="$(find iri_akp_local_planner)/maps/$(arg map)_forces.txt" />-->
+ <!-- maps: fme_open_destinations_sim_Vform; fme_open_destinations_brl_big; brl_master_big_destinations -->
+ <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/fme_open_destinationsATR_staySIM.txt" />
+ <!--<param name="AkpLocalPlanner/destination_map_path" value="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/bin/archives/destination_map_for_learning.txt" /> -->
+ <param name="AkpLocalPlanner/robot" value="$(optenv ROBOT tibi)"/>
+ <param name="AkpLocalPlanner/person_goal_id" value="200"/>
+ <param name="AkpLocalPlanner/speed_k" value="1.0"/> <!--MUY importante: en sim, speed_k=1.0; en robot real, speed_k=1.5; -->
+<param name="AkpLocalPlanner/speed_k_angular" value="1.0"/>
+ <param name="AkpLocalPlanner/robot" value="tibi"/>
+<param name="AkpLocalPlanner/frame_robot_footprint" value="tibi/base_link"/>
+ <param name="AkpLocalPlanner/frame_map" value="map"/>
+ <param name="AkpLocalPlanner/change_sim" value="True"/>
+ <param name="AkpLocalPlanner/sim_target_per" value="True"/>
+ <param name="AkpLocalPlanner/conf_id_to_remove" value="100000"/>
+ <param name="AkpLocalPlanner/conf_low_range_scan_detect_obstacles" value="0.0"/>
+<param name="AkpLocalPlanner/number_vertex" value="200.0"/>
+<param name="AkpLocalPlanner/conf_save_in_file_SideAndVform" value="True"/>
+ </node>
+
+ </group>
+
+</launch>
diff --git a/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim2.launch b/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim2.launch
new file mode 100755
index 0000000000000000000000000000000000000000..ba7a61a1b2055b3453fb6bda62b8278bb6dd8353
--- /dev/null
+++ b/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim2.launch
@@ -0,0 +1,38 @@
+<!-- -->
+<launch>
+
+ <arg name="map" default="fme_open2" />
+
+ <rosparam command="delete" ns="/$(optenv ROBOT dabo)/move_base2" />
+
+ <group ns="$(optenv ROBOT dabo)">
+
+ <!--launch-prefix="xterm -e ddd -args" -->
+ <!--output="screen"-->
+ <node pkg ="move_base"
+ type="move_base"
+ name="move_base2">
+ <remap from="/$(optenv ROBOT dabo)/cmd_vel"
+ to="/$(optenv ROBOT dabo)/segway/cmd_vel" />
+ <remap from="/$(optenv ROBOT dabo)/odom"
+ to="/$(optenv ROBOT dabo)/segway/odom" />
+ <remap from="/$(optenv ROBOT dabo)/front_scan"
+ to="/$(optenv ROBOT dabo)/sensors/front_laser_scan" />
+ <remap from="/$(optenv ROBOT dabo)/rear_scan"
+ to="/$(optenv ROBOT dabo)/sensors/rear_laser_scan" />
+ <remap from="/$(optenv ROBOT dabo)/tracks"
+ to="/$(optenv ROBOT dabo)/mht/tracks" />
+ <rosparam file="$(find iri_akp_local_planner_companion)/config/$(optenv ROBOT dabo)/move_base_params.yaml" command="load" />
+ <rosparam file="$(find iri_akp_local_planner_companion)/config/$(optenv ROBOT dabo)/costmap_common_params.yaml" command="load" ns="global_costmap" />
+ <rosparam file="$(find iri_akp_local_planner_companion)/config/$(optenv ROBOT dabo)/costmap_common_params.yaml" command="load" ns="local_costmap" />
+ <rosparam file="$(find iri_akp_local_planner_companion)/config/$(optenv ROBOT dabo)/local_costmap_params.yaml" command="load" />
+ <rosparam file="$(find iri_akp_local_planner_companion)/config/$(optenv ROBOT dabo)/global_costmap_params.yaml" command="load" />
+ <rosparam file="$(find iri_akp_local_planner_companion)/config/$(optenv ROBOT dabo)/akp_local_planner_params.yaml" command="load" />
+ <!--<param name="AkpLocalPlanner/force_map_path" value="$(find iri_akp_local_planner)/maps/$(arg map)_forces.txt" />-->
+ <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_akp_local_planner_companion)/maps/fme_open_destinations_sim.txt" />
+ <param name="AkpLocalPlanner/robot" value="$(optenv ROBOT dabo)"/>
+ </node>
+
+ </group>
+
+</launch>
diff --git a/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim2_v3.launch b/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim2_v3.launch
new file mode 100755
index 0000000000000000000000000000000000000000..802a4891f09ce9f65d0c5633c3a492078c4d569e
--- /dev/null
+++ b/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim2_v3.launch
@@ -0,0 +1,79 @@
+<!-- -->
+<launch>
+
+ <arg name="map" default="empty" />
+
+ <rosparam command="delete" ns="/$(optenv ROBOT dabo)/move_base" />
+
+ <group ns="$(optenv ROBOT dabo)">
+
+ <!--launch-prefix="xterm -e ddd -args" -->
+ <!--output="screen"-->
+ <node pkg ="move_base"
+ type="move_base"
+ name="move_base2"
+ >
+ <param name="number_persons" value="1"/>
+ <param name="vis_mode" value="True"/>
+ <param name="destination_map_path" value="$(find iri_simulated_person_companion_akp_local_planner)/map/fme_open_destinations.txt" />
+ <remap from="/$(optenv ROBOT dabo)/cmd_vel"
+ to="/$(optenv ROBOT dabo)/segway/cmd_vel" />
+ <remap from="/$(optenv ROBOT dabo)/odom"
+ to="/$(optenv ROBOT dabo)/segway/odom" />
+ <remap from="/$(optenv ROBOT dabo)/odom_other_person_of_group"
+ to="/$(optenv ROBOT dabo)/segway/odom" />
+ <remap from="/$(optenv ROBOT dabo)/odom_tibi"
+ to="/$(optenv ROBOT tibi)/segway/odom" />
+ <remap from="/$(optenv ROBOT dabo)/front_scan"
+ to="/$(optenv ROBOT dabo)/sensors/front_laser_scan" />
+ <remap from="/$(optenv ROBOT dabo)/rear_scan"
+ to="/$(optenv ROBOT dabo)/sensors/rear_laser_scan" />
+ <!-- <remap from="/$(optenv ROBOT dabo)/tracks"
+ to="/fusion_tracks_people_sim/dabo1_tracks" />-->
+ <remap from="/$(optenv ROBOT dabo)/tracks"
+ to="/$(optenv ROBOT tibi)/mht/tracks" />
+ <!--<remap from="~tracksMarkers" to="/$(optenv ROBOT tibi)/mht/tracksMarkers"/>-->
+ <remap from="/$(optenv ROBOT dabo)/cmd_vel_stop"
+ to="/turtlebot_teleop_keyboard/cmd_vel2"/>
+ <remap from="/$(optenv ROBOT dabo)/status_init"
+ to="/tibi/status_init"/>
+ <remap from="/$(optenv ROBOT dabo)/init_simulations"
+ to="/tibi/init_simulations"/>
+ <!-- remap for other person_companion tracks:-->
+ <!-- <remap from="/$(optenv ROBOT tibi)/tracks"
+ to="/$(optenv ROBOT dabo)/tracks"/> -->
+ <remap from="/$(optenv ROBOT dabo)/other_person_destination_tracks"
+ to="/$(optenv ROBOT dabo2)/destinations_of_tracks"/>
+ <remap from="/$(optenv ROBOT dabo)/dabo_pose_for_sim"
+ to="/$(optenv ROBOT dabo)/initialpose"/>
+ <!--<remap from="/$(optenv ROBOT dabo)/mht/tracks" to="/$(optenv ROBOT tibi)/mht/tracks"/> -->
+ <rosparam file="$(find iri_simulated_person_companion_akp_local_planner)/config/$(optenv ROBOT dabo)/move_base_params.yaml" command="load" />
+ <rosparam file="$(find iri_simulated_person_companion_akp_local_planner)/config/$(optenv ROBOT dabo)/costmap_common_params.yaml" command="load" ns="global_costmap" />
+ <rosparam file="$(find iri_simulated_person_companion_akp_local_planner)/config/$(optenv ROBOT dabo)/costmap_common_params.yaml" command="load" ns="local_costmap" />
+ <rosparam file="$(find iri_simulated_person_companion_akp_local_planner)/config/$(optenv ROBOT dabo)/local_costmap_params.yaml" command="load" />
+ <rosparam file="$(find iri_simulated_person_companion_akp_local_planner)/config/$(optenv ROBOT dabo)/global_costmap_params.yaml" command="load" />
+ <rosparam file="$(find iri_simulated_person_companion_akp_local_planner)/config/$(optenv ROBOT dabo)/akp_local_planner_params.yaml" command="load" />
+ <!--<param name="AkpLocalPlanner/force_map_path" value="$(find iri_akp_local_planner)/maps/$(arg map)_forces.txt" />-->
+ <param name="AkpLocalPlanner/conf_bool_group_go_to_interact_with_other_person" value="False"/>
+ <param name="AkpLocalPlanner/person_goal_id" value="200"/>
+ <param name="AkpLocalPlanner/robot_sim_comp_pers_name" value="$(optenv ROBOT dabo)"/>
+ <param name="AkpLocalPlanner/other_person_of_group_comp_sim_name" value="$(optenv ROBOT dabo2)"/>
+ <param name="AkpLocalPlanner/id_person_companion" value="1"/>
+ <param name="AkpLocalPlanner/id_person_other_person_of_group_companion" value="3"/>
+ <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_simulated_person_companion_akp_local_planner)/maps/fme_open_destinationsATR_staySIMOTHERpeople_stop.txt" />
+
+ <!-- destination maps, congres humanoids sim: fme_open_destinations_simPerson_Vform_caseSideBySide_humanoids_SIDE_left.txt; fme_open_destinations_simPerson_Vform_caseSideBySide_humanoids_SIDE_right.txt; fme_open_destinations_simPerson_Vform_caseSideBySide_humanoids_SIDE_both.txt;
+fme_open_destinations_simPerson_Vform_caseSideBySide_humanoids_SIDE_all_directions.txt; fme_open_destinations_simPerson_Vform_caseSideBySide_humanoids_SIDE_diagonals.txt-->
+ <!-- maps= fme_open_destinationsATR_staySIM.txt; fme_open_destinationsATR_staySIMOTHERpeople_stop.txt (For people stop); person_passages: (V-form) fme_open_destinationsATR_staySIM_pasage_robot.txt
+ mapa bueno, people en lateral: fme_open_destinationsATR_staySIM_pasage_robot_SIDE.txt lo iba cambiando por side_left y etc.
+
+fme_open_destinations_simPerson_Vform_caseSideBySide_humanoids_SIDE_all_directions.txt-->
+ <param name="AkpLocalPlanner/robot" value="$(optenv ROBOT dabo)"/>
+ <param name="AkpLocalPlanner/max_vel_comp_pers" value="0.5"/>
+ <param name="AkpLocalPlanner/id_second_companion" value="3"/>
+ <!-- TODO: correr la simulacion con personas, pero con menos velocidad de estas personas Vp=0.5 -->
+ </node>
+
+ </group>
+
+</launch>
diff --git a/launch/launchs_iterative_simulation/simulation_test3_ATR_Vform_ok.launch b/launch/launchs_iterative_simulation/simulation_test3_ATR_Vform_ok.launch
new file mode 100755
index 0000000000000000000000000000000000000000..2f3b91e57cdcd6e775d8c15936395245ffa535d9
--- /dev/null
+++ b/launch/launchs_iterative_simulation/simulation_test3_ATR_Vform_ok.launch
@@ -0,0 +1,78 @@
+<!-- -->
+<launch>
+
+<!-- 3 maps: empty, brl and fme_open , corridor, pasillo_simulation , empty_with_obstacles -->
+<!-- 3 maps: empty, brl and fme_open -->
+<!-- 3 maps: empty, brl and fme_open -->
+
+<!-- DABO iri_simulated_person_companion_akp_local_planner-->
+
+
+ <include file="$(find iri_people_simulation_assaop)/launch/model2.launch" />
+ <include file="$(find iri_akp_tools_companion)/launch/diff_platform_simulator2.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/fake_laser_gen2.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/fake_localization2.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/map_server_local2.launch">
+ <arg name="map" value="empty" /> <!--before_map: empty_with_obstacles2 -->
+ </include>
+ <include file="$(find iri_people_simulation_assaop)/launch/sim_simulador_akp_for_Vform_and_new_side_by_side/tibi_dabo_akp_local_sim2_v3.launch">
+ <arg name="map" value="empty"/>
+ </include> <!--before_map: empty_with_obstacles2 -->
+ <!-- 3 maps: empty, brl and fme_open , corridor -->
+<!-- 3 maps: empty, brl and fme_open -->
+<!--<include file="$(find iri_people_simulation_companion)/launch/people_simulation2.launch">
+ <arg name="map" value="pasillo_simulation" />
+ </include> -->
+
+<!-- DABO 2 -->
+<!--before_map: empty_with_obstacles2 -->
+<!--
+ <include file="$(find iri_simulated_person_companion_akp_local_planner)/launch/model3.launch" />
+ <include file="$(find iri_akp_tools_companion)/launch/diff_platform_simulator3.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/fake_laser_gen3.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/fake_localization3.launch"/>
+ <include file="$(find iri_akp_tools_companion)/launch/map_server_local3_atr.launch">
+ <arg name="map" value="empty" />
+ </include>
+ <include file="$(find iri_simulated_person_companion_akp_local_planner)/launch/tibi_dabo_akp_local_sim3.launch">
+ <arg name="map" value="empty"/>
+ </include>
+ -->
+
+<!-- Fusion tracks 2 simulated robots -->
+<!-- <include file="$(find iri_simulated_person_companion_akp_local_planner)/launch/fusion_tracks_2_simul_people_dabos.launch" />-->
+
+<!-- TIBI -->
+ <include file="$(find iri_robot_aspsi)/launch/launchs_iterative_simulation/internos/model.launch" />
+<!-- linea de abajo, carga el modelo del segundo robot, el que acompanya, no el central de la trajectoria. -->
+<!-- <include file="$(find iri_atr_akp_local_planner_companion)/launch/model_robot_company.launch" /> -->
+ <include file="$(find iri_akp_tools_companion)/launch/diff_platform_simulator.launch"/>
+
+ <include file="$(find iri_akp_tools_companion)/launch/fake_laser_gen.launch"/>
+
+ <include file="$(find iri_akp_tools_companion)/launch/fake_localization.launch"/>
+
+ <include file="$(find iri_akp_tools_companion)/launch/map_server_local_atr.launch">
+ <arg name="map" value="empty" /> <!--before_map: empty_with_obstacles2 -->
+ </include>
+
+
+ <include file="$(find iri_robot_aspsi)/launch/launchs_iterative_simulation/internos/tibi_dabo_akp_local_sim.launch">
+ <arg name="map" value="empty"/> <!--before_map: empty_with_obstacles2 -->
+ </include>
+
+ <!--<include file="$(find iri_people_simulation_companion)/launch/people_simulation.launch">
+ <arg name="map" value="pasillo_simulation" />
+ </include>
+
+MAPA ultimo que uso para obstaculos centrales:
+(bueno!!!) => empty_with_obstacles2
+
+ -->
+
+
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find iri_atr_akp_local_planner_companion)/config/$(optenv ROBOT dabo).rviz" />
+
+ <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" />
+
+</launch>
diff --git a/maps/fme_open_destinationsATR_staySIM.txt b/maps/fme_open_destinationsATR_staySIM.txt
new file mode 100755
index 0000000000000000000000000000000000000000..8c045f5901802c96f39c1622aaf1918a1a0cbc48
--- /dev/null
+++ b/maps/fme_open_destinationsATR_staySIM.txt
@@ -0,0 +1,27 @@
+20
+0, 40.0, 20.5, 0.25, 1, 1
+1, 1.0, -20.5, 0.25, 1, 0
+2, 20.0, 15.0, 0.25, 5, 3,4,5,6,7
+3, 7.0, 15.0, 0.25, 5, 2,4,5,6,7
+4, 20.0, -15.0, 0.25, 5, 2,3,5,6,7
+5, 7.0, -15.0, 0.25, 5, 2,3,4,6,7
+6, 7.0, 4.0, 0.25, 5, 2,3,4,5,7
+7, 20.0, 4.0, 0.25, 5, 2,3,4,5,6
+8, -2.0, 0, 0.25, 1, 9
+9, 40.0, 0.5, 0.25, 1, 8
+10, -2.0, 0, 0.25, 1, 11
+11, 40.0,20.5, 0.25, 1, 10
+12, -2.0, 0, 0.25, 1, 13
+13, 40.0,-2.5, 0.25, 1, 12
+14, -2.0, 0, 0.25, 1, 8
+15, 40.0, -2.5, 0.25, 1, 1
+16, 1.0, 20.5, 0.25, 1, 0
+17, 40.0, -2.5, 0.25, 1, 1
+18, 1.0, 0.0, 0.25, 1, 0
+19, -4.0, 0.5, 0.25, 1, 19
+
+
+
+
+
+
diff --git a/maps/fme_open_destinationsATR_staySIMOTHERpeople_stop.txt b/maps/fme_open_destinationsATR_staySIMOTHERpeople_stop.txt
new file mode 100755
index 0000000000000000000000000000000000000000..8ec98823cc7661ec4c0c130733689187663f0d57
--- /dev/null
+++ b/maps/fme_open_destinationsATR_staySIMOTHERpeople_stop.txt
@@ -0,0 +1,27 @@
+20
+0, 40.0, 20.5, 0.25, 1, 1
+1, 1.0, -20.5, 0.25, 1, 0
+2, -18.0, -20.5, 0.25, 5, 3,4,5,6,7
+3, -10.0, -20.5, 0.25, 5, 2,4,5,6,7
+4, -18.0, -20.5, 0.25, 5, 2,3,5,6,7
+5, -10.0, -20.5, 0.25, 5, 2,3,4,6,7
+6, -18.0, -20.5, 0.25, 5, 2,3,4,5,7
+7, -18.0, -20.5, 0.25, 5, 2,3,4,5,6
+8, -2.0, -2.0, 0.25, 1, 9
+9, 40.0, 0.5, 0.25, 1, 8
+10, -2.0, 0, 0.25, 1, 11
+11, 40.0,20.5, 0.25, 1, 10
+12, -2.0, 0, 0.25, 1, 13
+13, 40.0,-2.5, 0.25, 1, 12
+14, -2.0, -2, 0.25, 1, 8
+15, 40.0, -2.5, 0.25, 1, 1
+16, -2.0, -1.0, 0.25, 1, 0
+17, 40.0, -2.5, 0.25, 1, 1
+18, -2.0, -1.0, 0.25, 1, 0
+19, -2.0, -1.0, 0.25, 1, 19
+
+
+
+
+
+
diff --git a/maps/fme_open_destinations_sim_Vform.txt b/maps/fme_open_destinations_sim_Vform.txt
new file mode 100755
index 0000000000000000000000000000000000000000..7c6d0d85c1cbe1b65db386aa88716a7088820b3e
--- /dev/null
+++ b/maps/fme_open_destinations_sim_Vform.txt
@@ -0,0 +1,27 @@
+20
+0, 27.0, 20.5, 0.25, 1, 1
+1, 1.0, -20.5, 0.25, 1, 0
+2, 20.0, 7.0, 0.25, 5, 3,4,5,6,7
+3, 7.0, 7.0, 0.25, 5, 2,4,5,6,7
+4, 20.0, -7.0, 0.25, 5, 2,3,5,6,7
+5, 7.0, -7.0, 0.25, 5, 2,3,4,6,7
+6, 7.0, -7.0, 0.25, 5, 2,3,4,5,7
+7, 20.0, -7.0, 0.25, 5, 2,3,4,5,6
+8, -2.0, 0, 0.25, 1, 9
+9, 27.0, 0, 0.25, 1, 8
+10, -2.0, 0, 0.25, 1, 11
+11, 27.0,20.5, 0.25, 1, 10
+12, -2.0, 0, 0.25, 1, 13
+13, 10.0,-20.5, 0.25, 1, 12
+14, -2.0, 0, 0.25, 1, 8
+15, 27.0,-15.5, 0.25, 1, 1
+16, 1.0, 20.5, 0.25, 1, 0
+17, 27.0, 0.0, 0.25, 1, 1
+18, 1.0, 0.0, 0.25, 1, 0
+19, -4.0, 0.5, 0.25, 1, 19
+
+
+
+
+
+
diff --git a/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp
index ca695222d9cf7ff07a04b8c2b9f3be19ab0ca753..777e5f95038d9d04c72eff068ee7f09693e751f3 100644
--- a/src/akp_local_planner_alg_node.cpp
+++ b/src/akp_local_planner_alg_node.cpp
@@ -1850,11 +1850,11 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
this->planner_.set_change_dynamic_of_final_dest(config.in_change_dynamically_final_dest);
+ simulation_=config.change_sim;
+ this->planner_.set_change_sim_target_per(config.change_sim_target_per);
-
-
this->planner_.set_metros_al_dynamic_goal_Vform(config.distance_to_dynamic_goal_Vform);
//std::cout << " OUT!!!! AkpLocalPlanner::reconfigureCallback this->move_base ="<< this->move_base << std::endl;