diff --git a/iri_robot_aspsi_how_to/instructions_dabo/Install_instructions_Dabo_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt b/iri_robot_aspsi_how_to/instructions_dabo/Install_instructions_Dabo_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt
index 33c87890552305e9af3497c1cea32c7e064ddf95..d6abd12faac2ea229d7702602ff2cf29df3b336c 100644
Binary files a/iri_robot_aspsi_how_to/instructions_dabo/Install_instructions_Dabo_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt and b/iri_robot_aspsi_how_to/instructions_dabo/Install_instructions_Dabo_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt differ
diff --git a/iri_robot_aspsi_how_to/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt b/iri_robot_aspsi_how_to/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt
index ca594e4c1b0afaeff321058cbc53cf8500b0fa03..6f94489f825f4766a211e80acd3161ff34877fac 100644
Binary files a/iri_robot_aspsi_how_to/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt and b/iri_robot_aspsi_how_to/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 8fb07a814a224c41c0c19e024905f2c334cca498..d6645cf4f217e0688dfa8c60efe88dc4b82cafad 100644
--- a/local_lib_people_prediction/src/nav/plan_local_nav.cpp
+++ b/local_lib_people_prediction/src/nav/plan_local_nav.cpp
@@ -1076,7 +1076,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
//clock_t pred_peopl_start, pred_peopl_end;
//pred_peopl_start=clock();
-/** Scene_prediction para todas las personas pero sin la persona que acompaña (Las colisiones con el acompañante son diferentes ***/
+/** Scene_prediction prediccion para todas las personas pero sin la persona que acompaña (Las colisiones con el acompañante son diferentes ***/
this->Cprediction_behavior::scene_prediction(); // scene prediction, needed, pero sin la person cmpanion y hará falta el robot en colisiones.
/*** get the angle of compnaion with the prediction of the person before scene_prediction (angulos de acompañamiento segun colisiones con otras personas)***/
@@ -1140,7 +1140,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
//std::cout << "ANTES init_robot_plan2!!!! "<< std::endl;
//clock_t init_robot_plan2_start, init_robot_plan2_end;
//init_robot_plan2_start=clock();
-/*** Inicialización del plan: calcula goal dynamico en mi¡ovimiento y cuando persona parada (hay dos casos: Akp=en movimiento y Stop=cuando persona parada (luego dentro digo nombres exactos de los casos que hay)***/
+/*** Inicialización del plan: calcula goal dynamico en mi movimiento y cuando persona parada (hay dos casos: Akp=en movimiento y Stop=cuando persona parada (luego dentro digo nombres exactos de los casos que hay)***/
init_robot_plan2(robot_person_distance);
//init_robot_plan2_end=clock();
//std::cout << " DESPUES init_robot_plan2"<< std::endl;
@@ -3290,7 +3290,7 @@ unsigned int Cplan_local_nav::find_nearest_vertex( const Spoint& random_goal )
}
-/*** funcion calcula propagacion plan. hay dos casos:
+/*** funcion calcula las fuerzas para la propagacion de la posicion del robot, plan en cada step. hay dos casos:
* 1-robot navega solo hasta el lateral de la persona. Antiguo akp
* 2- robot navega acompañando a la persona (usa tambien fuerza companion para propagar el trocito de la rama del path) ***/
Sedge_tree Cplan_local_nav::calculate_edge( unsigned int parent_vertex, const Sdestination& random_goal, Cperson_abstract::companion_reactive reactive )
@@ -3495,7 +3495,7 @@ Sedge_tree Cplan_local_nav::calculate_edge( unsigned int parent_vertex, const Sd
}
-/*** propagar el vertex con las fuerzas claculadas para el caso en que el robot navega solo (sin companion)***/
+/*** propagar el vertex con las fuerzas claculadas para el caso en que el robot navega solo (sin companion). Las fuerzas se calculaban en calculate_edge***/
bool Cplan_local_nav::propagate_vertex( unsigned int parent_index , const Sedge_tree& u, Cperson_abstract::companion_reactive reactive)
{
//robot_->get_current_pose().theta
@@ -3885,8 +3885,9 @@ double Cplan_local_nav::cost_to_go( const Spoint& random_goal, unsigned int i ,
}
/*** HEREDADA directa de Gonzalo, pero incluyendo el coste companion
- * ES la que USA de verdad el codigo para calcular el minimo coste de acompañamiento.
- * Hay X-modos de calcularlo, pero en realidad siempre solo se usa uno, el k vio Gonzalo que iba mejor.***/
+ * ES la que USA de verdad el codigo para calcular el minimo coste de acompañamiento (seleccionar ese path con minimo coste).
+ * Hay X-modos de calcularlo, pero en realidad siempre solo se usa uno, el k vio Gonzalo que iba mejor. Esta remarcado DENTRO***/
+/*** obtiene el indice final del path con menos coste (para asà saber que path es el de menos coste, que será seleccionado como best path.)***/
unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson_abstract::companion_reactive reactive )
{
//std::cout<<" INI function: global_min_cost_index()" << std::endl;
diff --git a/local_lib_people_prediction/src/prediction_bhmip.cpp b/local_lib_people_prediction/src/prediction_bhmip.cpp
index d47db656e5e0f80293b62815dc84a5282c23c6ae..f984edc916a74f9789f5a2d90e84f17450e454d6 100644
--- a/local_lib_people_prediction/src/prediction_bhmip.cpp
+++ b/local_lib_people_prediction/src/prediction_bhmip.cpp
@@ -53,6 +53,7 @@ void Cprediction_bhmip::see_tracks_on_scene()
}
/*** esta explicada en el nodo. basicamente es la funcion principal que incluye las nuevas posiciones a las personas y dentro hay otras de otros códigos que hacen cosas más concretas.***/
+/*** update/prediccion de las personas usando el bhmip metodo. ***/
void Cprediction_bhmip::update_scene(const std::vector<SdetectionObservation>& observation,bool& we_have_person_companion, bool person_or_robot)
{
// the filter bool sets if we want to filter velocities or use the provided in the update.
diff --git a/local_lib_people_prediction/src/scene_abstract.cpp b/local_lib_people_prediction/src/scene_abstract.cpp
index b3b6912126f4a6901a8a77570b072bcb3267ee49..94bc8d4ab2646069c46fc0db686d06037541e2a2 100644
--- a/local_lib_people_prediction/src/scene_abstract.cpp
+++ b/local_lib_people_prediction/src/scene_abstract.cpp
@@ -64,7 +64,7 @@ Cscene_abstract::~Cscene_abstract()
{
}
-
+/*** incluir destinaciones estaticas del entorno para la escena.***/
void Cscene_abstract::set_destinations( std::vector<Sdestination>& dest )
{
destinations_ = dest;
@@ -74,7 +74,7 @@ void Cscene_abstract::set_destinations( std::vector<Sdestination>& dest )
}
// robot destinations are not required, are set if planned
}
-
+/*** añadir una nueva persona a la lista de personas. person_list***/
Cperson_abstract* Cscene_abstract::add_person( unsigned int id )
{
std::list<Cperson_abstract*>::iterator iit = person_list_.begin();
@@ -92,7 +92,7 @@ Cperson_abstract* Cscene_abstract::add_person( unsigned int id )
return add_person_container(id,iit);//virtual function adding the specific person container
}
-
+/*** update the la prediccion de todas las personas menos la accompañante.***/
void Cscene_abstract::update_other_person_companion( unsigned int id, SpointV_cov in_other_person_companion_point )
{
/*std::list<Cperson_abstract*>::iterator iit = person_list_.begin();
@@ -122,7 +122,7 @@ void Cscene_abstract::update_other_person_companion( unsigned int id, SpointV_co
//return add_person_container(id,iit);//virtual function adding the specific person container
}
-
+/*** eliminar una persona de la lista de personas. person_list***/
void Cscene_abstract::remove_person( unsigned int id )
{
std::list<Cperson_abstract*>::iterator iit;
@@ -135,7 +135,7 @@ void Cscene_abstract::remove_person( unsigned int id )
}
}
-/*** update de la posicion del robot para esta iteracion. Explicada en el nodo.***/
+/*** update de la posicion del robot para esta iteracion (detectada por la odometria). Explicada en el nodo.***/
void Cscene_abstract::update_robot(Spose observation, bool case_zanlungo)
{
@@ -288,7 +288,7 @@ bool Cscene_abstract::find_person(unsigned int id , Cperson_abstract** person,
return false;
}
-
+/*** printar la lista de personas.***/
void Cscene_abstract::print()
{
std::cout << "Cscene_abstract::print() person_list_.empty="<<person_list_.empty()<< std::endl;
@@ -496,7 +496,7 @@ void Cscene_abstract::read_laser_scan_person_companion_akp_planner( const std::v
}
-
+/*** leer los scans del dobot para obtener los obstaculos estaticos de la escena (markers=cilindros negros obstaculos).***/
void Cscene_abstract::read_laser_scan_companion( const std::vector<Spoint>& laser_scan , bool person_simulation)
{
@@ -565,7 +565,7 @@ void Cscene_abstract::read_laser_scan_companion( const std::vector<Spoint>& las
read_laser_obstacle_success_ = true;//flag to indicate that indeed we can make use of laser information regarding obstacles.
}
-
+/*** to do not use these points to compute the obstacles in the laser scan. ***/
bool Cscene_abstract::point_corresponds_person( Spoint laser_point )
{
for( Cperson_abstract* iit : person_list_ )
@@ -589,7 +589,7 @@ bool Cscene_abstract::point_corresponds_person( Spoint laser_point )
return false;
}
-
+/*** to do not use these points to compute the obstacles in the laser scan. (these points are from the thrack of the person companion) ***/
bool Cscene_abstract::point_corresponds_person_companion( Spoint laser_point )
{
@@ -615,7 +615,7 @@ bool Cscene_abstract::point_corresponds_person_companion( Spoint laser_point )
}
-
+/*** to use these points to compute the obstacles in the laser scan. ***/
bool Cscene_abstract::point_belongs_to_laser_obstacle( Spoint p, bool person_simulation)
{
@@ -692,7 +692,7 @@ bool Cscene_abstract::point_belongs_to_laser_obstacle( Spoint p, bool person_sim
}
return false;
}
-
+/*** calcular fuerzas de los obstaculos del entorno***/
Sforce Cscene_abstract::force_objects_laser_int( Cperson_abstract* person)
{
Sforce force_res;
@@ -707,7 +707,7 @@ Sforce Cscene_abstract::force_objects_laser_int( Cperson_abstract* person)
return force_res;
}
-
+/*** calcular fuerzas de las personas del entorno***/
Sforce Cscene_abstract::force_persons_int( Cperson_abstract* center , bool if_robot_is_person_companion )
{
//std::cout << " (1) !!! force_persons_int" << std::endl;
@@ -751,7 +751,7 @@ Sforce Cscene_abstract::force_persons_int( Cperson_abstract* center , bool if_ro
return force_res;
}
-
+/*** calcular fuerzas de la accompanied person del entorno***/
Sforce Cscene_abstract::force_persons_int_companion( Cperson_abstract* center , unsigned int id_person_companion, bool person_reactive, Cperson_abstract::companion_reactive type_rec_akp)
{
Sforce force_res;
@@ -770,7 +770,7 @@ Sforce Cscene_abstract::force_persons_int_companion( Cperson_abstract* center ,
return force_res;
}
-
+/*** calcular fuerzas para el grupo o otros tipos de interaccion. Tanto esta force_persons_int2 como esta force_persons_int3 **/
Sforce Cscene_abstract::force_persons_int2( Cperson_abstract* center, std::vector<int> ids_of_persons_in_group ) // made by ely (companion, no take into accound the person companion of the group force.)
{
// para tener grupos de personas en las personas simuladas.
@@ -829,7 +829,7 @@ Sforce Cscene_abstract::force_persons_int3( Cperson_abstract* center ) // made b
return force_res;
}
-
+/*** to obtain the actual sfm_params for robot (sin companion)***/
const std::vector<double>* Cscene_abstract::get_sfm_params( const Cperson_abstract * center_person)
{
assert( center_person != NULL );
@@ -849,7 +849,7 @@ const std::vector<double>* Cscene_abstract::get_sfm_params( const Cperson_abstra
-
+/*** to obtain the actual sfm_params for robot (con companion)***/
std::vector<double> Cscene_abstract::new_get_sfm_params( const Cperson_abstract * center_person)
{
assert( center_person != NULL );
@@ -866,7 +866,7 @@ std::vector<double> Cscene_abstract::new_get_sfm_params( const Cperson_abstract
return social_forces_param_to_obs_;
}
}
-
+/*** to obtain the actual sfm_params for people ***/
const std::vector<double>* Cscene_abstract::get_sfm_int_params( const Cperson_abstract * center_person, const Cperson_abstract * interacting_person)
{
//std::cout << " IIINNNNNNNNN Cscene_abstract::get_sfm_int_params " << std::endl;
@@ -931,7 +931,9 @@ const std::vector<double>* Cscene_abstract::get_sfm_int_params( const Cperson_ab
return NULL;
}
-
+/*** lee el mapa para sacar las fuerzas de este, pero no se usa...
+ * Creo que deprecated desde Gonzalo. Idem para todas las que intenten sacar obstaculos del mapa y
+ * NO desde el laser. Las que se usan son las que sacan obstaculos del laser***/
bool Cscene_abstract::read_force_map( const char * path )
{
FILE * fid;
@@ -1022,7 +1024,7 @@ Spoint Cscene_abstract::m_to_xy( unsigned int m)
double y = min_y_ + m/map_number_x_*map_resolution_;
return Spoint( x,y );
}
-
+/*** leer el mapa de destinaciones estaticas: el .txt que se usan para cambiar las destinaciones.***/
bool Cscene_abstract::read_destination_map( const char * path )
{
/* * how to use this function: you need a destination's file
@@ -1073,7 +1075,7 @@ bool Cscene_abstract::read_destination_map( const char * path )
this->set_destinations( destinations_ );
return read_destination_map_success_;
}
-
+/*** leer el mapa de destinaciones estaticas: el .txt que se usan para cambiar las destinaciones.***/
bool Cscene_abstract::read_destination_map2( const char * path ) // made by (ely), to make people groups in the simulation.
{
/* * how to use this function: you need a destination's file
diff --git a/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp b/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp
index 8510bb4975d55172cd285bd7e46fd05afba509a1..60f98001467ff5ec8ccf55209ec27edfd7b99c03 100644
--- a/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp
+++ b/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp
@@ -33,6 +33,7 @@ Cperson_bhmip::~Cperson_bhmip()
/*** añadir nueva posicion a la ventana de posiciones de las personas para luego poder predecirlas con una velocidad mejor, hay varios filtros que se pueden usar,
* se usa el que se ponga en: Cperson_abstract::filtering_method filter. El que uso, suele ser el que mejor funciona para el companion.***/
+/*** used to add a person point to the window used to estimate the real velocity of this person and after that do the bhmip propagation.***/
void Cperson_bhmip::add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter, bool robot_or_person, double in_new_value_windowing)
{
time_window_=in_new_value_windowing;
@@ -592,7 +593,7 @@ SpointV_cov Cperson_bhmip::low_pass_filter_current_state_linear_regression( )
return filtered_pose;
}
-
+/*** calcular la best destination (calcular la intencion de la persona, hacia donde va del entorno)***/
void Cperson_bhmip::intention_precalculation()
{
if(debug_intention_precalculation_){
@@ -690,7 +691,7 @@ void Cperson_bhmip::refresh_person( double now )
}
now_ = now;
}
-
+/*** does the prediction using the bhmip method, which includes the estimation of the best static-destination for this person in the actual instant of time.***/
void Cperson_bhmip::prediction( double min_v_to_predict )
{
//std::cout << " 333333377777777 IN Cperson_bhmip::prediction( double min_v_to_predict ) current_pointV_.v()="<<current_pointV_.v()<< std::endl;
diff --git a/local_lib_people_prediction/src/scene_elements/robot.cpp b/local_lib_people_prediction/src/scene_elements/robot.cpp
index 61c4989cb8cc9ddf63c30be194276d14f48ad3f7..624458d25285c1cdc47109cfd93c7e23b8f7302d 100644
--- a/local_lib_people_prediction/src/scene_elements/robot.cpp
+++ b/local_lib_people_prediction/src/scene_elements/robot.cpp
@@ -140,6 +140,7 @@ void Crobot::robot_propagation(double dt , unsigned int index, double v, double
/***Para calcular propagacion del path del grupo. heredada. OJO, esto ( aw -= normal_vel_dampening_parameter_*ini_pose.w;) NO se toca
* Lo necesita y lo comprobe en experimental, ya ni me acuerdo exactamente para que me dijo Gonzalo que lo necesita la propagación,
* pero era algo teorico, aunque el valor lo ajusto experimentalmente (creo).***/
+/*** obtener la propagación del robot, usando las fuerzas de interaccion calculadas y los limites de velocidades y aceleraciones para el movimiento del robot.***/
void Crobot::robot_propagation(double dt , unsigned int index, const Sforce &f , bool debug_force_vel) // con fuera entro en esta!!!
{
//kinodynamic unicycle
diff --git a/local_lib_people_prediction/src/scene_sim.cpp b/local_lib_people_prediction/src/scene_sim.cpp
index 589d651de5f69b7a3186242a3912fef00af353f0..d329ed508b8f88def0a50d90920ed6c2c47a6fbe 100644
--- a/local_lib_people_prediction/src/scene_sim.cpp
+++ b/local_lib_people_prediction/src/scene_sim.cpp
@@ -148,6 +148,11 @@ Cperson_abstract* Cscene_sim::add_person_container( unsigned int id,
return (Cperson_abstract*) person;
}*/
+
+
+/*** funcion principal, que hace que otras personas de la escena se muevan ramdom hacia los goals estaticos incluidos en la escena
+ * además de incluir una persona a la que acompaña el robot, de la cual se puede cambiar sus destinaciones
+ * y usa el akp-planner para obtener un comportamiento más realistico a la hora de evitar obstaculos. ***/
void Cscene_sim::update_scene(const std::vector<SdetectionObservation>& observation, bool& we_have_person_companion , bool person_or_robot)
{
//the filter variable is not used in the simulated scene. All observations have been filtered.
@@ -669,7 +674,7 @@ void Cscene_sim::insert_more_random_people(const std::vector<SdetectionObservati
// std::cout << " out insert more people " << std::endl;
}
-
+/*** igual que la anterior, pero la necesita la persona acompañante del robot, no el robot. Por eso esta "doble"***/
bool Cscene_sim::update_scene_companion_simulation_akp_person_companion(const std::vector<SdetectionObservation>& observation , bool ini_case, double ini_theta_person_companion, Spose& pose_command)
{
d_min_=1e10;