En esta práctica realizaremos un algoritmo basado en seguir la pared, para ello utilizamos el sensor láser y los LifeCycle nodes de ROS2.
Para saber donde están los obstáculos, en este caso la pared, usamos el láser. Como procesado, extraemos la medida mínima en un cono de amplitud de 10º centrado en tres regiones: la frontal (0), la izquierda (pi/2) y la derecha (-pi/2). Finalmente, las tres medidas se guardan en un vector.
Aprovechando las características de los LifeCycle Nodes y sus estados y transiciones, usamos la transición on_configure() para ver si el robot tiene algún obstáculo cercano a izquierda o derecha y comenzar siguiendo ese lado del robot directamente. Si no hay nada cerca, se elige un lado del robot para seguir la pared al azar.
FollowWallNode::on_configure() {
// Esperar por medidas del láser
while (laser_regions.size() == 0) {continue;}
// Elegir un lado a seguir
if (laser_regions[A] < distance) {side = LEFT;}
else if (laser_regions[C] < distance) {side = RIGHT;}
else {
if (rand() % 2 == 0) {side = LEFT;}
else {side = RIGHT;}
}
}
Una vez se ha realizado la configuración, se espera a que el nodo sea activado para comenzar a mover el robot. Si se desease cambiar la configuración de qué lado sigue el robot, basta con volver a ejecutar la transición on_configure().
Cuando se active el nodo se realizará el siguiente proceso de comprobación. Una vez hecho, en función del estado seleccionado se seleccionarán velocidades lineales y angulares correspondientes a cada estado:
