diff --git a/include/fake_laser_gen_alg_node.h b/include/fake_laser_gen_alg_node.h index 9a3479f75dd522641b92dec87983c4f505964d65..27647e053f24cccf60a768364ec95f1fb9b75952 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, n_obstacles_3_marta_approaching,n_obstacles_4_marta_approaching,n_obstacles_5_marta_approaching }; + 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,n_obstacles9_v2 ,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 1e2bffc65b1bd9883a9b8f7cbd4b5c061c6d2d35..796b08294cfdfb7a82c32ae8bd3ec45e7a9703d9 100644 --- a/src/fake_laser_gen_alg_node.cpp +++ b/src/fake_laser_gen_alg_node.cpp @@ -105,9 +105,12 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level) // 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 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. +//laser_mode_=FakeLaserGenAlgNode::n_obstacles9_v2; // Caso obstaculo en el lateral izquierdo que lo evita bien SOLO companion 1 persona! + +//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 @@ -487,11 +490,13 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level) case FakeLaserGenAlgNode::n_obstacles11 : // caso obstaculos a 1.4965 m entre ellos. va con empty_map_whit_obstacles2 { ROS_INFO("n obstacles 3"); - for (unsigned int n = 665; n<688; ++n){ // 726.5 -> 745 ooo 665 -> 688 + //for (unsigned int n = 545; n<678; ++n){ // 726.5 -> 745 ooo 665 -> 688 + for (unsigned int n = 630; n<678; ++n){ LaserScan_msg_.ranges[n]=13.15; } - for (unsigned int n = 746.5; n<770; ++n){ // 726.5 -> 745 ooo 665 -> 688 + //for (unsigned int n = 746.5; n<880; ++n){ // 726.5 -> 745 ooo 665 -> 688 + for (unsigned int n = 746.5; n<790; ++n){ // 726.5 -> 745 ooo 665 -> 688 LaserScan_msg_.ranges[n]=13.15; } @@ -522,6 +527,30 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level) } break; + case FakeLaserGenAlgNode::n_obstacles9_v2 : // caso obstaculos a 1.4965 m entre ellos. va con empty_map_whit_obstacles2 + { + 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 = 635; n<698; ++n){ // FOR 1-person!726.5 -> 745 ooo 665 -> 688 + for (unsigned int n = 670; n<710; ++n){ //to accompany 2 person, robot lateral + 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; + case FakeLaserGenAlgNode::n_obstacles10 : // caso obstaculo a la izquierda del grupo. Bueno para luego hacer experimentos con obstaculos. Se ve bien como se acerca más a la persona con menos fuerza de obstaculos. { ROS_INFO("n obstacles 3");