Skip to content
Snippets Groups Projects
Commit f1a0570c authored by Ely Repiso Polo's avatar Ely Repiso Polo
Browse files

test en tibi side-by-side 2pers

parent 648c4324
No related branches found
No related tags found
No related merge requests found
......@@ -41,7 +41,7 @@
*/
class FakeLaserGenAlgNode : public algorithm_base::IriBaseAlgorithm<FakeLaserGenAlgorithm>
{
enum laser_mode{Zero=0, one_obstacle, two_obstacles, n_obstacles, n_obstacles_2, scene, scene_2,corrido_obstacles_companion_2_paths,corrido_obstacles_companion_3_paths,corridor_obstacles_which_narrows_companion_1_paths, n_obstacles3, n_obstacles4, scene_3, scene_4, n_obstacles5, n_obstacles6, n_obstacles7, n_obstacles8,n_obstacles7_robot2017_videos, scene_5,scene_6,n_obstacles9,scene_7,n_obstacles10,n_obstacles11 };
enum laser_mode{Zero=0, one_obstacle, two_obstacles, n_obstacles, n_obstacles_2, scene, scene_2,corrido_obstacles_companion_2_paths,corrido_obstacles_companion_3_paths,corridor_obstacles_which_narrows_companion_1_paths, n_obstacles3, n_obstacles4, scene_3, scene_4, n_obstacles5, n_obstacles6, n_obstacles7, n_obstacles8,n_obstacles7_robot2017_videos, scene_5,scene_6,n_obstacles9,scene_7,n_obstacles10,n_obstacles11, n_obstacles_3_marta_approaching,n_obstacles_4_marta_approaching,n_obstacles_5_marta_approaching };
private:
// [publisher attributes]
......
......@@ -101,10 +101,18 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
//laser_mode_=FakeLaserGenAlgNode::scene_4; // pasillo esquina, medio ancho, medio estrecho!
// laser_mode_=FakeLaserGenAlgNode::scene_7; // Caso pared lateral grande en lado izquierdo pantalla.
// NEW CASES OBSTACLES side-by-side 2people
//laser_mode_=FakeLaserGenAlgNode::n_obstacles9; // (un poco más hacia izquierda, que el caso n_obstacles10)Caso obstaculo en el lateral izquierdo que lo evita bien el grupo (solo faltaria salir de un poco más atras)
//laser_mode_=FakeLaserGenAlgNode::n_obstacles10; //Caso obstaculo en el lateral izquierdo que lo evita bien el grupo (solo faltaria salir de un poco más atras)
//laser_mode_=FakeLaserGenAlgNode::n_obstacles10; //Caso obstaculo en el lateral derecho que lo evita bien el grupo (solo faltaria salir de un poco más atras) OK
//laser_mode_=FakeLaserGenAlgNode::n_obstacles11; // nuevo caso puerta para el caso del companion de 2 personas.
// Escenarios approachign marta: n_obstacles_3_marta_approaching,n_obstacles_4_marta_approaching,n_obstacles_5_marta_approaching
//laser_mode_=FakeLaserGenAlgNode::n_obstacles_5_marta_approaching; // caso multiple obstaculos, pero no va muy bien al no ser casos realistas y ser el grupo grande.
laser_mode_=FakeLaserGenAlgNode::n_obstacles11; // nuevo caso puerta para el caso del companion de 2 personas.
// TODO: para el side-by-side => el path no puede entrar en colision con p1 y p2. Si fuerzas, pero NO colision.
switch(laser_mode_)
{
......@@ -500,7 +508,7 @@ laser_mode_=FakeLaserGenAlgNode::n_obstacles11; // nuevo caso puerta para el cas
LaserScan_msg_.ranges[680-5]= 13.25;
LaserScan_msg_.ranges[680]= 13.15;
LaserScan_msg_.ranges[680+8]= 13.15; */ // 6.5 pasan...
for (unsigned int n = 655; n<678; ++n){ // 726.5 -> 745 ooo 665 -> 688
for (unsigned int n = 635; n<658; ++n){ // 726.5 -> 745 ooo 665 -> 688
LaserScan_msg_.ranges[n]=13.15;
}
/* LaserScan_msg_.ranges[730-3.5]= 17.15; //4.5 pasan...
......@@ -518,21 +526,14 @@ laser_mode_=FakeLaserGenAlgNode::n_obstacles11; // nuevo caso puerta para el cas
ROS_INFO("n obstacles 3");
/* LaserScan_msg_.ranges[680-15]= 13.0;
LaserScan_msg_.ranges[680-10]= 13.25;
LaserScan_msg_.ranges[680-5]= 13.25;
LaserScan_msg_.ranges[680]= 13.15;
LaserScan_msg_.ranges[680+8]= 13.15; */ // 6.5 pasan...
for (unsigned int n = 675; n<698; ++n){ // 726.5 -> 745 ooo 665 -> 688
/*for (unsigned int n = 675; n<698; ++n){ // 726.5 -> 745 ooo 665 -> 688
LaserScan_msg_.ranges[n]=13.15;
}*/
for (unsigned int n = 746.5; n<770; ++n){ // 726.5 -> 745 ooo 665 -> 688
LaserScan_msg_.ranges[n]=13.15;
}
/* LaserScan_msg_.ranges[730-3.5]= 17.15; //4.5 pasan...
LaserScan_msg_.ranges[730]= 17.15;
LaserScan_msg_.ranges[730+3]= 17.15;
LaserScan_msg_.ranges[730+6]= 17.15;
LaserScan_msg_.ranges[730+9]= 17.15;
LaserScan_msg_.ranges[730+12]= 17.15;
// LaserScan_msg_.ranges[1300+18]= 7.0;*/
}
break;
......@@ -631,6 +632,88 @@ laser_mode_=FakeLaserGenAlgNode::n_obstacles11; // nuevo caso puerta para el cas
LaserScan_msg_.ranges[1300+40]= 4.0;
}
break;
case FakeLaserGenAlgNode::n_obstacles_3_marta_approaching : // 7!?
ROS_INFO("n obstacles 3");
LaserScan_msg_.ranges[100]= 4.0;
LaserScan_msg_.ranges[340]= 4.0;
LaserScan_msg_.ranges[580]= 4.0;
LaserScan_msg_.ranges[820]= 4.0;
LaserScan_msg_.ranges[1060]= 4.0;
LaserScan_msg_.ranges[1300]= 4.0;
LaserScan_msg_.ranges[1]= 10.0;
LaserScan_msg_.ranges[241]= 10.0;
LaserScan_msg_.ranges[481]= 10.0;
LaserScan_msg_.ranges[721]= 10.0;
LaserScan_msg_.ranges[961]= 10.0;
LaserScan_msg_.ranges[1201]= 10.0;
break;
case FakeLaserGenAlgNode::n_obstacles_4_marta_approaching : // 8!?
ROS_INFO("n obstacles 4");
LaserScan_msg_.ranges[100]= 4.0;
LaserScan_msg_.ranges[340]= 4.0;
LaserScan_msg_.ranges[580]= 4.0;
LaserScan_msg_.ranges[820]= 4.0;
LaserScan_msg_.ranges[1060]= 4.0;
LaserScan_msg_.ranges[1300]= 4.0;
LaserScan_msg_.ranges[1]= 10.0;
LaserScan_msg_.ranges[121]= 10.0;
LaserScan_msg_.ranges[241]= 10.0;
LaserScan_msg_.ranges[361]= 10.0;
LaserScan_msg_.ranges[481]= 10.0;
LaserScan_msg_.ranges[601]= 10.0;
LaserScan_msg_.ranges[721]= 10.0;
LaserScan_msg_.ranges[841]= 10.0;
LaserScan_msg_.ranges[961]= 10.0;
LaserScan_msg_.ranges[1081]= 10.0;
LaserScan_msg_.ranges[1201]= 10.0;
break;
case FakeLaserGenAlgNode::n_obstacles_5_marta_approaching : // 9!?
ROS_INFO("n obstacles 5");
//LaserScan_msg_.ranges[100]= 4.0;
LaserScan_msg_.ranges[340]= 4.0;
LaserScan_msg_.ranges[580]= 4.0;
LaserScan_msg_.ranges[820]= 4.0;
LaserScan_msg_.ranges[1060]= 4.0;
// LaserScan_msg_.ranges[1300]= 4.0;
//LaserScan_msg_.ranges[1]= 7.0;
LaserScan_msg_.ranges[121]= 7.0;
LaserScan_msg_.ranges[241]= 7.0;
LaserScan_msg_.ranges[361]= 7.0;
LaserScan_msg_.ranges[481]= 7.0;
LaserScan_msg_.ranges[601-30]= 7.0;
LaserScan_msg_.ranges[731]= 7.0;
LaserScan_msg_.ranges[841]= 7.0;
LaserScan_msg_.ranges[961]= 7.0;
LaserScan_msg_.ranges[1081]= 7.0;
LaserScan_msg_.ranges[1201]= 7.0;
//LaserScan_msg_.ranges[50]= 12.0;
LaserScan_msg_.ranges[200]= 12.0;
LaserScan_msg_.ranges[300]= 12.0;
LaserScan_msg_.ranges[400]= 12.0;
LaserScan_msg_.ranges[500]= 12.0;
LaserScan_msg_.ranges[600-20]= 12.0;
LaserScan_msg_.ranges[730]= 12.0;
LaserScan_msg_.ranges[800]= 12.0;
LaserScan_msg_.ranges[900]= 12.0;
LaserScan_msg_.ranges[1000]= 12.0;
LaserScan_msg_.ranges[1100]= 12.0;
break;
case FakeLaserGenAlgNode::Zero :
default :
ROS_INFO("zero");
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment