diff --git a/include/fake_laser_gen_alg_node.h b/include/fake_laser_gen_alg_node.h
index b61fd62d23849e7b50ba79f14ccb275de930576d..99a535460280458c7d9a081915597e23bc23f7ef 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};
+ 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 };
private:
// [publisher attributes]
diff --git a/src/fake_laser_gen_alg_node.cpp b/src/fake_laser_gen_alg_node.cpp
index 6d353ff56df9fa688670425b9f22be921d7c9cb9..f44aea88fa60a2e11cd0f22ae197392dd15d324e 100644
--- a/src/fake_laser_gen_alg_node.cpp
+++ b/src/fake_laser_gen_alg_node.cpp
@@ -91,18 +91,20 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
//*********/
//laser_mode_=FakeLaserGenAlgNode::n_obstacles7; // [ultimo que estaba usando BUENO, datos paper robot2017] caso estrechez en medio del camino! caso obstaculos a 1.94 m entre ellos. va con empty_map_whit_obstacles2
-
-
//laser_mode_=FakeLaserGenAlgNode::n_obstacles5; //caso estrechez en medio del camino. Muy estrecho!
//laser_mode_=FakeLaserGenAlgNode::n_obstacles3; //caso estrechez en medio del camino! obstaculos a 2.2305 m entre ellos va con empty_map_whit_obstacles1
-
// laser_mode_=FakeLaserGenAlgNode::n_obstacles7_robot2017_videos;
- //laser_mode_=FakeLaserGenAlgNode::n_obstacles8; // [ultimo que estaba usando BUENO] caso estrechez en medio del camino! caso obstaculos a 1.4965 m entre ellos. va con empty_map_whit_obstacles2
+ // laser_mode_=FakeLaserGenAlgNode::n_obstacles8; // [ultimo que estaba usando BUENO] caso estrechez en medio del camino! caso obstaculos a 1.4965 m entre ellos. va con empty_map_whit_obstacles2
//laser_mode_=FakeLaserGenAlgNode::n_obstacles6; //caso estrechez en medio del camino!
//laser_mode_=FakeLaserGenAlgNode::scene_3; // caso pasillo, esquina. (Ancho)
//laser_mode_=FakeLaserGenAlgNode::scene_2; // Estrecho! caso pasillo, esquina.
//laser_mode_=FakeLaserGenAlgNode::scene_4; // pasillo esquina, medio ancho, medio estrecho!
+ //laser_mode_=FakeLaserGenAlgNode::scene_7; // Caso pared lateral grande en lado izquierdo pantalla.
+//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_obstacles11; // nuevo caso puerta para el caso del companion de 2 personas.
+
switch(laser_mode_)
{
case FakeLaserGenAlgNode::one_obstacle :
@@ -231,6 +233,44 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
}
break;
+ case FakeLaserGenAlgNode::scene_5 : // Case only one lateral big wall.
+ {
+ ROS_INFO("scene");
+ for (unsigned int n = 20; n<700; ++n)
+ LaserScan_msg_.ranges[n] = 5.0 / sin(LaserScan_msg_.angle_increment * (double) n);
+ }
+ break;
+
+ case FakeLaserGenAlgNode::scene_6 : // Case only one lateral big wall.
+ {
+ ROS_INFO("scene");
+ /*for (unsigned int n = 20; n<700; ++n)
+ LaserScan_msg_.ranges[n] = 5.0 / sin(LaserScan_msg_.angle_increment * (double) n);*/
+ for (unsigned int n = 758; n<805; ++n)
+ LaserScan_msg_.ranges[n] = -2.0 / sin(LaserScan_msg_.angle_increment * (double) n);
+ for (unsigned int n = 805; n<1040; ++n)
+ LaserScan_msg_.ranges[n] = -5.0 / cos(LaserScan_msg_.angle_increment * (double) n);
+ for (unsigned int n = 1137; n<1260; ++n)
+ LaserScan_msg_.ranges[n] = 3.0 / cos(LaserScan_msg_.angle_increment * (double) n);
+ for (unsigned int n = 1260; n<1400; ++n)
+ LaserScan_msg_.ranges[n] = -3.0 / sin(LaserScan_msg_.angle_increment * (double) n);
+ }
+ break;
+
+ case FakeLaserGenAlgNode::scene_7 : // Case only one lateral big wall.
+ {
+ ROS_INFO("scene");
+ for (unsigned int n = 758; n<805; ++n)
+ LaserScan_msg_.ranges[n] = -2.0 / sin(LaserScan_msg_.angle_increment * (double) n);
+ for (unsigned int n = 805; n<1040; ++n)
+ LaserScan_msg_.ranges[n] = -5.0 / cos(LaserScan_msg_.angle_increment * (double) n);
+ /*for (unsigned int n = 1137; n<1260; ++n)
+ LaserScan_msg_.ranges[n] = 3.0 / cos(LaserScan_msg_.angle_increment * (double) n);
+ for (unsigned int n = 1260; n<1400; ++n)
+ LaserScan_msg_.ranges[n] = -3.0 / sin(LaserScan_msg_.angle_increment * (double) n);*/
+ }
+ break;
+
case FakeLaserGenAlgNode::corrido_obstacles_companion_2_paths :
{
ROS_INFO("corrido_obstacles_companion_2_paths");
@@ -413,41 +453,87 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
case FakeLaserGenAlgNode::n_obstacles8 : // caso obstaculos a 1.4965 m entre ellos. va con empty_map_whit_obstacles2
{
ROS_INFO("n obstacles 3");
- /* LaserScan_msg_.ranges[100]= 4.0;
- LaserScan_msg_.ranges[100+40]= 4.0;
- LaserScan_msg_.ranges[340]= 4.0;
- LaserScan_msg_.ranges[340+40]= 4.0;
- LaserScan_msg_.ranges[580]= 4.0;
- LaserScan_msg_.ranges[580+40]= 4.0;
- LaserScan_msg_.ranges[820]= 4.0;
- LaserScan_msg_.ranges[820+40]= 4.0;
- LaserScan_msg_.ranges[1060]= 4.0;
- LaserScan_msg_.ranges[1060+40]= 4.0;
- LaserScan_msg_.ranges[1300]= 4.0;
- LaserScan_msg_.ranges[1300+40]= 4.0;*/
-
- LaserScan_msg_.ranges[100]= 7.0;
- LaserScan_msg_.ranges[100+40]= 7.0;
- LaserScan_msg_.ranges[340]= 7.0;
- //LaserScan_msg_.ranges[340+40]= 7.0;
- // LaserScan_msg_.ranges[660]= 13.25;
- LaserScan_msg_.ranges[680-15]= 13.0;
+ for (unsigned int n = 665; n<688; ++n){ // 726.5 -> 745 ooo 665 -> 688
+ LaserScan_msg_.ranges[n]=13.15;
+ }
+ /* 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...
+ LaserScan_msg_.ranges[680+8]= 13.15; // 6.5 pasan...*/
// LaserScan_msg_.ranges[680+5]= 13.0;
- LaserScan_msg_.ranges[730-3.5]= 13.15; //4.5 pasan...
+ /* LaserScan_msg_.ranges[730-3.5]= 13.15; //4.5 pasan...
LaserScan_msg_.ranges[730]= 13.15;
LaserScan_msg_.ranges[730+5]= 13.15;
LaserScan_msg_.ranges[730+10]= 13.15;
LaserScan_msg_.ranges[730+15]= 13.15;
- LaserScan_msg_.ranges[730+20]= 13.15;
- LaserScan_msg_.ranges[1300+18]= 7.0;
+ LaserScan_msg_.ranges[730+20]= 13.15;*/
+ for (unsigned int n = 726.5; n<740; ++n){ // 726.5 -> 745 ooo 665 -> 688
+ LaserScan_msg_.ranges[n]=13.15;
+ }
+ // LaserScan_msg_.ranges[1300+18]= 7.0;
+ }
+ break;
+
+ 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 = 675; n<698; ++n){ // 726.5 -> 745 ooo 665 -> 688
+ LaserScan_msg_.ranges[n]=13.15;
+ }
+
+ for (unsigned int n = 736.5; n<760; ++n){ // 726.5 -> 745 ooo 665 -> 688
+ LaserScan_msg_.ranges[n]=13.15;
+ }
+
}
break;
+ case FakeLaserGenAlgNode::n_obstacles9 : // 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 = 665; n<688; ++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;
+
+ 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");
+
+
+ /* 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
+ 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_obstacles6 :
{