diff --git a/include/fake_laser_gen_alg_node.h b/include/fake_laser_gen_alg_node.h index 99a535460280458c7d9a081915597e23bc23f7ef..9a3479f75dd522641b92dec87983c4f505964d65 100644 --- a/include/fake_laser_gen_alg_node.h +++ b/include/fake_laser_gen_alg_node.h @@ -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] diff --git a/src/fake_laser_gen_alg_node.cpp b/src/fake_laser_gen_alg_node.cpp index 2b9c6467ef17dcef8d9480e6860ca3a2f9d3ac53..8a53e875dfd9cd9e3e15b116412c545ecef0e8be 100644 --- a/src/fake_laser_gen_alg_node.cpp +++ b/src/fake_laser_gen_alg_node.cpp @@ -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");