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 761f5cb30a1fc3d3433676f03e26b6c43e980413..c3ed696d7c26076688d5c75c0df10a00b238c96a 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-ASUS-TUF-Gaming-F15-FX506LU-FX506LU,09.07.2021 14:25,file:///home/life/.config/libreoffice/4;
\ No newline at end of file
+,life,life-ASUS-TUF-Gaming-F15-FX506LU-FX506LU,12.07.2021 14:32,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 31789318fd02d57d8f73da7d6336a15babfe6991..247bd938f575a8ec1b25ac8758e1cae194929abd 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/local_lib_people_prediction/src/nav/plan_local_nav.cpp b/local_lib_people_prediction/src/nav/plan_local_nav.cpp
index 3e3fc1577b2ec9684e5577b5f2ad959a38116ba6..42dfbbe79f9c5bbcb2b001073804100dbcb6f496 100644
--- a/local_lib_people_prediction/src/nav/plan_local_nav.cpp
+++ b/local_lib_people_prediction/src/nav/plan_local_nav.cpp
@@ -479,12 +479,14 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
if(see_save_in_file_){
std::cout <<"ENTRO EN SIM, se cambia nombre FILE segun el caso de simulacion, el fichero del reconfigure."<< std::endl;
}
+ std::cout << "global_iter_to_change_simulation_case_"<< global_iter_to_change_simulation_case_<< std::endl;
- if(global_iter_to_change_simulation_case_<=100){ // 0-> 100
+ if(global_iter_to_change_simulation_case_<=3){ // 0-> 100
// change results file. Case 0
- if(debug_sideBySide2019_){
+ //if(debug_sideBySide2019_){
+
std::cout << "PPPP CASE 0; SIM_initial_person_goal_pose1_.x" << SIM_initial_person_goal_pose1_.x<<"; SIM_initial_person_goal_pose1_.y="<< SIM_initial_person_goal_pose1_.y<< std::endl;
- }
+ //}
std::string data_file="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/local_lib_ATR_7_junio/1_data_results_side_by_side/results_person_companion_side_by_side1.txt";
results_file_=data_file;
@@ -499,7 +501,7 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
actual_case_=Cplan_local_nav::case0;
- }else if((global_iter_to_change_simulation_case_>100)&&(global_iter_to_change_simulation_case_<=200)){ // 100 -> 200
+ }else if((global_iter_to_change_simulation_case_>3)&&(global_iter_to_change_simulation_case_<=6)){ // 100 -> 200
std::cout << "PPPP CASE 1; SIM_initial_person_goal_pose2_.x" << SIM_initial_person_goal_pose2_.x<<"; SIM_initial_person_goal_pose2_.y="<< SIM_initial_person_goal_pose2_.y<< std::endl;
// change case =1
if(first_time_case1_){
@@ -519,7 +521,7 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
// Use initial goal person2 and change the person goals case 1 in Cscene sim
// change results file. Case 1
- }else if((global_iter_to_change_simulation_case_>200)&&(global_iter_to_change_simulation_case_<=300)){ // 200 -> 300
+ }else if((global_iter_to_change_simulation_case_>6)&&(global_iter_to_change_simulation_case_<=9)){ // 200 -> 300
std::cout << "PPPP CASE 2; SIM_initial_person_goal_pose3_.x" << SIM_initial_person_goal_pose3_.x<<"; SIM_initial_person_goal_pose3_.y="<< SIM_initial_person_goal_pose3_.y<< std::endl;
// change case =2
if(first_time_case2_){
@@ -592,7 +594,7 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
double dist_person_companion_to_ini_goal;
SpointV_cov spoint_person_companion_act=actual_person_Companion_SpointV_;
dist_person_companion_to_ini_goal=SIM_initial_person_companion_pose2_.distance(spoint_person_companion_act);
- //std::cout <<"START SIM; (goal3) SIM_initial_robot_pose2_.x="<<SIM_initial_robot_pose2_.x<<"; SIM_initial_robot_pose2_.y="<<SIM_initial_robot_pose2_.y<< std::endl;
+ std::cout <<"START SIM; (goal3) SIM_initial_robot_pose2_.x="<<SIM_initial_robot_pose2_.x<<"; SIM_initial_robot_pose2_.y="<<SIM_initial_robot_pose2_.y<< std::endl;
if((robot_->get_current_pointV().distance(SIM_initial_robot_pose2_)<2.0)&&(dist_person_companion_to_ini_goal<1.0)){ // antes era 1.75
// OJO!!! con personas hay que esperar a que las otras lleguen a su goal!!!
@@ -656,11 +658,15 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
/*** SIMULACION-iterativa: gestionar el saltar al Inicio de la simulacion ***/
if(sim){
- if(debug_sideBySide2019_){
+ std::cout <<"sim="<<sim<<"first_in_itter_="<<first_in_itter_<<"Action_="<<Action_<< std::endl;
+ //if(debug_sideBySide2019_){
std::cout <<"ATR COMPANION robot_plan_companion3; robot_->get_current_pointV().distance(actual_dests->at(2)="<<robot_->get_current_pointV().distance(actual_dests->at(2))<< std::endl;
- }
- if(actual_dests->size()>3){
- if((robot_->get_current_pointV().distance(actual_dests->at(0))<4.5)||(robot_->get_current_pointV().distance(actual_dests->at(2))<3.5)||(robot_->get_current_pointV().distance(actual_dests->at(3))<3.5)){
+ std::cout <<"actual_dests->at(0).x="<<actual_dests->at(0).x<<"actual_dests->at(0).y="<<actual_dests->at(0).y<< std::endl;
+ std::cout <<"actual_dests->at(9).x="<<actual_dests->at(9).x<<"actual_dests->at(9).y="<<actual_dests->at(9).y<< std::endl;
+ std::cout <<"actual_dests->at(17).x="<<actual_dests->at(17).x<<"actual_dests->at(17).y="<<actual_dests->at(17).y<< std::endl;
+ //}
+ if(actual_dests->size()>18){
+ if((robot_->get_current_pointV().distance(actual_dests->at(0))<4.5)||(robot_->get_current_pointV().distance(actual_dests->at(9))<3.5)||(robot_->get_current_pointV().distance(actual_dests->at(17))<3.5)){
Action_=Cplan_local_nav::START;
if(first_in_itter_){ // cambia caso, nos vamos al inicio que toque!
global_iter_to_change_simulation_case_++;
diff --git a/local_lib_people_prediction/src/scene_sim.cpp b/local_lib_people_prediction/src/scene_sim.cpp
index d329ed508b8f88def0a50d90920ed6c2c47a6fbe..679eda0d502c4f71b8cd7b2da684fcf1e4ced6e6 100644
--- a/local_lib_people_prediction/src/scene_sim.cpp
+++ b/local_lib_people_prediction/src/scene_sim.cpp
@@ -279,7 +279,7 @@ void Cscene_sim::insert_more_random_people(const std::vector<SdetectionObservati
double now = observation[0].time_stamp;
std::list<Cperson_abstract*>::iterator iit;
//number_virtual_people_=40; // para fuera personitas id=5 + cambiar txt.destinaciones
- number_virtual_people_=20;
+ //number_virtual_people_=20;
if(debug_approach_people_random_){
std::cout << "(insert_more_random_people) number_virtual_people_= "<< number_virtual_people_ << std::endl;
@@ -712,7 +712,7 @@ bool Cscene_sim::update_scene_companion_simulation_akp_person_companion(const st
std::list<Cperson_abstract*>::iterator iit;
//number_virtual_people_=40;
- number_virtual_people_=20;
+ //number_virtual_people_=20;
// std::cout << "(Cscene_sim)2 update_scene_companion_simulation_akp_person_companion="<< std::endl;
// INI: update ides if are set the tracks of the other person companion.
@@ -2207,9 +2207,9 @@ if(!Scene_create_fake_fixed_second_person_companion_){
dest_1 = get_destinations()->at( 0 );
dest_2 = get_destinations()->at( 1 );
- //std::cout << " case 0: "<< std::endl;
- //std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
- //std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+ //std::cout << " (SIMULATED PERSON) case 0: "<< std::endl;
+ //std::cout << " (SIMULATED PERSON) dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+ //std::cout << " (SIMULATED PERSON) dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
Sdestination goal_person_dest(2, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
(*iit)->set_best_dest( goal_person_dest );
//std::cout << " [1] [ojo if id=1 valor seteado mal] (*iit)->id="<<(*iit)->get_id()<< std::endl;
@@ -2223,9 +2223,9 @@ if(!Scene_create_fake_fixed_second_person_companion_){
neighbours_ids_act.push_back(1);
dest_1 = get_destinations()->at( 15 );
dest_2 = get_destinations()->at( 16 );
- //std::cout << " case 1: "<< std::endl;
- //std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
- //std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+ //std::cout << "(SIMULATED PERSON) case 1: "<< std::endl;
+ //std::cout << "(SIMULATED PERSON) dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+ ///std::cout << "(SIMULATED PERSON) dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
Sdestination goal_person_dest(2, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
(*iit)->set_best_dest( goal_person_dest );
//std::cout << " [2] [ojo if id=1 valor seteado mal] (*iit)->id="<<(*iit)->get_id()<< std::endl;
@@ -2235,9 +2235,9 @@ if(!Scene_create_fake_fixed_second_person_companion_){
neighbours_ids_act.push_back(1);
dest_1 = get_destinations()->at( 17 );
dest_2 = get_destinations()->at( 18 );
- //std::cout << " case 2: "<< std::endl;
- //std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
- //std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+ //std::cout << "(SIMULATED PERSON) case 2: "<< std::endl;
+ //std::cout << "(SIMULATED PERSON) dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+ //std::cout << "(SIMULATED PERSON) dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
Sdestination goal_person_dest(2, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
(*iit)->set_best_dest( goal_person_dest );
//std::cout << " [3] [ojo if id=1 valor seteado mal] (*iit)->id="<<(*iit)->get_id()<< std::endl;
@@ -3218,7 +3218,7 @@ if(!Scene_create_fake_fixed_second_person_companion_){
akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_);
}
- std::cout << " PERSON COMPANION; d_min_="<<d_min_<<" personComp_->vel="<< akp_for_person_companion_.get_person_companion_akp()->get_desired_velocity()<< std::endl;
+ std::cout << " PERSON COMPANION (DABO); d_min_="<<d_min_<<" personComp_->vel="<< akp_for_person_companion_.get_person_companion_akp()->get_desired_velocity()<< std::endl;
// akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_);
// akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_);
/* }else{
@@ -3778,7 +3778,7 @@ if(!Scene_create_fake_fixed_second_person_companion_){
if(actual_sim_case_==Cplan_local_nav_person_companion::case0){
- std::cout << "(person companion) Cplan_local_nav::case0 destinations_[8].x"<<destinations_ini_security_copy[ 8 ].x<<"; destinations_[8].y"<<destinations_ini_security_copy[ 8 ].y << std::endl;
+ std::cout << "(person companion Dabo) Cplan_local_nav::case0 destinations_[8].x"<<destinations_ini_security_copy[ 8 ].x<<"; destinations_[8].y"<<destinations_ini_security_copy[ 8 ].y << std::endl;
if ( ((person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person)||(person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person_companion)) &&
person_companion_person_abstract_.get_current_pointV().distance( (Spoint)person_companion_person_abstract_.get_best_dest() ) < 1.5 )//destination achieved
@@ -3795,7 +3795,7 @@ if(!Scene_create_fake_fixed_second_person_companion_){
person_best_dest_[0]=person_actual_best_dest_;
}
}else if(actual_sim_case_==Cplan_local_nav_person_companion::case1){
- std::cout << "(person companion) Cplan_local_nav_person_companion::case1 destinations_[10].x"<<destinations_ini_security_copy[ 10 ].x<<"; destinations_[10].y"<<destinations_ini_security_copy[ 10 ].y << std::endl;
+ std::cout << "(person companion Dabo) Cplan_local_nav_person_companion::case1 destinations_[10].x"<<destinations_ini_security_copy[ 10 ].x<<"; destinations_[10].y"<<destinations_ini_security_copy[ 10 ].y << std::endl;
//std::cout << "(person companion) Cplan_local_nav_person_companion::case1; destinations_[10].x"<<destinations_[4].x<<"; destinations_[4].y"<<destinations_[4].y<< std::endl;
if ( ((person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person)||(person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person_companion)) &&
person_companion_person_abstract_.get_current_pointV().distance( (Spoint)person_companion_person_abstract_.get_best_dest() ) < 1.5 )//destination achieved
@@ -3812,7 +3812,7 @@ if(!Scene_create_fake_fixed_second_person_companion_){
person_best_dest_[0]=person_actual_best_dest_;
}
}else if(actual_sim_case_==Cplan_local_nav_person_companion::case2){
- std::cout << "(person companion) Cplan_local_nav_person_companion::case2 destinations_[12].x"<<destinations_ini_security_copy[ 12 ].x<<"; destinations_[12].y"<<destinations_ini_security_copy[ 12 ].y << std::endl;
+ std::cout << "(person companion Dabo) Cplan_local_nav_person_companion::case2 destinations_[12].x"<<destinations_ini_security_copy[ 12 ].x<<"; destinations_[12].y"<<destinations_ini_security_copy[ 12 ].y << std::endl;
if ( ((person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person)||(person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person_companion)) &&
person_companion_person_abstract_.get_current_pointV().distance( (Spoint)person_companion_person_abstract_.get_best_dest() ) < 1.5 )//destination achieved
{
diff --git a/local_lib_people_prediction/src/scene_sim.h b/local_lib_people_prediction/src/scene_sim.h
index 8fe0c84d4376820d84c75518f175b51f8b6fa1ce..e1e48554bb84a5820c10d0a9aec916a83b9bcb0e 100644
--- a/local_lib_people_prediction/src/scene_sim.h
+++ b/local_lib_people_prediction/src/scene_sim.h
@@ -264,7 +264,7 @@ class Cscene_sim : public Cscene_abstract
SpointV_cov SpointV_cov_fake_second_person_companion;
unsigned int get_id_fake_second_person_companion(){
- std::cout<<" get_id_fake_second_person_companion=> id_second_companion_= "<<id_second_companion_<< std::endl;
+ //std::cout<<" get_id_fake_second_person_companion=> id_second_companion_= "<<id_second_companion_<< std::endl;
return id_second_companion_;
}
diff --git a/maps/fme_open_destinationsATR_staySIM.txt b/maps/fme_open_destinationsATR_staySIM.txt
index 8c045f5901802c96f39c1622aaf1918a1a0cbc48..df19077e0a1f603a9c3ff4983a6c7222c8e8bd86 100755
--- a/maps/fme_open_destinationsATR_staySIM.txt
+++ b/maps/fme_open_destinationsATR_staySIM.txt
@@ -12,7 +12,7 @@
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
+13, 40.0,-20.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
diff --git a/maps/fme_open_destinationsATR_staySIMOTHERpeople_stop.txt b/maps/fme_open_destinationsATR_staySIMOTHERpeople_stop.txt
index 8ec98823cc7661ec4c0c130733689187663f0d57..21e09307c8b8c1c57a9a6288c54578db7751db58 100755
--- a/maps/fme_open_destinationsATR_staySIMOTHERpeople_stop.txt
+++ b/maps/fme_open_destinationsATR_staySIMOTHERpeople_stop.txt
@@ -12,11 +12,11 @@
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
+13, 40.0,-20.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
+17, 40.0, -20.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/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp
index 777e5f95038d9d04c72eff068ee7f09693e751f3..cdfe6c28366820ec0b0bbccd454c7b5d2780ced9 100644
--- a/src/akp_local_planner_alg_node.cpp
+++ b/src/akp_local_planner_alg_node.cpp
@@ -1852,7 +1852,7 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
simulation_=config.change_sim;
this->planner_.set_change_sim_target_per(config.change_sim_target_per);
-
+ this->planner_.set_change_sim(config.change_sim_target_per);
this->planner_.set_metros_al_dynamic_goal_Vform(config.distance_to_dynamic_goal_Vform);