Simulador JDE/Gazebo Sara

Hemos dejado nuestro robot físico, para adentrarnos en el mundo de los robots simulados, con el entorno Gazebo. Disponíamos de un mundo con el diseño del departamental y nuestro robot tenía que navegar por él sin chocarse, ayudados de las imágenes que obtuviéramos por la cámara, nuestro único sensor.
Una vez definido el problema, había que pasar a solucionarlo… Lo primero que hice fue pasarle un filtro de color, para distinguir el suelo del resto del entorno que es lo que le estorbará al robot para navegar. Una vez hecho esto, pensé…bueno y ahora que?? Lo primero que se me ocurrió fue ir comprobando (de abajo hacía arriba, y de izquierda a derecha) los pixeles que pertenecían al suelo, y poner un umbral de pixeles mínimos de suelo que debía haber, y si encontraba ese umbral que el robot se parase (girase…)… Dándole unas vueltas a esto, me di cuenta de que no iba a llegar muy lejos… tenía un problema grande, que pasaba si me encontraba un objeto en medio del pasillo?? Era muy probable que mi robot se estampara con él, porque seguramente la suma de los pixeles a la derecha e izquierda del objeto superasen ese umbral.
Desestimada esta idea, busqué una solución más fácil, cogería una fila de la imagen y la analizaría. Elegí la fila 210, estaba por debajo de la mitad y parecía una distancia razonable para detectar objetos con los que poder chocar (para no hacerlo claro). Y lo que hacía a continuación era bastante sencillo. Simplemente buscaba el hueco más grande de píxeles “pasillo” que hubiese en el array, calculaba el centro y hacía girar al robot mas o menos en función de ese valor medio, es decir, si me daba mas o menos alrededor del centro, el giro iba a ser cero, si me daba un poco por encima (o por debajo) le haría girar un poco y si me daba muy por encima (o muy por debajo) el giro sería mas brusco. Pues bien, esta solución tan sencilla funcionó a la primera, recorría perfectamente el pasillo y con la idea de ir siempre al centro del grupo de píxeles “pasillo” más grande, se centraba solo el robot, por lo que navegaba por el centro del pasillo sin riesgo a rozar alguna pared y que se viniera abajo todo el departamental.
El problema vino cuando nos cambiaron el mundo…y nos añadieron miles de papeleras… en un principio yo pensé bueno, que mas da una que veinte?? Si el robot pasa una bien, tiene que pasar las veinte bien… pero ahí teníamos la “maldita papelera” en una esquina, cosa imprevista que mi robot al hacer un giro tan brusco a la derecha se comía de lleno. Así que le bajé un poco la velocidad angular y le aumenté la velocidad (sí, antes iba aún mas despacio), probé pero seguía comiéndose la papelera, total que había que bajar aún mas esa velocidad de giro, pero ya no me dio tiempo a cambiarla, porque la competición empezaba, así que mi robot se quedó en la papelera del infierno.

Aquí os dejo el video para que podais verlo.



Y mi código.

void yourcode_iteration(void)
{
int y, x, i, j;
int r,g,b;
char matriz[SIFNTSC_COLUMNS][SIFNTSC_ROWS];
int mediorow, mediocol;
int maxina = 0;
int maxact = 0;
int beginact = -1;
int beginina = -1;
int comienzaact=-1;
int comienzaina = -1;
int seguidosact = 0;
int seguidosina = 0;
int medio;
int entra = 0;
int entra2 = 0;
v = 350;
for (y = 0; y < SIFNTSC_ROWS; y++)
for (x = 0; x < SIFNTSC_COLUMNS; x++) {
b = (int)(unsigned char)imagenRGB[(y*SIFNTSC_COLUMNS+x)*3];
g = (int)(unsigned char)imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+1];
r = (int)(unsigned char)imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+2];
if (((r > 230) && (r <= 255)) && ((g > 230) && (g <= 255)) && ((b > 110) (b < 140))) {
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3] = (unsigned char)255;
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+1] = (unsigned char)255;
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+2] = (unsigned char)255;
matriz[x][y] = '.';
}
else {
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3] = (unsigned char)0;
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+1] = (unsigned char)0;
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+2] = (unsigned char)0;
matriz[x][y] = '_';
}
}
j = 210;
for (i = 0; i < SIFNTSC_COLUMNS; i++) {
if (matriz[i][j] == '.') {
if (maxina < seguidosina) {
comienzaina = beginina;
maxina = seguidosina;
}
beginina = -1;
seguidosina = 0;
if (beginact == -1) {
beginact = i;
seguidosact = seguidosact +1;
}
else
seguidosact = seguidosact +1;
}
else {
if (maxact < seguidosact) {
comienzaact = beginact;
maxact = seguidosact;
}
beginact = -1;
seguidosact = 0;
if (beginina == -1) {
beginina = i;
seguidosina = seguidosina +1;
}
else
seguidosina = seguidosina +1;
}
printf("posicion: [%d,%d], %c\n", i,j, matriz[i][j]);
}
if (maxact < seguidosact) {
comienzaact = beginact;
maxact = seguidosact;
}
else if (maxina < seguidosina) {
comienzaina = beginina;
maxina = seguidosina;
}
printf("maxact %d", maxact);
printf("begin %d", comienzaact);
medio = ((comienzaact + maxact) + comienzaact)/2;
printf("medio %d", medio);
if ( medio >= 0 && medio <= 75){
w = -65;
v=0;
}
else if(medio > 75 && medio <= 125) {
w = -30;
v=0;
}
else if (medio > 125 && medio <= 140) {
w = -10;
v=0;
}
else if (medio > 140 && medio <= 180) {
w = 0;
}
else if (medio > 180 && medio <= 195) {
w = 10;
v=0;
}
else if (medio > 295 && medio <= 245) {
w = 30;
v=0;
}
else {//if (medio > 235 && medio <= 259)
w = 65;
v=0;
}
//_exit(" ");
printf ("salir");

}