arreglando cosas del companion

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]

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 :
     {