Simulación de la navegación

Simulación con rviz

Leer el manual de usuario, afortunadamente es corto.

De la lista completa de tutoriales de rviz, comenzar con:

1 y 2.

Finalmente simular a la kobuki instalando softkobuki. Seguir las instrucciones en:

Simulated Kobuki Navigation Demo. El navigation demo no funciona, pero programaremos el nuestro, así que no hay problema. El código de este paquete puede ser una buena fuente información.
Los comandos básicos para iniciar el simulador (una vez instalado) son:

> roslaunch kobuki_softnode full.launch

> rosrun rviz rviz

> roslaunch kobuki_keyop keyop.launch

Para visualizar en rviz, seleccionar el cuadro '\odom' y agregar un 'RobotModel'.

Marcador tipo nav_msgs::OccupancyGrid.

Creación del mapa:

Código básico en git

Crear una variable tipo

nav_msgs::OccupancyGrid map;

Llenar la información sobre el mapa:

const int WIDTH = 24; /// A lo largo del eje rojo x const int HEIGHT = 31; /// A lo largo del eje verde map.header.frame_id = "/odom"; map.header.stamp = ros::Time::now(); // No caduca map.info.resolution = 0.3; // [m/cell] map.info.width = WIDTH; // [cells] map.info.height = HEIGHT; // [cells] map.info.origin.position.x = 0; map.info.origin.position.y = 0; map.info.origin.position.z = 0; map.info.origin.orientation.x = 0.0; map.info.origin.orientation.y = 0.0; map.info.origin.orientation.z = 0.0; map.info.origin.orientation.w = 1.0;

Crear un arreglo de char para agregar los valores iniciales

int size = WIDTH * HEIGHT; char* data = new char[size]; for(int i = 0; i < size; i++) { data[i] = 0; }

Inicializar data usando vector: (Previamente en el encabezado usar #include <vector>)

map.data = std::vector<int8_t>(data, data + size);

Para visualizar en rviz, publicar el mensaje con el mapa

ros::NodeHandle n; ros::Rate r(1); ros::Publisher marker_pub = n.advertise<nav_msgs::OccupancyGrid>("visualization_marker", 1); marker_pub.publish(map);

Posición inicial

Se puede informar a rviz sobre la posición inicial de la kobuki enviando un mensaje al tópico /initialpose, de tipo /geometry_msgs/PoseWithCovarianceStamped. NOTA: Ojo con los espacios después de los :, sin ellos no funciona.

> rostopic pub -1 /initialpose geometry_msgs/PoseWithCovarianceStamped -- '{seq: 0, stamp: [0,0], frame_id: "/odom"}' '{pose: {position: {x: 3.0, y: 3.0}, orientation: {w: 1.0}}}'
Otra altertativa:
> rostopic pub -1 /initialpose geometry_msgs/PoseWithCovarianceStamped -- '[0, [0,0], /odom]' '{pose: {position: {x: 3.0, y: 3.0}, orientation: {w: 1.0}}}'
Formato YAML

Aunque esto no basta para cambiar la posición inicial del modelo del robot en la visualización.

Meta de la navegación

Para indicar al simulador a dónde queremos mover a la Kobuki, podemos usar la interfaz de rviz. Haciendo click en "2D Nav Goal" el indicador del ratón cambiará por una flechita, cuando se haga click en el mapa, rviz publicará en el tópico "/move_base_simple/goal" un mensaje de tipo geometry_msgs/PoseStamped, por lo que bastará con agregar un suscriptor al nodo que necesite conocer las coordenadas destino.

Intersección con los obstáculos a partir de la posición del robot.

/** * Lanza un rayo a partir de las coordenadas (<code>x</code>,<code>y</code>) * en dirección <code>angulo</code> y devuelve la distancia al obstáculo más * cercano o a un muro en el mapa. * @param x coordenada horizontal, de izquierda a derecha. * @param y coordenada vertical, de arriba a abajo. * @param ngulo dirección en la que se extiende el rayo en radianes. * @param limpia borra las línea usadas para calcular las distancias. * @return distancia */ public float distanciaAColision(float x, float y, float angulo, boolean limpia) { if (limpia) { origen[0] = x; origen[1] = y; ilumina.clear(); colisiones.clear(); } angulo = aRango(angulo); double m = Math.tan(angulo); double b = - y - m * x; // y está invertida int[] ij = celda(x, y); int i = ij[0], j = ij[1]; double xn, yn; if (angulo > 0) { if (angulo < Math.PI/2) { // Primer cuadrante int l = i, k = j; // arriba y a la derecha while(k < mapaDiscretizado[0].length && l >= 0) { int[] indx = {l, k}; ilumina.add(indx); xn = (k + 1) * anchoCelda; yn = y - m * (xn - x); if (yn < (l + 1) * anchoCelda && yn > l * anchoCelda) { k++; // ve a la derecha if (k == mapaDiscretizado[0].length || this.mapaDiscretizado[l][k] == OCUPADA) { //System.out.println("Caso 1"); return colision(x, y, xn, yn); } } else if (yn == l * anchoCelda) { k++; // a la derecha l--; // arriba if (l < 0 || k == mapaDiscretizado[0].length || this.mapaDiscretizado[l][k] == OCUPADA) { //System.out.println("Caso 2"); return colision(x, y, xn, yn); } } else { l--; // arriba yn = (l + 1) * anchoCelda; xn = (-yn - b)/m; if (l < 0 || this.mapaDiscretizado[l][k] == OCUPADA) { //System.out.println("Caso 3"); return colision(x, y, xn, yn); } } } } else if (angulo <= Math.PI) { // Segundo cuadrante int l = i, k = j; // arriba y a la izquierda while(k >= 0 && l >= 0) { int[] indx = {l, k}; ilumina.add(indx); yn = l * anchoCelda; xn = (-yn - b)/m; if (xn > k * anchoCelda && xn < (k + 1) * anchoCelda) { l--; // ve arriba if (l < 0 || this.mapaDiscretizado[l][k] == OCUPADA) { return colision(x, y, xn, yn); } } else if (xn == (k + 1) * anchoCelda) { k--; // a la izquierda l--; // arriba if (l < 0 || k < 0 || this.mapaDiscretizado[l][k] == OCUPADA) { return colision(x, y, xn, yn); } } else { k--; // a la izquierda xn = (k + 1) * anchoCelda; yn = y - m * (xn - x); if (k < 0 || this.mapaDiscretizado[l][k] == OCUPADA) { return colision(x, y, xn, yn); } } } } } else { if (angulo > -Math.PI/2) { // Cuarto cuadrante int l = i, k = j; // abajo y a la derecha while(k < mapaDiscretizado[0].length && l < mapaDiscretizado.length) { int[] indx = {l, k}; ilumina.add(indx); xn = (k + 1) * anchoCelda; yn = y - m * (xn - x); if (yn < (l + 1) * anchoCelda && yn > l * anchoCelda) { k++; // ve a la derecha if (k == mapaDiscretizado[0].length || this.mapaDiscretizado[l][k] == OCUPADA) { //System.out.println("Caso 1"); return colision(x, y, xn, yn); } } else if (yn == (l + 1) * anchoCelda) { k++; // a la derecha l++; // abajo if (l == this.mapaDiscretizado.length || k == mapaDiscretizado[0].length && this.mapaDiscretizado[l][k] == OCUPADA) { //System.out.println("Caso 2"); return colision(x, y, xn, yn); } } else { l++; // abajo if (l == this.mapaDiscretizado.length || this.mapaDiscretizado[l][k] == OCUPADA) { yn = l * anchoCelda; xn = (-yn - b)/m; //System.out.println("Caso 3"); return colision(x, y, xn, yn); } } } } else if (angulo > -Math.PI) { // Tercer cuadrante int l = i, k = j; // abajo y a la izquierda while(k >= 0 && l < this.mapaDiscretizado.length) { int[] indx = {l, k}; ilumina.add(indx); yn = (l + 1) * anchoCelda; xn = (-yn - b)/m; if (xn > k * anchoCelda && xn < (k + 1) * anchoCelda) { l++; // ve abajo if (l == this.mapaDiscretizado.length || this.mapaDiscretizado[l][k] == OCUPADA) { return colision(x, y, xn, yn); } } else if (xn == (k + 1) * anchoCelda) { k--; // a la izquierda l++; // ve abajo if (l == this.mapaDiscretizado.length || k < 0 || this.mapaDiscretizado[l][k] == OCUPADA) { return colision(x, y, xn, yn); } } else { k--; // a la izquierda if (k < 0 || this.mapaDiscretizado[l][k] == OCUPADA) { xn = (k + 1) * anchoCelda; yn = y - m * (xn - x); return colision(x, y, xn, yn); } } } } } return -1; }

Simulación en Gazebo

roslaunch

Para utilzar al simulador se lanzarán y configurarán varios nodos y paquetes, por lo que es necesario revisar primero el funcionamiento de roslaunch.

Gazebo

Tutorial de gazeebo para lanzar simulaciones con mundos personalizados: Tutorial: Using roslaunch to start Gazebo, world files and URDF models.

Kobuki

Aunque ya habíamos visto esto, incluyo de nuevo el punto de partida con los demos para la Kobuki:

Gazebo Simulation.

SDF

SDF es un formato para describir robots y sus ambientes, utilizado por Gazebo.

Con Python Tkinter

Crear un paquete de ROS, donde se pondrá el código para que navegue el robot simulado.
Programar un script que dibuje el mapa en 2D donde se moverá el robot.
- Usar uno o varios canvas de tkinter.
- Para ubicar componentes de la interfaz puede ser necesario usar un manejador: Layout.
Para que el código quede más ordenado, se recomienda poner todo lo referente a Tkinter dentro de una clase, ver el tutorial Hello, Again.
Probar que el nodo corra con rosrun, que se pueda cerrar la ventana o apagar el nodo con Ctrl-C. Para ello puede ser necesario utlizar la función after de Tk.