Carlos Caminero Abad

Estudiante de Ingeniería de Robótica Software

2 minute read

Como comenté en la anterior semana, conseguí implementar una animación de un modelo de gazebo que representa una persona, a través de un script programado sobre un fichero xml con extensión .world. En un principio parecía que iba a funcionar correctamente para cualquier escenario. Sin embargo, cuando lo probé en el mundo del hospital, encontré un fallo que suponía un impedimento en el avance de mi TFG.

Resulta que cuando la simulación de la persona tenía lugar y lanzaba los ficheros de lanzamiento del robot Turtlebot 2 para su aparición en el mundo simulado, la animación de la persona dejaba de funcionar correctamente provocando que se desplazara a pasos bruscos.

Por lo tanto, tuve que dirigir la programación del modelo animado por otro camino: esta vez no usaría un <script> en un fichero .world sino que implementaría su comportamiento a través de un Plugin en ROS Foxy.

Para la integración del plugin seguí algunos tutoriales que ofrece la documentación oficial de ROS 2 Foxy y me orienté un poco gracias al Trabajo Fin de Máster de Pedro Arias que implementó 2 plugins para controlar a una persona simulada para ROS 1.

Una vez que tenía las ideas claras, procedí a su desarrollo. Quería que la persona se desplazara por todo el hospital a través de varios puntos de paso o waypoints respetando la orientación adecuada y permitiendo que el programador pudiera añadir tantos waypoints como quisiera manera sencilla.

La programación del plugin era en C++ de modo que hice uso de la librería std para incoporar vectores y tuplas. La lista de waypoints estaría descrita de la siguiente manera:

std::vector <std::tuple<float, float, int>> wp;
wp = {
    std::make_tuple(4, 6, 1),
    std::make_tuple(5, 3, 2),
    std::make_tuple(5, -14.5, 3),
    std::make_tuple(-5, -14.5, 4),
    std::make_tuple(-5, -25, 5),
    std::make_tuple(5, -25, 6),
    std::make_tuple(5, -14.5, 7),
    std::make_tuple(-5, -14.5, 8),
    std::make_tuple(-5, -1, 9),
    std::make_tuple(-4, 2, 10),
    std::make_tuple(-4, 5, 11),
    std::make_tuple(-2.5, 13, 12),
    std::make_tuple(3, 13, 13),
    std::make_tuple(4, 10, 0),
};

los dos primeros números decimales (float) indican las coordenadas X, Y de los puntos de paso. El último número entero corresponde al siguiente waypoint de la lista (siguiende las reglas de indexación). De tal manera que si es necesario modificar el recorrido se podría cambiar dicho número para desplazarse al waypoint deseado. En este caso sigo un recorrido circular (el último waypoint está concatenado al primero)

Para programar el desplazamiento hasta el siguiente Waypoint lo dividí en 4 subobjetivos:

  • Encontrar el ángulo entre la persona y el waypoint objetivo (radianes). Se corresponderá al ángulo de Yaw deseado
  • Determinar la mejor dirección de giro (derecha o izquierda). Implica medir cual es la dirección que tiene el recorrido angular más corto.
  • Girar la orientación de Yaw de la persona hasta alcanzar en ángulo de Yaw deseado.
  • Mover en línea recta hasta el waypoint objetivo, haciendo uso además de la distancia Euclídea

Programar el plugin me ayudó para generar una mejor simulación, ya que tenía el control absoluto de lo que quería hacer con el modelo. Algo que mediante un fichero .world no hubiera podido implementar. El resultado fue el siguiente:

You May Also Enjoy