diff --git a/launch/ivo_launch/ivo_nav_companion_only.launch b/launch/ivo_launch/ivo_nav_companion_only.launch
index 62518923a944e5ba9649a7d194555d2e0af1f022..2dccfdf115c0bb98e4be01db6b0d89478c803ac5 100644
--- a/launch/ivo_launch/ivo_nav_companion_only.launch
+++ b/launch/ivo_launch/ivo_nav_companion_only.launch
@@ -41,31 +41,32 @@
FME_with_obts=destinaciones_ana_FME_ubuntu18_ros_melodic.txt
master_big=destinaciones_ana_BRL_ubuntu18_ros_melodic.txt-->
<param name="AkpLocalPlanner/results_filename_Zanlungo" value="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/local_lib_ATR_7_junio/1_data_results_Vform/results_person_companion_Vform.txt" />
- <param name="AkpLocalPlanner/robot" value="tiago"/>
- <param name="AkpLocalPlanner/person_goal_id" value="200"/>
+ <param name="AkpLocalPlanner/robot" value="ivo"/>
+ <!-- No existe: <param name="AkpLocalPlanner/person_goal_id" value="200"/> -->
<param name="AkpLocalPlanner/person_radi_amp" value="0.5"/> <!-- <[antes estaba a 0.2 (13/04/2021), pero prefiero probarlo a 0.5, hay más margen y si va bien, ya esta.] MUY importante: en sim, person_radi_amp=0.5 (SIDE-BY-SIDE); en robot real, (zanlungo) person_radi_amp=0.0; -->
- <param name="AkpLocalPlanner/change_treshold_distance_between_steps" value="0.3"/> <!-- change_treshold_distance_between_steps=0.5; real creo que es change_treshold_distance_between_steps=0.1, es el error de posición respecto a una iteracion y la otra. -->
+ <!--NO existe: <param name="AkpLocalPlanner/change_treshold_distance_between_steps" value="0.3"/>--> <!-- change_treshold_distance_between_steps=0.5; real creo que es change_treshold_distance_between_steps=0.1, es el error de posición respecto a una iteracion y la otra. -->
<!-- for SIM-->
<param name="AkpLocalPlanner/frame_robot_footprint" value="base_footprint"/>
<param name="AkpLocalPlanner/change_sim" value="False"/>
<param name="AkpLocalPlanner/sim_target_per" value="False"/>
-
-
-<!-- NEW variables, maybe I need to add them-->
<param name="AkpLocalPlanner/detection_laser_obstacle_distances" value="3.5"/>
<param name="AkpLocalPlanner/real_companion_angle_SideBySide" value="90.0"/> <!-- No lo setea bien desde aquÃ, si hace falta, mejor cambiarlo en el dynamic reconfigure.-->
<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/conf_id_to_remove" value="100000"/>
- <param name="AkpLocalPlanner/conf_low_range_scan_detect_obstacles" value="0.0"/>
- <param name="AkpLocalPlanner/max_real_speed_out" value="0.5"/>
- <param name="AkpLocalPlanner/max_real_angular_speed_out" value="0.4"/>
+<param name="AkpLocalPlanner/max_real_speed_out" value="0.5"/>
+
<param name="AkpLocalPlanner/conf_final_max_v" value="1.0"/> <!-- (estas 2 siguientes) calcular fuerzas internas, no se pueden bajar más o NO se movera el robot-->
<param name="AkpLocalPlanner/v_max" value="1.0"/>
<param name="AkpLocalPlanner/av_max" value="0.2"/> <!-- incremento normal es 0.4, probaremos primero con 0.2-->
<param name="AkpLocalPlanner/av_max_VrobotZero" value="0.2"/><!-- incremento normal es 0.6, probaremos primero con 0.2. Aunque para IVO mejor 0.4 como mucho.-->
<param name="AkpLocalPlanner/av_break" value="0.2"/><!-- incremento normal es 0.4, probaremos primero con 0.2. -->
<param name="AkpLocalPlanner/aw_max" value="0.6"/><!-- incremento normal es 0.9, como mucho se puede reducir a 0.6, que parece que funciona en simulación con obstaculos (valores menores se CHOCA con obstaculos).-->
+
+<!-- NEW variables, maybe I need to add them-->
+ <!-- <param name="AkpLocalPlanner/speed_k_angular" value="1.0"/>
+ <param name="AkpLocalPlanner/max_real_angular_speed_out" value="0.4"/>
+ <param name="AkpLocalPlanner/conf_id_to_remove" value="100000"/>
+ <param name="AkpLocalPlanner/conf_low_range_scan_detect_obstacles" value="0.0"/>-->
+
</node>
<!--</group>-->
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 194724c8258aa31edee5566c477dfd53c9b80e23..a16ec12fb9cc2872c5f491427063e3bb445d890b 100644
--- a/local_lib_people_prediction/src/nav/plan_local_nav.cpp
+++ b/local_lib_people_prediction/src/nav/plan_local_nav.cpp
@@ -4,6 +4,7 @@
* Created on: Dec 22, 2013. Last Modified on 2020
* Author: Initial code of the AKP Gonzalo Ferrer.
* Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ * Main .cpp for the planner.
*/
#include "nav/plan_local_nav.h"
@@ -221,6 +222,7 @@ output_screen_messages_(false)
std::cout << " (INI akp_companion_person planner) max_iter_ ="<<max_iter_<<"; horizon_time="<<horizon_time_<< std::endl;
}
+/* file initialization to save data */
std::string data_file;
std::string data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case2.txt";
evaluate_costs_file_=data_file2;
@@ -252,7 +254,9 @@ output_screen_messages_(false)
//new_matlab_file_To_evaluate_change_distance_and_angle(); // generar archivo y ya en esa iter ira bien!
//new_matlab_file_To_evaluate_costs();
- // poses for the iterative simulation:
+
+
+ /**** poses for the iterative simulation: ****/
//SIM_initial_robot_pose2_=Spose(-7.0,-0.83,0.0,0.0,0.0,0.0); //(julio2019) FINAL Initial POSE robot (case central!) antes -1.25
SIM_initial_robot_pose2_=Spose(-7.0,3.0,0.0,0.0,0.0,0.0); //(julio2019) FINAL Initial POSE robot (case lateral!)!
//SIM_initial_robot_pose2_=Spose(1.0,-0.83,0.0,0.0,0.0,0.0); // antes -1.25
@@ -296,7 +300,8 @@ Cplan_local_nav::~Cplan_local_nav()
//all memory allocations are freed in Cprediction_behavior/bhmip
}
-
+/**** Funcion principal de la libreria: coje las companion-person, calcula la orientación del grupo, gestiona el simulador iterativo
+ * Incluye la funcion "core" que usa el AKP+companion-embeded y incluye funcion que guarda resultados en txt. ****/
bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt)
{
//std::cout <<" In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
@@ -315,6 +320,8 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
std::cout <<" In robot_->get_v_max()="<<robot_->get_v_max() << std::endl;
}
+
+
robot_->set_desired_velocty(max_v_by_system_); // reset velocity for the approaching cases.
//Spose robot_act=pose_command;
@@ -349,7 +356,7 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
-
+ /**** Get person companion pointers ****/
// std::cout <<"4 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
pointer_to_person_companion_=person_obj_companion_person;
//std::cout <<"4.1 In Local Lib ASSAOP robot_plan_companion3; we_have_pointer_to_first_person_="<<we_have_pointer_to_first_person_<<"; id_person_companion_="<<id_person_companion_<< std::endl;
@@ -383,11 +390,14 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
people_act.push_back(initial_person_companion_point_);
center_of_the_group_Zanlungo_=calculate_centre_of_the_group(robot_now,people_act);
//std::cout <<"7 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+
+ /**** Calc group velocities ****/
double x_center_group_people=(initial_person_companion_point_.x)/number_of_group_people_;
double y_center_group_people=(initial_person_companion_point_.y)/number_of_group_people_;
double vx_center_group_people=(initial_person_companion_point_.vx)/number_of_group_people_;
double vy_center_group_people=(initial_person_companion_point_.vy)/number_of_group_people_;
+ /**** Calc group orientation for dynamic destination***/
mean_people_Zalungo_=SpointV(x_center_group_people, y_center_group_people, initial_person_companion_point_.time_stamp, vx_center_group_people, vy_center_group_people );
if(initial_person_companion_point_.v()>0.2){
theta_of_the_group_Zanlungo_=atan(vy_center_group_people/vx_center_group_people);
@@ -422,6 +432,7 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
clock_start4= (unsigned int) robot_plan_companion3_start_ITER;
clock_end4= (unsigned int) robot_plan_companion3_end_ITER;
+ /*** Get spoints for person companion ***/
for( auto iit: person_list_ )
{
if(iit->get_id()==id_person_companion_){
@@ -459,7 +470,9 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
std::cout << "(before initialice files) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
}
//std::cout <<"11 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
- // INI Starting points of people! when only have companion task.
+
+/**** INI Starting points of people! for simulation of the companion task. ****/
+
if(sim_target_per){
if(see_save_in_file_){
@@ -593,6 +606,9 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
break;
case ITER:
+
+/*** CASO iteracion, dentro se hace el planner con companion + AKP ***/
+
if(in_debug_all_IVO_){
std::cout <<"ATR companion ITER"<< std::endl;
}
@@ -600,6 +616,8 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
go_behid_comp_person_=true;
goal1_=3;
+/*** Estas partes estan desabilitadas, pero como sabéis en el planner son muy importantes los tiempos y tengo varios de estos ***/
+/*** en ciertas partes del código para saber que parte es la que hace incrementar el coste computacional. ***/
unsigned int clock_start2;
unsigned int clock_end2;
if(check_execution_times_){
@@ -613,7 +631,7 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
std::cout << "(before robot_plan_companion2) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
}
//std::cout <<"ATR COMPANION robot_plan_companion3; debug 13 (antes de robot_plan_companion2)"<< std::endl;
- // iteracion akp!!!
+/*** iteracion akp + companion "embeded" (Core del código) !!! ***/
act_result=robot_plan_companion2(pose_command, reactive,dt);
//std::cout <<" despues de robot_plan_companion2 "<< std::endl;
@@ -632,7 +650,10 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
std::cout << "(FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
}
+ // Destinación actual para sacarla fuera de la libreria.
const std::vector<Sdestination>* actual_dests= get_destinations();
+
+/*** SIMULACION-iterativa: gestionar el saltar al Inicio de la simulacion ***/
if(sim){
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;
@@ -653,6 +674,7 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
}
}
+/*** Restart de los ficheros de datos ***/
if(restart_real_){ // restart document, new companion experiment.
restart_real_=false;
Action_=Cplan_local_nav::ITER; // cambiar por esperar hasta que la person_companion y person_goal lleguen al goal...
@@ -688,6 +710,10 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
}
//find_comp_pers= find_person(id_person_companion_ , &person_obj3);
+
+/*** (este cambio es culpa de la destinación dinamica) Reset de destinacion persona companion dentro de otras funciones del AKP, si no lo hacÃa asÃ, si la persona cambiaba de destinación,
+ * el robot se quedaba con la destinación erronea y no hacia bien el companion. ***/
+
if(we_have_pointer_to_first_person_){
if(actual_person_Companion_pointer_->get_best_dest().id!=before_companion_person_destination_.id){
actual_person_Companion_pointer_->reset_before_destination_prob();
@@ -709,7 +735,8 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
final_compt_time_=((clock_end-clock_start)/clocks_per_sec_my_var_)*1000;
- // Side-by-side for simulation iterative.
+
+/***SOLO en Side-by-side for simulation iterative: PARA restart sim, que pare hasta que lleguen las personas a la posicion inicial. ***/
if((final_goal_reached_in_node_)&&(!change_final_robot_orientation_)&&(sim)){
std::cout << " V=0; w=0; (final_goal_reached_in_node_)&&(!change_final_robot_orientation_); final_goal_reached_in_node_="<<final_goal_reached_in_node_<<"; !change_final_robot_orientation_="<<change_final_robot_orientation_<< std::endl;
pose_command.v=0.0; // MIRAR si puede ser usar el case stop. Creo que no...
@@ -727,14 +754,15 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
clock_end= (unsigned int) robot_plan_companion3_end;
// std::cout << "before/antes limit velocity: pose_command.v="<<pose_command.v<<"; final_max_v_="<<final_max_v_<<"; act_result="<<act_result<< std::endl;
- if(final_max_v_<pose_command.v){ // limit the robot's velocity until the maximum one.
+/***** limit the robot's velocity until the maximum one.****/
+ if(final_max_v_<pose_command.v){
pose_command.v=final_max_v_;
if(see_std_out_velocities_){
std::cout << " LIMIT vel vMax after/despues limit velocity: pose_command.v="<<pose_command.v<<"; final_max_v_="<<final_max_v_<< std::endl;
}
}
-
+/*** Para salbar los resultados en el fichero****/
if(save_in_file_){
// save results in file for SIDE-by-SIDE
if(sim){
@@ -755,6 +783,7 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
if(((distance_to_goal1>meters_goal_to_save_results_in_file_)&&(distance_to_goal2>meters_goal_to_save_results_in_file_))&&(save_results_in_file_)){ // guarda datos.
// Not save the results very near to the goal, because the sim people do not turn as a normal person. Then this behavior is not normal and can afert to the real results of the method.
if(save_results_on_files_for_robot_){
+/*** printToMatlab() es la funcion que salba los resultados en el fichero .txt ***/
printToMatlab();
}
first_time_=true;
@@ -782,11 +811,12 @@ bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstrac
std::cout << "(2) OUTPUT ROBOT PLAN 3 pose_command.v="<<pose_command.v<<"; pose_command.w="<<pose_command.w<< std::endl;
}
return act_result;
-
+// FIN funcion principal!!!
}
-////////////////////////////
+/***** Inicialización del plan, pero para el simulador-iterativo para llegar a la pose inicial del simulador *****/
+/*** (en explicación, esta mejor saltarla: ya que ahora no la usarán, usaran simulador gazebo y luego al robot) ***/
bool Cplan_local_nav::robot_plan_companion_return_initial_pose(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt)
{ // function to return to the initial pose in the cases of the robot's iterative simulations.
// std::cout <<"(1) INI robot_plan_companion_return_initial_pose"<< std::endl;
@@ -926,7 +956,7 @@ bool Cplan_local_nav::robot_plan_companion_return_initial_pose(Spose& pose_comma
-
+/*** FUNCION-CORE, es la que incluye TODO lo importante, la que hace el planner propiamente dicho ***/
bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt)
{ // FUNCTION that calculates the local-planner for the robot's Adaptive Side-by-Side Accompaniment of One person (ASSAOP) and the robot's Adaptive Side-by-Side Accompaniment of Groups of People (ASSAGP)
//std::cout <<" !!!INI robot_plan_companion2!!! " << std::endl;
@@ -944,6 +974,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
before_initial_robot_spoint_.x=-10000;
before_initial_robot_spoint_.y=-10000;
+/*** Inicio goal del grupo == goal persona. Por si goal dinamico esta en el limite o la persona esta parada, entonces se usa el goal statico=mismo-goal-persona-companion ***/
if(companion_same_person_goal_){
//Cperson_abstract* person_obj;
//find_person(id_person_companion_ , &person_obj);
@@ -973,7 +1004,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
clock_t scene_prediction_start, scene_prediction_end;
scene_prediction_start=clock();
- //// INI: to set initial angle of companion for the calculation of the angle companion with the person prediction.
+/**** INI: to set initial angle of companion for the calculation of the angle companion with the person prediction. ****/
if(!calc_goal_companion_with_group_path_){
//std::cout << "ATR code ENTER: => if(!calc_goal_companion_with_group_path_)"<< std::endl;
// Calc angle of compnaion with the prediction of the person before scene_prediction
@@ -1039,13 +1070,15 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
}
}
-
+/*** Calculo del goal companion con el path del grupo ***/
this->Cprediction_behavior::set_calc_goal_companion_with_group_path(calc_goal_companion_with_group_path_);
//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 ***/
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
+
+/*** get the angle of compnaion with the prediction of the person before scene_prediction (angulos de acompañamiento segun colisiones con otras personas)***/
if(!calc_goal_companion_with_group_path_){
orientation_person_robot_angles_with_prediction_of_person_companion_=this->Cprediction_behavior::get_orientation_person_robot_angles_pred();
min_distance_collision_vector_from_pred_=this->Cprediction_behavior::get_min_distance_collision_vector_from_prediction();
@@ -1064,14 +1097,14 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
SpointV_cov centro_robot_spoint=robot_->get_current_pointV();
initial_robot_spoint_=robot_->get_current_pointV();
- // initial angle to accompany the person companion!
+/*** initial angle to accompany the person companion = angulo final ideal de la iteracion anterior. ***/
if(firts_iter_obtain_angle_person_companion_){
last_good_theta_person_=robot_initial_pose_.theta;
firts_iter_obtain_angle_person_companion_=false;
}
- /* Inicio, get person_robot_distance.*/
+/***** Inicio, get person_robot_distance real, para el acompañamiento.*****/
double robot_person_distance=calc_robot_person_companion_distance();
robot_person_distance_=robot_person_distance;
//std::cout << " after get's calc_robot_person_companion_distance(); "<< std::endl;
@@ -1100,18 +1133,23 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
fileMatlab2.close();
}
+ /*** Distancia de diametro de la "burbuja" de acompañamiento que engolba a companion-y-robot para luego calcular colisiones y hacer el acompañamiento dinamico***/
robot_person_companion_distance_=(robot_person_proximity_distance_+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2;
//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)***/
init_robot_plan2(robot_person_distance);
//init_robot_plan2_end=clock();
//std::cout << " DESPUES init_robot_plan2"<< std::endl;
+/*** funcion que Calcula el incremento de angulo para acompañamiento adaptativo "smooth" == los distintos angulos que se calcularan hacia el futuro para posicionar al robot detras para evitar obstaculos.***/
ini_increment_angle(); // for robot companion
+
double people_velocity=mean_people_velocity_;
+/*** CASE AKP-planning (persona andando, NO esta parada!) ***/
if(((((robot_person_distance<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_))&&(robot_person_distance>0.5))&&(people_velocity>0.2) ))&&(!case_akp_plan_por_freanado_)){
//std::cout << "CASE AKP PLANNING"<< std::endl;
is_case_akp_true_=true;
@@ -1122,28 +1160,34 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
fileMatlab2.close();
}
+/*** Estos dos casos gestionan si es AKP-companion o si estoy muy lejos y quiero acercarme al lateral de la persona lo más rápido posible ***/
if(!init_plan_approach_side_){ // ARREGLADO EL PARAR cerca del goal side-by-side en companion con esto!!!
- reactive=Cperson_abstract::Akp_planning;
+ reactive=Cperson_abstract::Akp_planning; // CASO: AKP-companion
robot_companion_case_=reactive;
}else{
- reactive=Cperson_abstract::Reactive_atractive;
+ reactive=Cperson_abstract::Reactive_atractive; // CASO: robot navega solo hacia el lateral de la persona.
robot_companion_case_=reactive;
}
//std::cout << " before robot_plan_anticipative_krrt_companion"<< std::endl;
+/*** Aqui empieza el core del akp-planning heredado de Gonzalo=> funcion_core_akp: robot_plan_anticipative_krrt_companion() ***/
if ( robot_plan_anticipative_krrt_companion(reactive) )
{
//std::cout << " IN if-robot_plan_anticipative_krrt_companion"<< std::endl;
clock_t end_part_start, end_part_end;
end_part_start=clock();
//std::cout << " CASE one person accompaniment get_best_planned_pose "<< std::endl;
+/*** una vez hecho el akp, si hay plan, cogemos la mejor pose siguiente para el robot del akp ***/
last_pose_command_ = get_best_planned_pose( dt ,reactive);
double other_people_cost_due_to_robot;
double companion_person_cost_due_to_robot;
+/*** Calculamos las fuerzas y obtenemos el movimiento del robot hacia esta "best-robot-pose"==last_pose_command_***/
this->Cprediction_behavior::calculate_current_forces_companion2( pr_force_mode_ ,id_person_companion_,false, reactive, min_next_companion_angle_,u_forces_robot_actual_.f_goal,u_forces_robot_actual_.f_people,u_forces_robot_actual_.f_obs,u_forces_robot_actual_.f_persongoal,other_people_cost_due_to_robot,companion_person_cost_due_to_robot);//for plotting purposes
+ // Estos son costes que salbo para calcular los costes de interaccion entre-robot y otras personas; y entre el robot-y-person-companion.
+ // Estan aquÃ, no recuerdo si luego se guardan aún en el .txt o si los suprimi, pero si se quieren sacar es solo añadir una linea que los incluya en el .txt
other_people_due_to_robot_cost_=other_people_cost_due_to_robot;
companion_person_due_to_robot_cost_=companion_person_cost_due_to_robot;
@@ -1152,8 +1196,8 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
last_pose_command_.print();
robot_->print();
}
-
- if(!final_collision_check_){
+/*** Check colision de la propagacion final del robot (esta de normal al usar las fuerzas es poco probable que pase, pero al usar el save-comand-vel a veces pasaba)***/
+ if(!final_collision_check_){ //
result = true;
result_=true;
}else{
@@ -1174,6 +1218,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
}
else
{
+/*** Check que no haya path viable en el local planner (por eso lo del mensaje que decia Tibi de no tengo path, hay obstaculos muy cerca. ***/
last_pose_command_ = Spose();
result = false;
enter_on_cero2_=true;
@@ -1185,6 +1230,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
}
}else if(robot_person_distance<0.5){
+/*** si estoy a menos de 0.5m de la pesona que acompaño, me alejo hacia el goal de acompañamiento ideal (repulsion) ***/
//else if(robot_person_distance<(robot_person_proximity_distance_-robot_person_proximity_tolerance_)){
// case reactive_repulsive_force
//std::cout << "CASE reactive_repulsive_force"<< std::endl;
@@ -1198,12 +1244,16 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
reactive=Cperson_abstract::Reactiva_repulsive;
robot_companion_case_=reactive;
+/*** planning akp para este caso repulsivo****/
if ( robot_plan_anticipative_krrt_companion(reactive) )
{
+
+/*** lo mismo que antes, obtener la mejor pose del plan y calculo de fuerzas + prediccion movimiento robot pero para este caso ***/
last_pose_command_ = get_best_planned_pose( dt,reactive);
double other_people_cost_due_to_robot;
double companion_person_cost_due_to_robot;
+
this->Cprediction_behavior::calculate_current_forces_companion2( pr_force_mode_ ,id_person_companion_,false, reactive, min_next_companion_angle_,u_forces_robot_actual_.f_goal,u_forces_robot_actual_.f_people,u_forces_robot_actual_.f_obs,u_forces_robot_actual_.f_persongoal,other_people_cost_due_to_robot,companion_person_cost_due_to_robot);//for plotting purposes
other_people_due_to_robot_cost_=other_people_cost_due_to_robot;
companion_person_due_to_robot_cost_=companion_person_cost_due_to_robot;
@@ -1220,7 +1270,8 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
result = false;
result_=false;
}
- }else if(people_velocity<0.2){ //VETE AL GOAL LATERAL O CENTRAL! if((case_akp_plan_por_freanado_)&&((people_velocity<0.2))){
+ }else if(people_velocity<0.2){
+/****La persona esta parada, el robot se va al GOAL LATERAL de esta O CENTRAL (en el companion de 2personas)! if((case_akp_plan_por_freanado_)&&((people_velocity<0.2))){ ***/
// if(robot_person_distance>(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)){ // case reactive_atractive_force
//std::cout << " IN robot_plan_anticipative_krrt_companion (case else_if-case ATRACTIVE)"<< std::endl;
@@ -1234,6 +1285,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
reactive=Cperson_abstract::Reactive_atractive;
robot_companion_case_=reactive;
//std::cout << "antes robot_plan_anticipative_krrt_companion-function"<< std::endl;
+/*** lo mismo que antes, planning akp+companion y obtener la mejor pose del plan y calculo de fuerzas + prediccion movimiento robot pero para este caso ***/
if ( robot_plan_anticipative_krrt_companion(reactive) )
{
//std::cout << "in if-robot_plan_anticipative_krrt_companion-function"<< std::endl;
@@ -1260,6 +1312,8 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
result_=false;
}
}else{
+/**** Caso La persona esta muy lejos del robot, el robot se aproxima más rápido al GOAL LATERAL de esta O CENTRAL (en el companion de 2personas)! if((case_akp_plan_por_freanado_)&&((people_velocity<0.2))){ ***/
+
//std::cout << " IN robot_plan_anticipative_krrt_companion (case else_if-case ATRACTIVE)"<< std::endl;
//if(see_std_out_velocities_){
// std::cout << "CASE reactive_atractive_force"<< std::endl;
@@ -1271,6 +1325,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
fileMatlab2 << "Entra REACTIVE force_ATRACTIVE!!!!!! robot_person_distance="<<robot_person_distance<<" > (robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)="<<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)<<"\n";
fileMatlab2.close();
}
+/*** lo mismo que antes, planning akp+companion y obtener la mejor pose del plan y calculo de fuerzas + prediccion movimiento robot pero para este caso ***/
reactive=Cperson_abstract::Reactive_atractive;
robot_companion_case_=reactive;
@@ -1300,14 +1355,14 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
}
} // fin else-if akp companion
-
- pose_command = last_pose_command_; // nueva pose de trayectoria calculada.
+/*** nueva pose de trayectoria calculada. La que contiene las velocidades del robot y sacaremos fuera del nodo.***/
+ pose_command = last_pose_command_;
//std::cout << "(output robot_plan_companion2) last_pose_command_.print():"<< std::endl;
//std::cout << " pose_command.v"<<pose_command.v<< std::endl;
//std::cout << " pose_command.w"<<pose_command.w<< std::endl;
- // *** Return robot max vel to really máx velocity ***/
+/*** Return robot max vel to really máx velocity ***/
return_max_velocity_systemRobot_to_max_value();
//robot_plan_companion2_end = clock();
@@ -1328,7 +1383,7 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
SpointV perosn_comp_act;
perosn_comp_act=actual_person_Companion_SpointV_;
- // FOR simulation!!! // Si la persona esta parada! NO se su orientación => me quedo en la posición actual.
+/**** FOR simulation-iterativa!!! // Si la persona esta parada! NO se su orientación => me quedo en la posición actual.****/
if((perosn_comp_act.v()<0.09)&&(sim)){ //for simulation if the group stops due to other people, stop with V=0, not with v=0.05 or soemthing similar to move all the time.
//std::cout << "!!!!!! 33333 777777 !!!!! entro en SIM set vel a 0.0!!! (perosn_comp_act.v()<0.09)&&(sim); perosn_comp_act.v()="<<perosn_comp_act.v() << std::endl;
pose_command=robot_initial_pose_;
@@ -1349,8 +1404,11 @@ bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstrac
}
return result;
-}
+} // Fin de la función "core-1" ya que la core real es la que hace el planning akp+companion = robot_plan_anticipative_krrt_companion(...)
+
+/*** Necesita varias funciones de set goal = caso goal fijo, caso goal dynamico, caso goal lateral. Quizas se pudiera reducir alguna
+ * o alguna esta "deprecated", pero no si no las borre es que no me atrevia a quitarlas por si se "rompia" algo y no iba. ****/
void Cplan_local_nav::set_robot_external_goal( const Spoint& goal )
{
//std::cout << " (function: set_robot_external_goal) [Cperson_abstract::Akp_planning/Reactiva_repulsive/Reactive_atractive] (set_robot_external_goal) "<< std::endl;
@@ -1415,7 +1473,8 @@ void Cplan_local_nav::set_robot_goal_person_goal_global_plan_IN_robot_VERSION( c
}
/*****************************************************************************************************/
-
+/***Funcion init robot plan, pero para simulación iterativa. Que retorna robot y persona a la posicion inicial del simulador***/
+/*** Esta de momento mejor saltarla de la explicación.***/
bool Cplan_local_nav::init_robot_plan2_return_initial_position()
{
// clear all data structures and push back the initial state : current state
@@ -1607,7 +1666,10 @@ bool Cplan_local_nav::init_robot_plan2_return_initial_position()
//////////////////////////
-
+/*** init_robot_plan2 = inicializacion del plan. Se inicializan los costes y ramdom goals (etc) del planner,
+ * Se calculan los goals dynamicos en diferentes situaciones,
+ * se filtra el número de personas y obstáculos a la local window
+ * y se calcula el std= desviacion estandard para el factor ramdom del planner, que hace que se calculen los paths más abiertos (cuando hay más obstaculos y más cerrados cuando hay menos)****/
bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
{
@@ -1658,9 +1720,9 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
robot_->clear_Zanlungo_vectors(); // hace falta. se usa tambien para el side-by-side de 1 persona.
- //Correct current robot location and state: hypothesis the current robot state and the past robot action may differ.
+/*Correct current robot location and state: hypothesis the current robot state and the past robot action may differ.
//So we must take into account the delay and the system platform control, specially for the w, and propagate an estimation
- //of the state at time t_now to time t_now + t_processing and sending command
+ //of the state at time t_now to time t_now + t_processing and sending command*/
robot_->correct_state_to_delay( last_pose_command_, now_ , dt_ );
Sdestination robot_goal_positivo_; // caso lejos de la persona, ir al goal del lado de la persona!!!
@@ -1677,12 +1739,15 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
// Case One person Accompaniment.
mean_people_velocity_=pointer_to_person_companion_->get_current_pointV().v();
-
+/***Gestion de booleanos para los casos posibles case_akp_plann=persona_andando o case_stop=persona_parada
+ * Era mejor asà porque hay dos grandes casos y varios dentro de esos dos grandes, como:
+ * Persona parada, pero alejada del robot = me he de acercar, no pararme a su lado. (aquà hay dos goals posibles, se elige el más cercano al lado del robot (si no hay muchos obstaculos)
+ * Persona parada cerca del robot=> he de frenar suabe, no brusco. se gestiona con bool: case_akp_plan_por_freanado_ (vuelve a haber dos goals, uno a cada lado(
+ * Acompañamiento normal (se gestiona con los angulos de acompañamiento, pero también ha de calcular en que lado de la persona esta.)***/
bool case_akp_plann=true;
bool case_stop=false;
// Case comapnion. CASES AKP-plan or not.
-
if((robot_person_distance<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)&&(robot_person_distance>0.5))){
//std::cout << " (casos dist) my model!!! case_akp_plann=true; "<< std::endl;
case_akp_plann=true;
@@ -1723,11 +1788,12 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
//std::cout << " case_stop="<<case_stop<<"; case_akp_plann="<<case_akp_plann<< std::endl;
// FIN parte para controlar el frenado suave.
- // INI case: go to the person GOAL!
+/*** INI case: go to the person dynamic GOAL or final goal! == Habrá que calcular goal dinamico también. ***/
if(case_akp_plann){
//std::cout << " (init_planinng) case: go to the person GOAL!; companion_same_person_goal_="<<companion_same_person_goal_<< std::endl;
/* INI parte conflictiva, cambio pose robot al centro entre persona y robot == Side-by-side only one person. */
+ /*** Inicio calcular goal dinamico. orientación del grupo, intersecta con recta perpendicular desde el punto del goal estatico. ***/
double angle1=atan2(robot_initial_pose_.y-companion_person_position_.y , robot_initial_pose_.x-companion_person_position_.x);
Spose centro_person_robot2=robot_initial_pose_;
SpointV_cov centro_robot_spoint2=initial_robot_spoint_;
@@ -1740,7 +1806,7 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
double person_orientation=calc_person_companion_orientation();
centro_person_robot2.theta=person_orientation;
-
+/*** Inicio calculo hacia que lado del goal final se pone el goal dinamico (calculando en que lado esta el robot respecto a la persona)***/
// Case side-by-side (robot pose.)
robot_->set_current_pose(robot_initial_pose_);
robot_->set_current_pointV(initial_robot_spoint_);
@@ -1838,10 +1904,10 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
companion_person_position_=actual_person_Companion_SpointV_;
//std::cout <<"GOAL dynamic; we_have_pointer_to_first_person_="<<we_have_pointer_to_first_person_<< "; pointer_to_person_companion_->get_current_pointV().v()="<<pointer_to_person_companion_->get_current_pointV().v()<<"change_dynamic_goal_1_person_sidebyside_="<<change_dynamic_goal_1_person_sidebyside_<<std::endl;
-
+/*** dentro del aquà se calcula la interseccion entre orientación grupo y goal estatico, para obtener el dynamic goal.***/
if(we_have_pointer_to_first_person_){
// Case Only One person accompaniment.
-if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_dynamic_goal_1_person_sidebyside_)
+ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_dynamic_goal_1_person_sidebyside_)
//if(((robot_->get_current_pointV().v()>0.3)&&(pointer_to_person_companion_->get_current_pointV().v()>0.2))&&(change_dynamic_goal_1_person_sidebyside_)){ // si el robot esta andando. centramos el goal, usando su orientación.
// calculando la intersección perpendicular entre la recta de la dirección del movimiento del robot y el punto correspondiente al goal.
double m;
@@ -1883,7 +1949,9 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
}
}
-
+/*** si la distancia entre el goal estatico y el dimamico es menor que X-limite (ver documento parámetros), entonces se cambia a goal dynamico,
+ * si es mallor se deja el goal estatico, ya que sino colisionara el goal dinamico con los obstaculos del entorno y el global planner de ros
+ * cancela el planning y pasan cosas "raras" (como que se mueva el robot sin ningun contro usando la ultima velocidad) ***/
if(!change_dynamically_finaldest_){ // if true, we change the final dest dynamically, if false we do not change it
goal_robot=Spoint(pointer_to_person_companion_->get_best_dest().x,pointer_to_person_companion_->get_best_dest().y,pointer_to_person_companion_->get_best_dest().time_stamp);
}
@@ -1891,7 +1959,7 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
// INI calcular goal final caso frenado.
//SpointV new_prop_p1_to_goal=calcular_goal_frenado();
// FIN calcular goal final caso frenado.
-
+ /**** trozo deprecated, ya que se calcula en el case_stop de abajo. Lo deje hasta probar que funcionara bien el otro, pero se quedo aquÃ****/
if(we_have_pointer_to_first_person_){
// INI Case Only One person accompaniment.
//std::cout << " In last goal people stop 2 "<< std::endl;
@@ -1920,6 +1988,7 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
//Spoint person_position_frenar=Spoint(person_companion_position.x,person_companion_position.y,robot_->get_current_pointV().time_stamp);
+/*** caso parar al robot, si la persona que acompaña se para. En este caso, en el nodo se hace que pare lentamente. ****/
if(case_stop){ //last_good_theta_person_p1_
//if((before_person_comp_goal_p1_.x==0)&&(before_person_comp_goal_p1_.y==0)){
@@ -1941,6 +2010,7 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
}
}
+/*** Caso goal inicial (cuando se lanza el nodo) Estan todos parados, hasta que no se hacia donde van las personas!****/
// initial case. robot goal is its actual position to do not move.
if(initial_goal_case_bool_){ // OK, este esta bien!!! Estar parado, hasta que no se hacia donde van las personas!
@@ -1954,7 +2024,8 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
//goal_robot.y=new_prop_p1_to_goal.y; // es para test del goal de frenado.
//std::cout << " (set_robot_external_goal caso3) goal_robot.x="<<goal_robot.x<<"goal_robot.y="<<goal_robot.y<< std::endl;
- set_robot_external_goal(goal_robot); // FINALLY, we set the value of the robot_goals!
+/*** estas 3 siguientes sirben para cambiar el goal del nodo (externo). Eso que vemos que mi goal final es dinamico. ****/
+ set_robot_external_goal(goal_robot); // FINALLY, we set the value of the robot_goals!
set_robot_goal(goal_robot);
robot_dest_for_companion_status_=Sdestination(0,goal_robot.x,goal_robot.y,1);
// FIN update robot goal
@@ -1967,7 +2038,8 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
}
// ----------------------------------------------------------------------
- // set navigation goals
+/*** Esto es de gonzalo (por eso cambie yo antes mi goal), se que se necesita. Ahora mismo no segura de que hace.
+ set navigation goals ***/
workspace_radii_ = horizon_time_ * max_v_by_system_;//robot_->get_v_max(); //approximation of the max distance
local_v_goal_tolerance_ = robot_->get_v_max()+0.1; //sets the tolerance to the max value to always return true
@@ -2007,7 +2079,8 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
//now_ = set when updated
// ----------------------------------------------------------------------
- //select persons considered in the scene and reserve memory for planning
+/*** (También de gonzalo, seleccionar personas para tener en cuenta en planning y para tambien la std)== Personas dentro local window.
+ * select persons considered in the scene and reserve memory for planning ***/
Spoint robot_position = (Spoint) robot_->get_current_pointV(); //ojo! esta es más interna todabia! (hay que cambiarla tambien)
double d_ini,d_end,radii_2(workspace_radii_*workspace_radii_),d_min(1e10);
@@ -2052,7 +2125,9 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
}
// ----------------------------------------------------------------------
- //number of nearby obstacles: first approach, just count them...
+/****(También de gonzalo, seleccionar static obst para tener en cuenta en planning y para tambien la std)== static obst dentro local window.
+ * number of nearby obstacles: first approach, just count them...
+ */
clock_t obstacles_start, obstacles_end;
obstacles_start=clock();
int number_of_obstacles(0);
@@ -2067,7 +2142,8 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
}
-
+/*** cambio std segun numero onstaculos (parte gonzalo, parte mia). Tube que modificarlo en simulacion y real,
+ * iban mejor valores diferentes por el generador que usaba gonzalo de los obstaculos simulados ***/
for( auto iit: laser_obstacle_list_ )
{
d_ini = iit.distance2( robot_position );
@@ -2160,13 +2236,23 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
d_min_global_=d_min;
dmin_global_=d_min;
-
+/**** CASO else akp_plan => ir al goal estando persona alejada (ya este parada o moviendose)
+ * Basicamente, se calculan y se elige solo un goal final dependiendo de dos cosas:
+ * (1) La posicion/lado en el que esta el robot respecto a la persona que acompaña
+ * y
+ * (2) si hay obstaculos en ese lado (el robot debera evitarlos), al estar lejos de la persona, puede elegir en que lado se aproxima a esta
+ * (y mejor sin obstaculos)
+ * => para este caso se necesita hacer ambos casos separados por la obtencion de los obstaculos anteriores.
+ * =>El caso AKP no da fallos estando delante de eso y
+ * este caso (2) no da fallos estando despues de eso (lo de los obstaculos).
+ * ****/
+/*** init_plan_approach_side_=true; CAse go to the goal of the side of the person. ***/
if(case_akp_plann){
// case: go to the person GOAL! (case akp) [We need to separate these two if-els cases because we need the midle section for all the coda and for this other part.]
//std::cout << " NO go to goal. Allways akp_plan. (init_planinng) Case: far of the person position, go to the person!"<< std::endl;
}else{
-
- init_plan_approach_side_=true; // CAse go to the goal of the side of the person.
+/*** init_plan_approach_side_=true; CAse go to the goal of the side of the person. ***/
+ init_plan_approach_side_=true;
robot_->clear_planning_trajectory();//first pose is inserted in the trajectory vector
// case reactive or repulsive (The final goal of the robot is to aproach to the person side)
// para el reactive el goal es ir a la persona a la que sigues!
@@ -2459,7 +2545,7 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
}
- //depending on the number of persons considered, set the std of the std_goal_workspace [rad]
+ /**** Parte de gonzalo: depending on the number of persons considered, set the std of the std_goal_workspace [rad] ***/
int obstacles=number_of_obstacles2;
for( auto iit: nearby_person_list_ )
{
@@ -2481,7 +2567,7 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
if(obstacles<0){ // problema al restar numero de personas companion.
obstacles=0;
}
-
+ /**** Parte de gonzalo: set the std_goal_workspace_ segun numero de obstaculos cerca del robot. ***/
switch( obstacles )
{
case 0: // explanation: in central-Side-by-side I need less std to scape from local minima, because the robot was more protected with the people to get between two obstacles or something similar.
@@ -2511,6 +2597,7 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
//std::cout << "case 0 : alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+/* CASO tengo obstaculo MUY MUY cerca, aumento más la std para salir: If we have obstacles very near, augment the std of the workspace to get out.*/
if( sqrt(dmin_global_) < 2.0){ //If we have obstacles very near, augment the std of the workspace to get out.
std_goal_workspace_ = 1.1;
//std::cout << " INIT ROBOT PLAN2 STD!!! std_goal_workspace_= "<<std_goal_workspace_<<"; obst mas cerca de 1m ; sqrt(dmin_global_)=" <<sqrt(dmin_global_)<< std::endl;
@@ -2522,7 +2609,10 @@ if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_
}
-/* fin Ini_robot companion, para caso person companion akp*/
+/* robot_plan_anticipative_krrt_companion => funcion que calcula el AKP-planning.
+ * La GRAN mayoria de esta es de GONZALO, poco puedo decir... Se que funciona.
+ * a groso modo: calcula ramdom goals (mirando que no colisionen las branches usando las fuerzas y calcula los costes.
+ * Dentro de esta funcion o dentro de una de las subfunciones yo tambien incluyo el coste de companion y las fuerzas del companion */
bool Cplan_local_nav::robot_plan_anticipative_krrt_companion( Cperson_abstract::companion_reactive reactive)
{
// std::cout << "ATR COMPANION! (1) INI robot_plan_anticipative_krrt_companion"<< std::endl;
@@ -2605,11 +2695,14 @@ bool Cplan_local_nav::robot_plan_anticipative_krrt_companion( Cperson_abstract::
else // robot desired velocity is again max velocity
robot_->set_desired_velocty( robot_->get_v_max () );//robot always tries to be at max velocity, modified by nearby ppl
}
- //4-calculate the input u to propagate towards the random goal
+/* Gonzalo calcula los ramdom goals de la local-window
+ * 4-calculate the input u to propagate towards the random goal*/
random_goals2_.push_back(random_goal);
input = calculate_edge(parent_vertex_index, Sdestination(0,random_goal.x,random_goal.y),reactive);
- //5- Propagate tree.
+/**** Gonzalo propaga la rama del arbol hacia los ramdom goals de la local-window.
+ * Aquà dentro estan las fuerzas.
+ * 5- Propagate tree. ***/
collision_detected_[i] = propagate_vertex( parent_vertex_index, input ,reactive);
if(!collision_detected_[i]){
@@ -2619,19 +2712,26 @@ bool Cplan_local_nav::robot_plan_anticipative_krrt_companion( Cperson_abstract::
v_delta_.push_back(delta_);
}
//std::cout << "antes de calculate_cost"<< std::endl;
+
+/**** Gonzalo calcula el coste de este trocito de la rama del arbol hacia los ramdom goals de la local-window.
+ * Aquà dentro estan todos los costes.
+ * //6-calculate the propagation cost ***/
calculate_cost(parent_vertex_index, input,reactive);
//std::cout << "despues de calculate_cost"<< std::endl;
- //6-calculate the propagation cost
+
edge_.push_back( input );
}
//bucle_krrt_companion_end=clock();
- //7- return tree. Seek for the minimum cost branch of the tree
+/***Gonzalo: busca la rama de todos los paths que tenga menos coste.
+ * 7- return tree. Seek for the minimum cost branch of the tree***/
Crobot* robot_act=robot_;
unsigned int min_branch_index = global_min_cost_index( robot_act , reactive );
+/*** Me guardo el companion angle ideal para el proximo instante de tiempo,
+ * para poder propagar al robot usando el mejor camino y la fuerza companion****/
double ideal_companion_angle_in_the_next_instant_of_time;
if(calc_goal_companion_with_group_path_){
ideal_companion_angle_in_the_next_instant_of_time=orientation_person_robot_angles_[1];
@@ -2650,7 +2750,7 @@ bool Cplan_local_nav::robot_plan_anticipative_krrt_companion( Cperson_abstract::
}
- // Esta luego habra que comentar el IF para ver que los angulos en el path los haga bien.
+ //(es solo visualización para asegurarme que lo calculaba bien) Esta luego habra que comentar el IF para ver que los angulos en el path los haga bien.
if((reactive==Cperson_abstract::Akp_planning)&&(we_have_cost_companion_) && debug_real_test_companion4_){//&&(actual_debug2_)){
see_companion_path_angle_and_cost_of_min_cost_paths(min_branch_index);
}
@@ -2669,7 +2769,7 @@ bool Cplan_local_nav::robot_plan_anticipative_krrt_companion( Cperson_abstract::
return false;
}
- //8- calculate vector of vertex. In reverse order the vertexes of the best path (initial not
+ // GONZALO: 8- calculate vector of vertex. In reverse order the vertexes of the best path (initial not
// included as it doesn't provide information)
unsigned int y=0;
do
@@ -2692,9 +2792,11 @@ bool Cplan_local_nav::robot_plan_anticipative_krrt_companion( Cperson_abstract::
//evaluate_costs_printToMatlab();
return true;
-}
+} /// FIN FUNCION AKP-plan.
+/**** replan_last_step== La uso para incluir mi fuerza companion siguiendo el mejor path de la persona, para el siguiente instante de tiempo
+ * De aquà sale la pose final del robot y por tanto las velocidades finales.***/
Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini, SpointV robot_Spoint_ini, Sdestination robot_goal, double dt)
{ // retorna la Spose final. // index es solo para la el angulo y para la posición de la persona, el goal de la persona.
@@ -2703,7 +2805,7 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
unsigned int index2=1;
- Sforce f_goal, f_int, f_obs, f,f_person_goal, f_goal_final;
+ Sforce f_goal, f_int, f_obs, f,f_person_goal, f_goal_final; // f_person_goal= fuerza acompañamiento; f_goal=Fuerza hacia el path; f_goal_final= fuerza suma ponderada final.
// replan the min_cost_branch to do the planing taking into account the angle respect to the person.
//caso planning, normal Gonzalo.
@@ -2723,9 +2825,12 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
robot_path_goal_=robot_goal;
+ /*** Tube que cambiar la force_goal de gonzalo por esta, porque mis goals son muy cercanos, siguiente step y con la normal a los ramdom
+ * goals no iba bien.***/
f_goal = robot_->force_goal_near( robot_goal, get_sfm_params(robot_),&robot_Spoint_ini,reduce_max_vel_dist_,go_with_vel_per_,real_vel_per);
- //////// Force to maintain the angle between people and robot!
+/*** Force companion to maintain the angle between people and robot! (se calcula esto hasta que empiece siguiente fuerza).
+ * Tambien usa lo de saber en que lateral esta la persona respecto al robot***/
Sdestination person_companion_goal;
//Cperson_abstract* person_obj;
@@ -2758,6 +2863,7 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
// corregir mejor orientacion con prediccion cuando la velocidad es igual a 0. pero mejor orientacion con la trajectoria anterior cuando la velocidad es mayor que 0
// if(debug_correct_angle_person_replan_last_step_){std::cout <<" person_tracj.size()="<<actual_person_Companion_pointer_->get_past_trajectory()->size()<< std::endl;}
+/*** Calcula la orientación de la person companion***/
double theta=calc_person_companion_orientation();
if(actual_person_Companion_pointer_->get_current_pointV().v()>0.1){
@@ -2814,7 +2920,9 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
person_companion_goal_out_=person_companion_goal;
- /* INI calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir. */
+/* INI calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir.
+ * (para incluir en lo dinamico colisiones tmabien. Es decir, como el robot para evitarlos se pone detras de la persona,
+ * luego si hay espacio sin obstaculos en uno de los laterales se va hacia ahà para volver al side-by-side*/
if(chose_better_side_to_acompani_person_){
Sdestination person_companion_goal_positive=Sdestination(0,person.x,person.y,1.0);
Sdestination person_companion_goal_negative=Sdestination(0,person.x,person.y,1.0);
@@ -2834,7 +2942,7 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
select_side_of_the_person_to_go=true;
}
- // falta calcular colisiones.
+ // calcular colisiones.
if(select_side_of_the_person_to_go){
Spoint Spoint_pose_command=Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y);
double min_dist_colli_p=check_collision_companion_goal(Spoint_pose_command,0);
@@ -2931,11 +3039,13 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
}
robot_goal_to_person_companion_=person_companion_goal;
-
+/*** se calcula por fin la fuerza al goal, sabiendo hacia que punto de acompañamiento (formacion "dinamica") se ha de posicionar el robot
+ * respecto a la persona. ***/
f_person_goal=robot_->force_goal_near(person_companion_goal,get_sfm_int_params(robot_),&robot_Spoint_ini,reduce_max_vel_dist_,go_with_vel_per_,real_vel_per);
//std::cout << " (AKP force_persons_int_planning_virtual) act_min_companion_angle="<<act_min_companion_angle<<"; parent_vertex="<<parent_vertex<< std::endl;
+ /*** esta era para testear otras formas de calcular la resultante entre la f_goal al path y la f_person_goal de acompañamiento.***/
double goal_percent=1-person_goal_percent_;
f_goal_final=Sforce(goal_percent*f_goal.fx+person_goal_percent_*f_person_goal.fx,goal_percent*f_goal.fy+person_goal_percent_*f_person_goal.fy);
@@ -2951,18 +3061,21 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
robot_traj[index2]=initial_robot_spoint_;
new_robot_in_pose_real_robot->set_planning_trajectory(robot_traj);
+/*** Calculamos fuerza a obstaculos dinamicos = personas***/
f_int = force_persons_int_planning_virtual_robot_companion_propagation(new_robot_in_pose_real_robot,index);
- //map force, used for simulations
+ //map force, used for simulations (NO VA, ya lo herede sin que funcionara)
if( read_force_map_success_ ){
f_obs = get_force_map(new_robot_in_pose_real_robot->get_robot_planning_trajectory()->at(index2).x,
new_robot_in_pose_real_robot->get_robot_planning_trajectory()->at(index2).y);
}
+/*** Calculamos fuerza a obstaculos staticos***/
if(read_laser_obstacle_success_){
f_obs = force_objects_laser_int_planning_virtual( new_robot_in_pose_real_robot, index2, 25.0, true );
}
- // INICION Limitar fuerza obstaculos cuando hay muchos solapados
+/*** INICION Limitar fuerza obstaculos cuando hay muchos solapados.
+ * Esto es necesario por la gestion que se hace de obstaculos (los multiples cilindros que forman un obstaculo) ***/
double fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
double fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
double signo_x;
@@ -2987,8 +3100,12 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
last_step_robot_obstacles_cost_=f_obs.module2();
+/*** calculo fuerza de propagacion final del robot=f con las ponderaciones buenas del companion
+ * (las otras ponderaciones se usan solo para el camino de Gonzalo, ya que usan lo de persona más agresiva o menos
+ * y yo en este último step no me hace falta y perjudicaba al accompañamiento) ***/
f=f_goal*alpha_companion_+f_person_goal*beta_companion_ + f_int*gamma_companion_ + f_obs*delta_companion_; // prueba, con parametros articulo revista robot companion
+ // ver ponderacion solo de las fuerzas atractivas, al goal final-path y goal de acompañamiento.
f_goal_final=f_goal*alpha_companion_+f_person_goal*beta_companion_;
if(output_screen_messages_){
@@ -3007,8 +3124,11 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
//std::cout << " IN set robot velocity, to person velocity, because the person_companion_goal_is_very_neat to the initial robot position."<< std::endl;
}
+/*** propagamos la pose del robot para acompañamiento. Necesita la f_person_companion,
+ * por tanto es modificada/diferente a la robot_propagation de los paths del grupo (o solo navegacion robot)***/
Spose robot_propagation=robot_->robot_propagation_companion_position( dt, u.f ,robot_pose_ini,false); // le doy la pose que quiero que me propague y la fuerza con la que quiero que haga la propagacion y me da la siguiente pose!
+/*** Comprobamos colisiones de esta propagacion del robot, ya que será la que enviemos para que se mueva el robot***/
bool collision_check = check_collision_final( robot_propagation, index ); // porque las personas estaran en ese index cuando llegue el robot a esa posicion final. NO es index=1
@@ -3018,7 +3138,7 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
//}else{
// std::cout << "final_collision_check_=false"<< std::endl;
//}
-
+/*** Esta es porque necesitaba tambien el spoint para otra cosa, pero el que se envia fuera es el Spose que tiene v y w.***/
Spoint robot_propagation_spoint(robot_propagation.x,robot_propagation.y,robot_propagation.time_stamp);
//double distance_to_goal_act = robot_->get_current_pointV().distance( robot_propagation_spoint);
@@ -3026,7 +3146,8 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
//double vel_teoric2=distance_to_goal_act/0.2;
//double error_in_orientation_between_person_and_robot=(person_orient_-robot_propagation.theta)*(180/3.14);
-
+/*** para calcular el goal real del robot (creo), ya que tengo 3 goals: 1- estatico del entorno, 2-dinamico persona que accompaña
+ * 3- goal real del robot (orientado en frente de su trajectoria) ***/
double distance_act_r=initial_robot_spoint_.distance(robot_propagation);
last_step_robot_distance_cost_=distance_act_r;
double dx=robot_propagation.x-initial_robot_spoint_.x;
@@ -3045,9 +3166,8 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
final_combined_goal_=robot_propagation;
- // Para evitar que el robot se mueva cuando esta muy cerca del goal!!!
+/*** Simulación iterativa SOLO: Para evitar que el robot se mueva cuando esta muy cerca del goal!!! ****/
double distance_to_goal_companion=person_companion_goal.distance(initial_robot_spoint_);
-
if(((distance_to_goal_companion<0.3)&&(actual_current_person_comp_point.vx<0.1)&&(actual_current_person_comp_point.vy<0.1))&&(sim)) {
//std::cout << "!!!!!!!! 333333333 777777777777 !!!!!!! replan_last_step companion case v=0; w=0; (d<0.3m)distance_to_goal_companion="<<distance_to_goal_companion<<"; (vx<0.1) actual_current_person_comp_point.vx="<<actual_current_person_comp_point.vx<<"; (vy<0.1) actual_current_person_comp_point.vy="<<actual_current_person_comp_point.vy<< std::endl;
robot_propagation=robot_initial_pose_;
@@ -3060,9 +3180,10 @@ Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini
//std::cout << " (overpas obstacles rear) robot_propagation.x="<<robot_propagation.x<<"; robot_propagation.y="<<robot_propagation.y<<"; act_min_companion_angle"<<act_min_companion_angle<< std::endl;
return robot_propagation;
-}
-
+} // FIN replan_last_step
+/****Es de Gonzalo, para samplear los goals de la local-window y calcula tambien las alpha, betha y gamma para los comportamientos distintos,
+ * mas agresivo y menos agresibo, etc.***/
Spoint Cplan_local_nav::sample_workspace( )
{
Spoint sample;
@@ -3133,7 +3254,7 @@ Spoint Cplan_local_nav::sample_workspace( )
-
+/*** Gonzalo: reset_scene_persons_propagation_flag***/
void Cplan_local_nav::reset_scene_persons_propagation_flag()
{
for( Cperson_abstract* iit : nearby_person_list_ )
@@ -3142,6 +3263,7 @@ void Cplan_local_nav::reset_scene_persons_propagation_flag()
}
}
+/*** Gonzalo: find_nearest_vertex***/
unsigned int Cplan_local_nav::find_nearest_vertex( const Spoint& random_goal )
{
//std::cout << "find_nearest_vertex (1)" << std::endl;
@@ -3167,7 +3289,9 @@ unsigned int Cplan_local_nav::find_nearest_vertex( const Spoint& random_goal )
}
-
+/*** funcion calcula propagacion plan. 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 )
{ // bool reactive is to approximate to the person when robot is far. (Modificated by ely. For person companion.)
@@ -3181,6 +3305,7 @@ Sedge_tree Cplan_local_nav::calculate_edge( unsigned int parent_vertex, const Sd
switch(reactive)
{
+ /*** CASOS AKP-puro: robot se acerca o se aleja de la persona porque este muy cerca.***/
case Cperson_abstract::Reactiva_repulsive:
case Cperson_abstract::Reactive_atractive:
@@ -3228,7 +3353,7 @@ Sedge_tree Cplan_local_nav::calculate_edge( unsigned int parent_vertex, const Sd
break;
-
+/*** CASO AKP-companion: robot navega con la persona (usa fuerza companion)***/
case Cperson_abstract::Akp_planning:
default:
//caso planning, normal Gonzalo.
@@ -3369,6 +3494,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)***/
bool Cplan_local_nav::propagate_vertex( unsigned int parent_index , const Sedge_tree& u, Cperson_abstract::companion_reactive reactive)
{
//robot_->get_current_pose().theta
@@ -3486,7 +3612,9 @@ bool Cplan_local_nav::propagate_vertex( unsigned int parent_index , const Sedge_
return collision_check;
}
-
+/*** HEREDADA directa de GONZALO, excepto añadir el coste companion.
+ * calculate_cost= calcula los costes de este torozo SOLO para las fuerzas.
+ * Es interna a la funcion: cost_to_go***/
void Cplan_local_nav::calculate_cost( unsigned int parent_index , Sedge_tree u , Cperson_abstract::companion_reactive reactive)
{
@@ -3544,14 +3672,16 @@ void Cplan_local_nav::calculate_cost( unsigned int parent_index , Sedge_tree u ,
// Simililarities wrt previous path --------------------------------------------------------------------------
if(reactive==Cperson_abstract::Akp_planning){
Crobot* robot_act=robot_;
- calculate_companion_cost_node(parent_index,robot_act); //funcion (ely) calculate cost companion.
+/*** funcion (ely) calculate cost companion: ***/
+ calculate_companion_cost_node(parent_index,robot_act);
}
//std::cout << " OUT for costs " << std::endl;
}
-
+/*** HEREDADA directa de GONZALO,
+ * creo que es la que calcula el coste en distancia para llegar al path ***/
double Cplan_local_nav::cost_to_go( const Spoint& random_goal, unsigned int i , Cperson_abstract::companion_reactive reactive)
{
double result;
@@ -3753,7 +3883,9 @@ double Cplan_local_nav::cost_to_go( const Spoint& random_goal, unsigned int i ,
return result;
}
-
+/*** 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.***/
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;
@@ -3877,7 +4009,9 @@ unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson
case Cplan_local_nav::MO_erf :
case Cplan_local_nav::MO_norm :
{
- // Entra en esta!
+/***** Entra en esta el código! Esta es la que verdaderamente usa Gonzalo.
+ * Dentro también hay varias opciones, no se seguro si usa solo una o dependen de la situacion, yo incluà el coste companion en todas
+ * para asegurarme y ya esta. *****/
// std::cout << "[Cplan_local_nav::MO_norm/MO_erf]; (usamos MO_erf=3) global_mode_="<< global_mode_ << std::endl;
//first approach: push into a set of best trajectories
nondominated_plan_vertex_index_.clear();
@@ -3923,6 +4057,7 @@ unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson
act_all_ids_path_positions.push_back( nondominated_branch_index);
act_path_positions.push_back(plans_act->at(nondominated_branch_index));
+/*** Calculo de todos los costes de gonzalo, excepto el de companion mio****/
std::vector<double> vect_costst_act;
vect_costst_act.push_back(cost_parameters_[0]*erf((cost_distance_[nondominated_branch_index] - mean_cost_distance_) / std_cost_distance_));
vect_costst_act.push_back(cost_parameters_[1]*erf((cost_orientation_[nondominated_branch_index] - mean_cost_orientation_) / std_cost_orientation_));
@@ -3938,6 +4073,7 @@ unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson
cost_parameters_[5]*erf((cost_obstacles_[nondominated_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_) +
cost_parameters_[6]*erf((cost_past_traj_[nondominated_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_);
+/*** Si tengo coste companion en este trozo de rama, lo añado al coste total ponderandolos****/
double new_cost=cost;
//std::cout << "(ant) [nondominated_branch_index] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
@@ -3974,7 +4110,7 @@ unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson
for( unsigned int i : nondominated_end_of_plan_vertex_index_)
{
//std::cout<<"[end of branch index] i ="<<nondominated_end_of_plan_vertex_index_[i]<< std::endl;
-
+/*** Calculo de todos los costes de gonzalo, excepto el de companion mio****/
cost = cost_parameters_[0]*erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
cost_parameters_[1]*erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
cost_parameters_[2]*erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
@@ -3983,7 +4119,7 @@ unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson
cost_parameters_[6]*erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
//std::cout << "[i] d_cost="<<cost_parameters_[0]*erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_)<<"; or_cost="<<cost_parameters_[1]*erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_)<<"; traj_cost="<<cost_parameters_[6]*erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_)<< std::endl;
-
+/*** Si tengo coste companion en este trozo de rama, lo añado al coste total ponderandolos****/
double new_cost=cost;
//std::cout << "(ant) [i] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
@@ -4140,7 +4276,7 @@ unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson
}
// std::cout << "out NO plans_act2->empty() part2"<< std::endl;
- /* FIN fill best_cost in struct SsavePath_multiple_paths_and_best_path */
+/* FIN fill best_cost in struct SsavePath_multiple_paths_and_best_path */
if(!plans_act2->empty()){
iter_act_multiple_paths_and_best_path_.set_number_of_total_paths(1 + nondominated_end_of_plan_vertex_index_.size() );
//Cperson_abstract* person_obj;
@@ -4150,6 +4286,7 @@ unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson
iter_act_multiple_paths_and_best_path_.set_iter_act(planner_iterations_);
}
+/*** aquà tambien se ha de añadir cualquie nuevo coste, calcula los best, mean and std costs***/
//fill best cost
best_costs_.resize( 7, 0.0 );
best_costs_[0] = cost_distance_[min_cost_index];
@@ -4193,6 +4330,11 @@ unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson
return min_cost_index;
}
+
+/*** si no recuerdo mal, planifica los paths de las otras personas usando las fuerzas. Pero esta es la version que yo modifique
+ * para la companion person con menos fuerza repulsiva hacia el robot (si no la cambiaba, en simulación las personas que lo acompañaban
+ * se alejaban del robot)
+ * La "pura" de gonzalo es la siguiente:force_persons_int_planning_virtual ****/
Sforce Cplan_local_nav::force_persons_int_planning_virtual_vers2comp(Cperson_abstract* center , unsigned int t_companion, unsigned int t_other_pers , double min_dist2, Cperson_abstract::companion_reactive reactive, bool use_person_companion_force_repulsive)
{
// ESTABA AQUI LIMPIANDO CODIGO
@@ -4287,7 +4429,7 @@ Sforce Cplan_local_nav::force_persons_int_planning_virtual_vers2comp(Cperson_abs
return force_res;
}
-
+/*** si no recuerdo mal, planifica los paths de las otras personas (las que tiene que evitar el grupo) usando las fuerzas. ***/
Sforce Cplan_local_nav::force_persons_int_planning_virtual(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive, bool use_person_companion_force_repulsive)
{
@@ -4375,7 +4517,9 @@ Sforce Cplan_local_nav::force_persons_int_planning_virtual(Cperson_abstract* cen
}
-
+/*** calcula la prediccion de las personas, es la que usa el robot para moverse y en caso acercarse o alejarse a la persona
+ * esta tiene la fuerza repulsiva completa,
+ * En el caso acompañamiento tiene fuerza repulsiva reducida como en los papers para que interactuen y no se eviten.***/
Sforce Cplan_local_nav::force_persons_int_planning_virtual_robot_prediction(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive, unsigned int id_pers_comp_rob)
{
@@ -4452,7 +4596,7 @@ Sforce Cplan_local_nav::force_persons_int_planning_virtual_robot_prediction(Cper
return force_res;
}
-
+/*** hay varias de este tipo, ahora mismo no se las diferencias ni si se usan todas aun. ****/
Sforce Cplan_local_nav::force_persons_int_planning_virtual_companion_person_akp_person_prediction(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive)
{
//std::cout << " (INT PERSON FORCES) (1) force_persons_int_planning_virtual_companion_person_akp_person_prediction"<< std::endl;
@@ -4556,7 +4700,7 @@ Sforce Cplan_local_nav::force_persons_int_planning_virtual_robot_companion_propa
}
-
+/*** Heredada de Gonzalo directa. ahora mismo no se que hace ***/
Sforce Cplan_local_nav::force_persons_int_robot_prediction_virtual(const SpointV& center , unsigned int t, double min_dist2)
{
//calculates the resultant forces to all nearby people due to the robot position
@@ -4572,6 +4716,7 @@ Sforce Cplan_local_nav::force_persons_int_robot_prediction_virtual(const SpointV
return force_res;
}
+/*** Heredada de Gonzalo directa. ahora mismo no se que hace ***/
Sforce Cplan_local_nav::force_objects_laser_int_planning_virtual( Cperson_abstract* person, unsigned int planning_index, double min_dist2, bool robot_collision_check_flag)
{ // calculate force for obstacles.
Sforce force_res;
@@ -4617,7 +4762,7 @@ Sforce Cplan_local_nav::force_objects_laser_int_planning_virtual( Cperson_abstra
return force_res;
}
-
+/*** Heredada de Gonzalo directa. ahora mismo no se que hace ***/
Sforce Cplan_local_nav::force_objects_laser_int_planning_virtual_robot_propagation( Cperson_abstract* person, unsigned int planning_index, double min_dist2, bool robot_collision_check_flag)
{
Sforce force_res;
@@ -4662,6 +4807,9 @@ Sforce Cplan_local_nav::force_objects_laser_int_planning_virtual_robot_propagati
return force_res;
}
+
+/*** check_collision: comprueba las colisiones entre grupo - obstaculos (static y dinamic). De estas hay varias, ya que una comprueba las colisiones directas con el robot y
+ * otra las colisiones del grupo, que son colisiones ampliadas al radio del grupo persona-robot. ****/
bool Cplan_local_nav::check_collision( const SpointV_cov& p2 , int t, SpointV_cov centre_group)
{ // CHECK collision of the path for the robot shape and for the companion shape (to calculate the companion cost and angle, before)
// IMPORTANT: Do not habilitate all the comments in loops for! Hight computational cost!. Then the robot do not work propertly and it is not fault of the method.
@@ -4695,6 +4843,8 @@ bool Cplan_local_nav::check_collision( const SpointV_cov& p2 , int t, SpointV_c
std::vector<double> min_distances_vector;
min_distances_vector.clear();
bool_collision_companion_=false;
+
+/***collision_threshold= radio de colision ampliado, para el grupo (persona-robot) ****/
double collision_threshold=(real_robot_person_distance+2*robot_radi+little_augmented_collision_margin_)/2;
// End: Init values of companion collisions
@@ -4749,6 +4899,8 @@ bool Cplan_local_nav::check_collision( const SpointV_cov& p2 , int t, SpointV_c
}
//END: debug path of the companion collisions calculation
+/*** es diferente para person companion (puede colisionar con el path), porque si no el robot no podia evitar obstaculos detras de la persona
+ * que acompañaba. ****/
//if t = -1, then no collision is calculated wrt ppl
if ( t > -1 )
{
@@ -4966,6 +5118,7 @@ bool Cplan_local_nav::check_collision( const SpointV_cov& p2 , int t, SpointV_c
}
+/*** check_collision_final: comprueba las colisiones entre robot - obstaculos (static y dinamic). ****/
bool Cplan_local_nav::check_collision_final( const Spoint& p , int t)
{
//std::cout << " IN check_collision_final" << std::endl;
@@ -5071,7 +5224,7 @@ bool Cplan_local_nav::check_collision_final( const Spoint& p , int t)
return false;
}
-
+/*** Heredada de Gonzalo, hace esto: set_number_of_vertex ***/
void Cplan_local_nav::set_number_of_vertex( unsigned int n )
{
max_iter_ = n;
@@ -5089,6 +5242,7 @@ void Cplan_local_nav::set_number_of_vertex( unsigned int n )
}
+/*** Heredada de Gonzalo, hace esto: preprocess_global_parameters ***/
void Cplan_local_nav::preprocess_global_parameters(Crobot* robot_act)
{
switch( global_mode_)
@@ -5112,6 +5266,7 @@ void Cplan_local_nav::preprocess_global_parameters(Crobot* robot_act)
}
}
+/*** Heredada de Gonzalo, hace esto: calculate_non_dominated_solutions ***/
void Cplan_local_nav::calculate_non_dominated_solutions()
{
//fill the multiobjective cost structure
@@ -5181,6 +5336,9 @@ void Cplan_local_nav::calculate_non_dominated_solutions()
//std::cout << "size of the non-dominated set = " << set.size() << " / " << end_of_branches_index_.size() << std::endl;
}
+/*** Heredada de Gonzalo, hace esto: calculate_normalization_cost_functions_parameters_erf;
+ * Solo añadi mi coste "copiando" lo que él hacia, para incluirlo en la normalización.
+ * Al añadir mi coste seguramente que es la única que usa de las diferentes que hay en el código***/
void Cplan_local_nav::calculate_normalization_cost_functions_parameters_erf()
{
mean_cost_int_forces_=0.0;
@@ -5303,6 +5461,7 @@ void Cplan_local_nav::calculate_normalization_cost_functions_parameters_erf()
//std::cout << "cost_companion = (" << mean_cost_companion_ << " , " << std_cost_companion_ << std::endl;
}
+/*** Heredada de Gonzalo, hace esto: calculate_normalization_cost_functions_parameters_norm ***/
void Cplan_local_nav::calculate_normalization_cost_functions_parameters_norm(Crobot* robot_act)
{
//calculates the Utopia point considering free space and the max value in order to linearly normalize ->[0,1]
@@ -5395,7 +5554,7 @@ void Cplan_local_nav::calculate_normalization_cost_functions_parameters_norm(Cro
//std::cout << "cost_past_traj = (" << mean_cost_past_traj_ << " , " << std_cost_past_traj_ << std::endl;
}
-
+/*** calcula la best pose final para el robot. ****/
Spose Cplan_local_nav::get_best_planned_pose(double dt, Cperson_abstract::companion_reactive reactive)
{
//std::cout << " IN get_best_planned_pose"<< std::endl;
@@ -5494,6 +5653,10 @@ Spose Cplan_local_nav::get_best_planned_pose(double dt, Cperson_abstract::compan
double robot_person_distance;
Spose robot_act;
+/*** aquà para cada uno de los 3 casos: AKP-plan, repulsivo=se-aleja y atractivo==se-acerca-goal-lateral
+ * Calculo la posicion futura del robot respecto a la posicion de la persona que acompaña, teniendo en cuenta la distancia entre ellos
+ * y el angulo de acompañamiento.
+ * tambien se guardan el coste minimo companion proximo y el angulo minimo proximo, que se usará en la siguiente iteración (si no recuerdo mal) ***/
if(reactive==Cperson_abstract::Akp_planning){
next_plan_index_companion_for_robot_velocity=BEST_path_parent_index_vector_.at( BEST_path_parent_index_vector_.size() - index_companion_for_robot_velocity -1 );
next_plan_index_companion = BEST_path_parent_index_vector_.at( BEST_path_parent_index_vector_.size() - index_companion -1 );
@@ -5737,10 +5900,10 @@ Spose Cplan_local_nav::get_best_planned_pose(double dt, Cperson_abstract::compan
return robot_act;
}
else
- return Spose();
+ return Spose(); // Heredado: creo que es para el caso sin paths o con colisiones.
}
-
+/*** Heredada de Gonzalo, hace esto: get_navigation_instant_work ***/
void Cplan_local_nav::
get_navigation_instant_work( double& work_robot, double& work_persons )
{
@@ -5748,7 +5911,7 @@ get_navigation_instant_work( double& work_robot, double& work_persons )
work_persons = work_persons_;
}
-
+/*** Heredada de Gonzalo, hace esto: calculate_navigation_instant_work ***/
void Cplan_local_nav::
calculate_navigation_instant_work( )
{
@@ -5772,7 +5935,8 @@ calculate_navigation_instant_work( )
}
}
-
+/*** Heredada de Gonzalo, hace esto: set_robot_params (setea los limites de las velocidades.
+ * Tambien tube que incluir los limites que yo necesitaba para los frenados suabes.***/
void Cplan_local_nav::set_robot_params( double v, double w, double av, double av_break, double aw, double platform_radii, double av_neg, double av_Vrobotzero, double lim_to_consider_robotZero)
{
robot_->set_v_max( v );
@@ -5801,7 +5965,7 @@ void Cplan_local_nav::set_plan_cost_parameters( double c_dist, double c_orientat
cost_parameters_[7] = c_l_minima;// [7] local minima scape cost
}
-//only use when no plan is calculated, as an evaluation of the observed performance
+//Heredada de Gonzalo directa: only use when no plan is calculated, as an evaluation of the observed performance
// of this method or any navigation method
void Cplan_local_nav::calculate_navigation_cost_values( std::vector<double>& costs )
{
@@ -5855,12 +6019,15 @@ void Cplan_local_nav::calculate_navigation_cost_values( std::vector<double>& cos
}
+/*** Heredada de Gonzalo, hace esto: Sedge_tree (solo incluà la f_persongoal igual que las otras f's que tenÃa él) ***/
Sedge_tree::Sedge_tree( unsigned int parent_, Sforce f_, Sforce f_goal_,
Sforce f_people_,Sforce f_obs_, Sforce f_persongoal_):
parent(parent_), f(f_), f_goal(f_goal_), f_people(f_people_), f_obs(f_obs_),f_persongoal(f_persongoal_)
{
}
+
+/*** SIGUIENTES Heredadas de Gonzalo, hacen lo que pone el titulo ***/
double Sedge_tree::cost( Sedge_tree parent) const
{
@@ -5929,7 +6096,8 @@ void Smulticost::print_ml ( ) const
-/*** Ini check_collision_companion_reactive_atractive_goal! (ely-modificada, solo) ***/
+/*** Ini check_collision_companion_reactive_atractive_goal! (ely-modificada, solo) Para ese caso:reactive_atractive,
+ * se necesitaba otra gestion diferente de las colisiones que las otras funciones de colision***/
bool Cplan_local_nav::check_collision_companion_reactive_atractive_goal( const SpointV_cov& p2 , int t)
// return if have collision and the minimum distance of the collision
{
@@ -6129,7 +6297,8 @@ bool Cplan_local_nav::check_collision_companion_reactive_atractive_goal( const S
/*** Fin Check check_collision_companion_reactive_atractive_goal! ***/
-/*** Ini Check collision companion! (ely-modificada, solo, esta vez para calculo colisiones robot goal respecto persona, cuando esta lejos de esta) ***/
+/*** (IGUAL que en el caso anterior, modificada porque cambien colisiones de algún modo)
+ * Ini Check collision companion! (ely-modificada, solo, esta vez para calculo colisiones robot goal respecto persona, cuando esta lejos de esta) ***/
double Cplan_local_nav::check_collision_companion_goal(const Spoint& p , int t)
// return if have collision and the minimum distance of the collision
{
@@ -6491,6 +6660,7 @@ void Cplan_local_nav::calculate_companion_cost_node( unsigned int parent_index,
}
/*** FIN calculate_companion_cost_node! ***/
+/*** Calculo incremento de angulo para acompañamiento adaptativo "smooth" == los distintos angulos que se calcularan hacia el futuro para posicionar al robot detras para evitar obstaculos.***/
void Cplan_local_nav::ini_increment_angle(){
// version 1: con velocidad maxima de las personas.
//std::cout << "*** ini_increment_angle() ***" << std::endl;
@@ -6572,7 +6742,8 @@ void Cplan_local_nav::ini_increment_angle(){
// std::cout << "TTTTTTTTTTTTTTTT (real_FINAL) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< std::endl;
}
-
+/*** usando el coste/colisiones calculamos el angulo dinamico de acompañamiento para el robot respecto a la persona para todos los caminos
+ * calculados en el planner. ****/
void Cplan_local_nav::calculate_companion_path_angle_and_cost(unsigned int parent_index, double actual_angle){
// calcula el coste to add en el caso en que haya gran diferencia entre el angulo(robot<->persona) comparando el del nodo actual y el del nodo anterior.
// calcula los angulos que hay que moverse en ese path.
@@ -6615,7 +6786,8 @@ void Cplan_local_nav::calculate_companion_path_angle_and_cost(unsigned int paren
}
-/* Arreglar angulos para pasar detras del robot */
+/* Arreglar angulos para pasar detras del robot (la S más suabe que tube que implementar, para que se posicionara mejor detras de la persona
+ * sin saltos, todo más suabe) */
void Cplan_local_nav::go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i){
// calcula los angulos que hay que moverse en ese path.
//std::cout << " !!! INI go_behind_robot-only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
@@ -6858,7 +7030,8 @@ void Cplan_local_nav::go_behind_robot_only_angle_in_final_tree_calculate_compani
-/* Arreglar angulos para pasar detras del robot */
+/* Arreglar angulos para pasar detras del robot (Caso robot delante == no se suele usar ya que el robot es más lento y preferimos
+ * usar siempre el pasar obstaculos detras de la persona, pero esta guardada por si algún dÃa se necesitara... */
void Cplan_local_nav::go_in_front_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i){
// calcula los angulos que hay que moverse en ese path.
@@ -7025,7 +7198,8 @@ void Cplan_local_nav::go_in_front_robot_only_angle_in_final_tree_calculate_compa
-
+/*** para ver la sequencia de angulos de acompañamiento del path de minimo coste, era para comprobar que evolucionaran los angulos de forma
+ * más suabe. ****/
void Cplan_local_nav::see_companion_path_angle_and_cost_of_min_cost_paths(double parent_index){
std::cout << "INI see_companion_path_angle_and_cost_of_min_cost_paths "<< std::endl;
std::cout << "orientation_person_robot_angles_.size()= "<<orientation_person_robot_angles_.size()<< std::endl;
@@ -7065,7 +7239,8 @@ void Cplan_local_nav::see_companion_path_angle_and_cost_of_min_cost_paths(double
}
-
+/*** Calcular distancia real de acompañamiento entre robot y persona. (se necesita seguro, ahora no se si para corregirla o si tambien para
+ * saber en que lado se ha de poner el robot.***/
double Cplan_local_nav::calc_robot_person_companion_distance(){
double robot_person_distance;
companion_person_position_=actual_person_Companion_SpointV_;
@@ -7080,10 +7255,7 @@ double Cplan_local_nav::calc_robot_person_companion_distance(){
return robot_person_distance;
}
-
-
-
-
+/**** es muy similar a la anterior, ahora mismo no se porque... ni si aun se usa.***/
double Cplan_local_nav::calc_robot_person_companion_distance_companion_person_akp(){
double robot_person_distance;
@@ -7097,8 +7269,7 @@ double Cplan_local_nav::calc_robot_person_companion_distance_companion_person_ak
}
-// velocidad robot!!!
-
+/**** velocidad robot para caso robot navegando solo ***/
void Cplan_local_nav::vel_robot_normal(double d_min){
// calculate the desired robot velocity depending on the nearbiest obstacle/person and goal
double distance_to_goal = robot_->get_current_pointV().distance( goal_);
@@ -7122,7 +7293,9 @@ void Cplan_local_nav::vel_robot_normal(double d_min){
}
-
+/**** calculamos la velocidad limite real durante el acompañamiento (es decir se reduce la velocidad máxima del robot
+ * para acompañar bien a la persona cuando esta con muy poquisimo error respecto a la posicion ideal (side-by-side
+ * y formaciones intermedias también. Recordad, es adaptativo de formación durante el acompañamiento para evitar obstaculos conjuntamente.) ****/
void Cplan_local_nav::vel_robot_companion(double d_min , double robot_person_distance, Spoint before_next_goal_of_robot, Cperson_abstract::companion_reactive reactive, unsigned int parent_vertex)
{
//std::cout <<"IMPORTANTE IMPORTANTE in (function: vel_robot_companion) robot_->get_des_vel="<< robot_->get_desired_velocity()<< std::endl;
@@ -7285,6 +7458,8 @@ void Cplan_local_nav::vel_robot_companion(double d_min , double robot_person_dis
//std::cout << " OUT CHANGE VEL person!!!! ; v_max_="<<robot_->get_v_max()<<"; max_v_by_system_="<<max_v_by_system_<< std::endl;
}
+/**** volver a la velocidad normal del robot (ya que usamos velicidad limitada adaptada a la persona, pero para calcularla al inicio de la iteracion
+ * el robot ha de volver a su velocidad máxima normal***/
void Cplan_local_nav::return_max_velocity_systemRobot_to_max_value(){
// calculate the desired robot velocity depending on the nearbiest obstacle/person and goal
@@ -7356,7 +7531,7 @@ void Cplan_local_nav::calculate_actual_angle_person_robot(unsigned int index){
// std::cout << " max_v_by_system_="<<max_v_by_system_<< std::endl;
}*/
-
+/*** Para calcular el angulo real actual entre el robot y la persona que acompaña***/
double Cplan_local_nav::calculate_actual_angle_person_robot(Spoint person, Spoint robot){
//std::cout << " !!! calculate_actual_angle_person_robot() !!!" << std::endl;
@@ -7398,7 +7573,7 @@ double Cplan_local_nav::calculate_actual_angle_person_robot(Spoint person, Spoin
}
-
+/*** Heredada directa de gonzalo. Hace esto: calculate_anisotropy para las fuerzas. ***/
double Cplan_local_nav::calculate_anisotropy( const SpointV_cov& center_person, const Spoint& interacting_person , double lambda )
{
//setting the corresponding SFM parameters:
@@ -7429,7 +7604,8 @@ double Cplan_local_nav::calculate_anisotropy( const SpointV_cov& center_person,
-
+/*** es interna al calculo de la posicion futura en todo el path, se reduce o amplia un poco la distancia de acompañamiento segun los angulos
+ * para conseguir una S más suabe a la hora de posicionarse detras de la persona que acompaña. ***/
double Cplan_local_nav::calculate_modif_person_robot_distance( double ite )
{
@@ -7618,7 +7794,7 @@ double Cplan_local_nav::calculate_modif_person_robot_distance( double ite )
}
-//////////////////////////////
+/**** Funcion que guarda los datos en Matlab ****/
void Cplan_local_nav::printToMatlab()
{
@@ -7898,7 +8074,7 @@ void Cplan_local_nav::printToMatlab()
}
-
+/*** creamos un debug file. es como el .txt de datos finales a analizar del robot, pero para debugar. ***/
void Cplan_local_nav::new_debug_file()
{
std::ofstream fileMatlab2;
@@ -7909,6 +8085,7 @@ void Cplan_local_nav::new_debug_file()
+/*** creamos un .txt de datos finales a analizar del robot***/
void Cplan_local_nav::new_matlab_file()
{
std::ofstream fileMatlab;
@@ -8023,6 +8200,8 @@ void Cplan_local_nav::new_matlab_file()
}
+
+/*** creamos un .txt de datos finales a analizar los costes del robot***/
void Cplan_local_nav::new_matlab_file_To_evaluate_costs()
{
@@ -8053,6 +8232,7 @@ void Cplan_local_nav::new_matlab_file_To_evaluate_costs()
+/*** creamos un .txt de datos finales a analizar los cambios de distancia y angulo de acompañamiento del robot***/
void Cplan_local_nav::new_matlab_file_To_evaluate_change_distance_and_angle()
{
std::ofstream fileMatlab;
@@ -8065,6 +8245,7 @@ void Cplan_local_nav::new_matlab_file_To_evaluate_change_distance_and_angle()
fileMatlab << " angle_companion_robot_person=[];\n";
}
+/*** creamos un .txt de datos finales a analizar los cambios de distancia y angulo de acompañamiento del robot con cambios de la beta (ponderacion-fuerza-companion)***/
void Cplan_local_nav::evaluate_change_distance_and_angle_companion_with_beta_change()
{
std::ofstream fileMatlab;
@@ -8138,7 +8319,7 @@ void Cplan_local_nav::evaluate_change_distance_and_angle_companion_with_beta_cha
}
-
+/*** rellenamos el .txt de datos para analizar los costes***/
void Cplan_local_nav::evaluate_costs_printToMatlab(Crobot* actual_robot)
{
std::ofstream fileMatlab;
@@ -8300,7 +8481,7 @@ void Cplan_local_nav::evaluate_costs_printToMatlab(Crobot* actual_robot)
}
-
+/*** calculamos la orientacion de la persona a la que acompañamos****/
double Cplan_local_nav::calc_person_companion_orientation(){ // return theta person companion in radians!
// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
//std::cout <<" IN: calc_person_companion_orientation"<< std::endl;
@@ -8413,8 +8594,7 @@ double Cplan_local_nav::calc_person_companion_orientation(){ // return theta per
-
-
+/*** IDEM (pero se usa en ROS) calculamos la orientacion de la persona a la que acompañamos****/
double Cplan_local_nav::calc_person_companion_orientation_from_outside( SpointV actual_p1_point, Sdestination person_dest, Spose in_robot){ // return theta person companion in radians!
//std::cout <<"IN: calc_person_companion_orientation_from_outside"<< std::endl;
// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
@@ -8461,7 +8641,7 @@ double Cplan_local_nav::calc_person_companion_orientation_from_outside( SpointV
}
-
+/*** para calcular la posicion del goal del robot, si la persona se para.***/
Sdestination Cplan_local_nav::calculate_companion_goal_stable_if_person_stop(double in_act_min_companion_angle){
//std::cout << " IDEAL FINAL ACT in_act_min_companion_angle="<<in_act_min_companion_angle<< std::endl;
@@ -8687,7 +8867,7 @@ void Cplan_local_nav::fix_angles_to_use_person_prediction_for_the_companion_goal
/////////////// end: function fix angles for prediction (vuelta hacia delante)
}
-
+/*** Siguientes: funciones heredadas directas de gonzalo, hacen lo que dicen. ***/
// Function to find mean.
double Cplan_local_nav::mean(std::vector<double> arr, int n)
{
@@ -8756,7 +8936,8 @@ SpointV Cplan_local_nav::calculate_centre_of_the_group(SpointV robot, std::vecto
}
-// Function to calculate the final goal translated for the position of the element=person/robot of the group. Transtated to be liniarly respect to this element of the group.
+/* Function to calculate the final goal translated for the position of the element=person/robot of the group.
+Transtated to be liniarly respect to this element of the group. (goal dinamico respecto del estatico, para la persona y para el robot*/
Spoint Cplan_local_nav::calculate_goal_translated_of_group_element(SpointV group_centre, SpointV group_element, Spoint group_goal, Spoint distRP, double theta_robot, unsigned int parent_vertex)
{ // group element == robot or person of the group.
// return el goal trasladado para este elemento del grupo.
@@ -8887,7 +9068,7 @@ double Cplan_local_nav::calc_desired_robot_velocity_act(SpointV robot, SpointV p
}
-
+/*** calcular angulo inicial del companion en caso solo un acompañante ***/
void Cplan_local_nav::calculate_initial_companion_angle_case_side_by_side_one_person(double theta, double angle2){
// Ini copiar
double ini_angle_act;
@@ -9098,7 +9279,7 @@ void Cplan_local_nav::calculate_correct_initial_angle(Cperson_abstract* person_o
////////// Fin calculo correcto initial angle!!! //////////////////////////////////////
}
-
+/***basicamente hace esto: set_robot_velocity_using_obstacles ***/
void Cplan_local_nav::set_robot_velocity_using_obstacles(double vel_per, double near_obst, double actual_V_Robot, double distance_PR7, double robot_person_distance, double d_min, double min_dist_colli_act_global2, double circunferencia_tope, double margen_angle_colision, double angle_colision) // funcion que setea la velocidad del robot a la deseada actual + alguna variable global relacionada con la velocidad que necesita el robot.
{
//std::cout << " (before obstacles) vel_per="<<vel_per<<"; d_min="<<d_min<< std::endl;
diff --git a/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.cpp b/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.cpp
index 4f446e0021269ded594ca5de862f10e1c48d862e..07c532641641a5bd67eb314c76a643fe8b3c2136 100644
--- a/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.cpp
+++ b/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.cpp
@@ -2,7 +2,8 @@
* plan_local_nav_person_companion.cpp
*
* Created on: Jan 5, 2019 by Ely Repiso to separate the elements of the Scene_sim for the person companion, from the planner elements of the robot.
- *
+ *=> es para la simulacion de la persona que acompaña al robot, que se mueva un poco más inteligente que las otras personas,
+ *=> al usar un akp-planner para moverse.
*/
#include "nav/plan_local_nav_person_companion.h"
diff --git a/local_lib_people_prediction/src/prediction_behavior.cpp b/local_lib_people_prediction/src/prediction_behavior.cpp
index 588df67a8dddec482e012df77ffca3704b0b96ff..dfa0d0b35c160c4622d0b25b214eaac7bd9cf952 100644
--- a/local_lib_people_prediction/src/prediction_behavior.cpp
+++ b/local_lib_people_prediction/src/prediction_behavior.cpp
@@ -64,7 +64,7 @@ Cperson_abstract* Cprediction_behavior::add_person_container( unsigned int id,
return (Cperson_abstract*) person;
}
-
+/*** predice las destinaciones finales de TODAS las personas en la escena/entorno. ***/
void Cprediction_behavior::scene_prediction(bool person_or_robot, std::vector<Sdestination>* person_best_dest) // person_or_robot= true==person_companion. False==robot
{
@@ -257,6 +257,8 @@ Cprediction_behavior::get_sfm_int_params( const Cperson_abstract * center_person
return result;
}
+
+/*** predice las trajectorias futuras de TODAS las personas en la escena. ***/
void Cprediction_behavior::scene_trajectory_prediction( )
{
//std::cout<<" DO scene_trajectory_prediction( ) horizon_time_index_= "<<horizon_time_index_<< std::endl;
@@ -417,8 +419,7 @@ void Cprediction_behavior::scene_trajectory_prediction( )
}
-
-
+/***PARA SIMULACION ITERATIVA: predice las trajectorias futuras de TODAS las personas en la escena. ***/
void Cprediction_behavior::scene_trajectory_prediction_simulation( )
{
//std::cout << "2222222 Entro en Cprediction_behavior::scene_trajectory_prediction_simulation"<< std::endl;
@@ -587,7 +588,7 @@ void Cprediction_behavior::scene_trajectory_prediction_simulation( )
}
}
-
+/***incluye fuerzas de obst estaticos detectados por el laser en la prediccion. ***/
Sforce Cprediction_behavior::force_objects_laser_int_prediction( Cperson_abstract* person, unsigned int prediction_index)
{
Sforce force_res;
@@ -647,7 +648,7 @@ Sforce Cprediction_behavior::force_objects_laser_int_prediction( Cperson_abstrac
return force_res;
}
-
+/***incluye fuerzas de otras personas en la prediccion. ***/
Sforce Cprediction_behavior::force_persons_int_virtual(Cperson_abstract* center , unsigned long t)
{
//std::cout << " ENTRO EN =>>>> force_persons_int_virtual=" << std::endl;
@@ -711,7 +712,7 @@ Sforce Cprediction_behavior::force_persons_int_virtual(Cperson_abstract* center
///
-
+/***incluye fuerzas respecto a la persona que acompaña el robot en la prediccion. ***/
Sforce Cprediction_behavior::force_persons_int_virtual_person_comp(Cperson_abstract* center , unsigned long t)
{
//std::cout << " ENTRO EN =>>>> force_persons_int_virtual=" << std::endl;
@@ -774,7 +775,7 @@ Sforce Cprediction_behavior::force_persons_int_virtual_person_comp(Cperson_abstr
////
-
+/**** cambia el tiempo horizonte del local planner.***/
void Cprediction_behavior::set_horizon_time( double t )
{
horizon_time_ = t;
diff --git a/local_lib_people_prediction/src/prediction_bhmip.cpp b/local_lib_people_prediction/src/prediction_bhmip.cpp
index f4a90f93c2b3be33c1967620a585b2ac8cf680da..d47db656e5e0f80293b62815dc84a5282c23c6ae 100644
--- a/local_lib_people_prediction/src/prediction_bhmip.cpp
+++ b/local_lib_people_prediction/src/prediction_bhmip.cpp
@@ -52,7 +52,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.***/
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 1a58c9333f762264ed6f38cacb3cdab2f93e3cc8..b3b6912126f4a6901a8a77570b072bcb3267ee49 100644
--- a/local_lib_people_prediction/src/scene_abstract.cpp
+++ b/local_lib_people_prediction/src/scene_abstract.cpp
@@ -135,6 +135,7 @@ void Cscene_abstract::remove_person( unsigned int id )
}
}
+/*** update de la posicion del robot para esta iteracion. Explicada en el nodo.***/
void Cscene_abstract::update_robot(Spose observation, bool case_zanlungo)
{
@@ -170,7 +171,7 @@ void Cscene_abstract::update_robot(Spose observation, bool case_zanlungo)
//std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!111 update_robot !!!!!!!!!!!!!!!!!!1111"<< std::endl;
//std::cout << "observation.w="<<observation.w<<"; observation.v="<<observation.v<<"; robot.w="<<robot_->get_current_pose().w<<"; robot.v="<<robot_->get_current_pose().v<< std::endl;
}
-
+/*** update de la posicion de la person-companion para esta iteracion. Explicada en el nodo.***/
void Cscene_abstract::update_person_companion(Spose observation, SpointV_cov obs_spoint)
{
if(debug_update_person_companion_){
@@ -201,6 +202,8 @@ void Cscene_abstract::update_person_companion(Spose observation, SpointV_cov obs
}
}
+
+/*** te busca una persona de todas las que hay detectadas en el entorno usando su id***/
bool Cscene_abstract::find_person(unsigned int id)
{
std::list<Cperson_abstract*>::iterator iit = person_list_.begin();
@@ -223,6 +226,7 @@ bool Cscene_abstract::find_person(unsigned int id)
return false;
}
+/*** te busca una persona de todas las que hay detectadas en el entorno usando su id y retorna el puntero a esta y un bool de si la encontro o no***/
bool Cscene_abstract::find_person(unsigned int id , Cperson_abstract** person) // (ely) suelo entrar en esta find person!!!
{
if(debug_find_person_){
@@ -257,7 +261,7 @@ bool Cscene_abstract::find_person(unsigned int id , Cperson_abstract** person) /
}
return false;
}
-
+/*** te busca una persona de todas las que hay detectadas en el entorno usando su id y retorna el puntero a esta y un bool de si la encontro o no; y it=posicion de esta en el container.***/
bool Cscene_abstract::find_person(unsigned int id , Cperson_abstract** person,
std::list<Cperson_abstract*>::iterator& it)
{
@@ -296,6 +300,8 @@ void Cscene_abstract::print()
}
}
+/*** lee un scan laser del robot y genera todos los cilindros de los obstaculos estaticos (a menos distancia del radio de deteccion de obstaculos
+ * == mirar documentacion sobre parametros), no genera obstaculo estatico en la misma posicion que existe un track de una persona. ***/
void Cscene_abstract::read_laser_scan( const std::vector<Spoint>& laser_scan , bool person_simulation)
{
@@ -432,6 +438,9 @@ void Cscene_abstract::read_laser_scan( const std::vector<Spoint>& laser_scan ,
read_laser_obstacle_success_ = true;//flag to indicate that indeed we can make use of laser information regarding obstacles.
}
+/*** (hace lo mismo que la anterior, pero desde el punto de vista de la person companion. Es para SIMULACION-iteratiba==person-companion==robot-dabo)
+ * lee un scan laser del robot y genera todos los cilindros de los obstaculos estaticos (a menos distancia del radio de deteccion de obstaculos
+ * == mirar documentacion sobre parametros), no genera obstaculo estatico en la misma posicion que existe un track de una persona. ***/
void Cscene_abstract::read_laser_scan_person_companion_akp_planner( const std::vector<Spoint>& laser_scan , bool person_simulation)
{
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 a0940ffc125bcc8428eb846f3551743731ba3a96..8510bb4975d55172cd285bd7e46fd05afba509a1 100644
--- a/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp
+++ b/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp
@@ -31,6 +31,8 @@ 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.***/
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;
@@ -335,7 +337,7 @@ void Cperson_bhmip::Cperson_bhmip_add_pointV( SpointV_cov point, Cperson_abstrac
}
}
-
+/// Los diferentes filtros...
SpointV_cov Cperson_bhmip::filter_current_state_linear_regression( )
{
@@ -631,6 +633,8 @@ void Cperson_bhmip::intention_precalculation()
}
+
+/*** Eliminar posiciones de las personas muy antiguas para mantener el mismo tamaño de ventana siempre ***/
void Cperson_bhmip::trajectory_windowing()
{
if(debug_trajectory_windowing_){
diff --git a/local_lib_people_prediction/src/scene_elements/robot.cpp b/local_lib_people_prediction/src/scene_elements/robot.cpp
index 0099b31dac50819a88a8236bca3e954109e4a0cc..61c4989cb8cc9ddf63c30be194276d14f48ad3f7 100644
--- a/local_lib_people_prediction/src/scene_elements/robot.cpp
+++ b/local_lib_people_prediction/src/scene_elements/robot.cpp
@@ -137,7 +137,9 @@ 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).***/
void Crobot::robot_propagation(double dt , unsigned int index, const Sforce &f , bool debug_force_vel) // con fuera entro en esta!!!
{
//kinodynamic unicycle
@@ -386,6 +388,9 @@ void Crobot::correct_state_to_delay( Spose last_control_cmd, double now, double
//TODO propagate state {x,y,theta}, not implemented yet
}
+
+/*** La función que usa el companion para propagar la siguiente posicion del robot, con todas las fuerzas del companion y
+ * además con los limites de aceleración necesarios para reducir saltos del robot (de normal( y al parar la persona. ***/
Spose Crobot::robot_propagation_companion_position(double dt, const Sforce& f ,Spose in_ini_pose, bool coments){ // made by (ely) for robot companion.
//kinodynamic unicycle
if(debug_antes_subgoals_entre_AKP_goals_){
diff --git a/local_lib_people_prediction/src/scene_sim.cpp b/local_lib_people_prediction/src/scene_sim.cpp
index 76b78fb4f2adffb23aca5844e21e1ee5ebd7bf10..589d651de5f69b7a3186242a3912fef00af353f0 100644
--- a/local_lib_people_prediction/src/scene_sim.cpp
+++ b/local_lib_people_prediction/src/scene_sim.cpp
@@ -3,6 +3,7 @@
*
* Created on: Mar 27, 2013. Modified on 2016 (final review on 2020)
* Author: Initial AKP code Gonzalo Ferrer, addition of code and maintenance of companion Ely Repiso (ASSAOP and ASSAGP)
+ * ===> es para simulacion iteratiba. Ahora mismo no hace falta y la usa otro nodo de simulacion de personas.
*/
#include "scene_sim.h"
#include "scene_elements/person_virtual.h"
diff --git a/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp
index 1d14d327f6fb513f82908768b6c90a13dc30171e..db9b824e5fce498cf042ed62b7c5ac6b3bb1186c 100644
--- a/src/akp_local_planner_alg_node.cpp
+++ b/src/akp_local_planner_alg_node.cpp
@@ -198,6 +198,7 @@ AkpLocalPlanner::~AkpLocalPlanner(void)
}
+//*** Def of init(): includes all definitions of publishers and subscrivers. ***/
void AkpLocalPlanner::init()
{
//if(output_screen_messages_){
@@ -252,6 +253,7 @@ void AkpLocalPlanner::init()
ROS_INFO("OUT tibi! AkpLocalPlanner init()");
}
+//*** Def of initialize: Includes all initializations of parameters (maximum velocity, SFM-parameters, etc. ***//
void AkpLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)
{
ROS_INFO("IN tibi! AkpLocalPlanner initialize()");
@@ -452,6 +454,8 @@ void AkpLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_
}
+
+/*** Def of computeVelocityCommands: main function of ROS node that includes the planner library function ***/
bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
if(debug_all_IVO_){
@@ -542,7 +546,8 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
}
double START_time_secs_read_scan =ros::Time::now().toSec();
- //update front and rear obstacles
+
+ /*** update front and rear obstacles (for internal static obstacles) ****/
this->laser_points.clear();
this->fscan_mutex_enter();
@@ -585,7 +590,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
//ROS_INFO( " before update_robot, robot.x=%f, y=%f; time=%f; robot.w=%f, robot.v=%f ",this->robot_pose_.x,this->robot_pose_.y,this->robot_pose_.time_stamp,this->robot_pose_.w,this->robot_pose_.v);
- // Function that updates the robot status.
+ /*** Function that updates the robot status (robot position) inside the environment ***/
this->planner_.update_robot(this->robot_pose_);
this->odom_mutex_exit();
@@ -597,7 +602,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
}
- /// INI ONLY first iteration. Update scene with obs() directly, to generate fake person if needed.
+ /*** INI ONLY first iteration. Update scene with obs() directly, to generate fake person if needed. ***/
if(first_iter_for_update_scene_){ // function that updates the scene.
this->planner_.update_scene(this->obs,this->we_have_companion_person_);
first_iter_for_update_scene_=false;
@@ -675,6 +680,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
double START_time_secs_updateScene =ros::Time::now().toSec();
// ROS_INFO(" Antes de update_scene; this->obs2.size()=%d",this->obs2.size());
+ //*** Update the scene para detectar las personas del entorno ***/
this->planner_.update_scene(this->obs2,this->we_have_companion_person_);
double END_time_secs_updateScene;
@@ -699,7 +705,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
Action_ROS_=this->planner_.get_state_Action();
Actual_case_ROS_=this->planner_.get_actual_case();
if(simulation_){
- /* INI server to use the same state in people simulation and robot */
+ /**** This is for the iterative simulation: INI server to use the same state in people simulation and robot ***/
// first know what state is now!!!
// + generar el mensaje a enviar al server.
Spoint person_companion_point1=this->planner_.get_SIM_initial_person_companion_pose1();
@@ -806,7 +812,8 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
//}
- // Function that computes the local-planner, the Robot's Adaptive Side-by-Side Accompaniment of One Person (ASPSI)
+//***(C++ LIBRARY of the planner) Function robot_plan_companion3 that computes the local-planner, the Robot's Adaptive Side-by-Side Accompaniment of One Person (ASPSI) ***/
+ // robot_plan_succed= true if the robot finds a path, false if robot do not have path.
robot_plan_succed = this->planner_.robot_plan_companion3(best_next_pose,reactive_);
ros_time_to_sec_general_before_iter_=ros::Time::now().toSec();
@@ -1027,7 +1034,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
this->planner_mutex_exit();
-
+/*** Multiplicar velocidades del robot real por division de velocidad del controlador y limitar velocidades del robot real. ***/
cmd_vel.linear.x*=this->config_.speed_k; // IMPORTANT, do not change for TIBI. It is for real robot. OUR velocity controller divides the robot's velocity by 2. Then, we need to multiply with this speed_k here.
cmd_vel.angular.z*=this->config_.speed_k_angular;
@@ -1132,6 +1139,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
ROS_INFO(" In save states to publish");
}
+ /**** Inicio parte publicar companion state ***/
//if no_people poner .numCompanionPeople=0; // si no encuentro los ids de las person companion
if(see_std_out_mesages_ros_){
//ROS_INFO(" to_stop_we_have_p1_=%d, to_stop_we_have_p2_=%d",to_stop_we_have_p1_,to_stop_we_have_p2_);
@@ -1285,6 +1293,8 @@ void AkpLocalPlanner::slice_plan()
}
+
+/**** Condiciones para parar al robot cuando llega al goal ****/
bool AkpLocalPlanner::isGoalReached()
{