Acciones en ROS (Parte 1)
En esta primera parte de esta entrada veremos qué son las acciones en ROS? Porqué usarlas? Cuales son sus mensajes? En esta primera entrada explicaremos qué es una acción, cómo funcionan y lo veremos con un ejemplo. Las acciones son uno de los temas que más dudas suscitan en el aprendizaje de ROS, así que Ánimo!!! Ya estamos más cerca de poder aplicar nuestros conocimientos en algún ejemplo «real».
Más info en el siguiente enlace
Introducción
Como hemos visto en entradas anteriores del blog, ROS nos permite usar dos maneras de transmitir información entre sus nodos.
La primera es a través de mensajes, que son enviados mediante topics, donde nos encontramos nodos que publican mensajes en este topic y nodos subscriptores que leen los mensajes que llegan en un topic determinado. En esta manera de comunicarse dará igual cuantos publishers, o cuantos subscribers tenemos, o si no tenemos ninguno, pues la comunicación no se hace de manera directa.
La otra manera de comunicarse es mediante los servicios, en este sistema existe un servidor y un cliente, es un sistema síncrono, es decir el cliente manda un mensaje al servidor, teniendo el cliente que esperar a que el servidor procese la información. Este sistema usa un mensaje que consta de una parte request que rellena el cliente y la parte response que rellena el servidor.
Pero con estos modos de comunicarse no satisfacemos todas las necesidades en robótica, imaginemos que tenemos un nodo que interactúa sobre un brazo robótico. Supongamos que esta comunicación la vamos a hacer mediante servicio/cliente, para ello vamos a mandar unas coordenadas objetivo a nuestro brazo y nuestro servidor realizará el cálculo para mover los motores hasta alcanzar ese punto. Pero qué pasa si una vez que le hemos mandado las coordenadas objetivo, estas ya no son válidas y tenemos que mandarlas de nuevo. Pues que el servidor no será capaz ni de cancelar la orden anterior ni nada más allá que no sea seguir con la tarea. Además, esta tarea mantendrá nuestros procesos en espera mientras el servidor computa el resultado, es decir mientras va a la posición objetivo nuestro programa se queda inutilizado.
Y si queremos cancelar la operación por que algo ha cambiado? No podemos hacer nada más que esperar a que el servidor acabe su ejecución. Además estaría bien tener un feedback , como por ejemplo cuanto falta por llegar a la pose objetivo. También sería deseable pasar más de un punto objetivo al robot, lamentablemente con los servicios de ROS no podemos hacer esto.
Es aquí cuando entran en juego el tercer tipo de comunicación entre nodos. Las acciones de ROS.
¿Qué son las acciones de ROS?
¿Porqué usarlas?
Pensemos de nuevo en el ejemplo anterior, tenemos un brazo robótico controlado por un nodo. Este nodo tras localizar la posición de unas piezas va a mandar la posición objetivo al robot, con el uso de acciones vamos a pasar un mensaje con las posiciones objetivo y nuestro programa va a ser capaz de:
- ser cancelado en cualquier momento, ya sea porque queremos o por que la posición anterior ya no es válida.
- nos reportará un feedback, por ejemplo, cuanto le falta para acabar
- nos aportará un resultado, por ejemplo, si está en curso, si está cancelada, si está acabada, etc….
- Además, nos permite pasarle varios puntos, es decir una trayectoria.
Para poder usar acciones haremos uso de un tipo de mensajes especiales, los mensajes .action . Al igual que en los servicios o en la comunicación estándar tipo publish/subcribe podemos crear nuestros propios mensajes.
Estos mensajes tienen a siguiente estructura:
#goal --- #feedback --- #result
Estos mensajes.accion portarán los datos entre los nodos y roscore. Qué datos? Pues en primer lugar deberán contener el objetivo o goal de nuestra acción, si es mover el brazo a un punto en el espacio, el objetivo será el pto en cuestión el el espacio.
Otro dato que contendrán es el feedback, es decir el servidor nos puede dar datos sobre el proceso en cuestión mientras este se ejecuta.
Y por último y no menos importante el campo result, que nos podrá ir dando en todo momento el resultado de nuestra acción, si ha llegado al punto objetivo, si está en ello, si está cancelada…
Un mensaje de una acción en realidad se puede descomponer en varios mensajes, un mensaje con el goal, otro con el feedback,.. y así sucesivamente. Para facilitar el uso de acciones, usaremos la librería actionlib, que nos ayudará a crear servidores y clientes y gestionar los diferentes topics de la acción. Esta librería nos va a permitir tratar los topics que conforman una acción en uno sólo, facilitándonos la tarea.
Ejemplo
En este ejemplo vamos a crear una acción que será encargada de llegar mediante un contador a un número objetivo (goal). Recoradad que el código de este ejemplo lo podéis encontrar en nuestro Github
Empezaremos viendo cómo crear un mensaje para nuestra acción (no la acción, esto es importante). Para ello crearemos un paquete aparte que contendrá sólo este mensaje (se puede no hacer así y hacer todo en un mismo paquete)
Creando el mensaje de la acción
Desde la raíz de nuestro workspace y en la carpeta src tecleamos:
$catkin_create_pkg entrada_accion_mensajes roscpp std_msgs actionlib_msgs
Ojo a la dependencia de actionlib_msgs para crear mensajes de acciones
Ahora dentro de este paquete vamos a crear una carpeta llamada action y dentro de ella nuestro mensaje.action, al que llamaremos Contador.action y que tendrá la siguiente estructura
#goal //objetivo int64 objetivo --- #result // resultado, está cabada?cancelada?etc.. int64 contador --- #feedback // cuanto me falta para llegar float64 proporcion
Una vez creado nuestro mensaje de acción lo que vamos a hacer es añadir en el archivo package.xml la siguiente línea para habilitar la generación de mensajes
<exec_depend>message_generation</exec_depend>
Dentro del archivo CmakeLists.txt, generamos los mensajes de la acción y luego ndicamos las dependencias necesarias para crear nuestros mensajes. Importante actionlib_msgs
## Generate actions in the 'action' folder add_action_files( FILES Contador.action )
## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES actionlib_msgs std_msgs )
Port último:
catkin_package( # INCLUDE_DIRS include # LIBRARIES entrada_acciones_mensajes CATKIN_DEPENDS actionlib_msgs roscpp std_msgs # DEPENDS system_lib )
Una vez realizados estos cambios y guardados podemos escribir en la terminal:
$catkin_make
Así si todo ha ido bien nuestro mensaje se habrá creado
Creando la acción
Creando el servidor de la acción
Ahora nos centraremos en la creación de un paquete, cuyo programa, creará una acción que usará el tipo de mensaje creado anteriormente, es decir crearemos una acción que hará uso del mensaje Contador.action. Este paquete contendrá un servidor para nuestra acción y un cliente para nuestra acción.
Así pues, empezamos creando el paquete de la acción:
En la terminal en el directorio src de nuestro workspace, creamos un paquete llamado entrada_acciones, por lo que escribimos:
$catkin_create_pkg entrda_acciones roscpp std_msgs actionlib actionlib_msgs entrada_acciones_mensajes
Mención especial a entrada_acciones_mensajes, es decir decimos a catkin que nuestro paquete va a depender (necesita sus mensajes) del paquete entrada_acciones_mensajes creado al principio del post y que contiene el mensaje.
Bien, ahora dentro de la carpeta src de nuestro paquete, crearemos un archivo llamado servidor.cpp en el que alojaremos el servidor de nuestra acción. El código es el siguiente, recuerda que puedes acceder a él en nuestro GitHub
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <entrada_acciones_mensajes/ContadorAction.h>
#include <entrada_acciones_mensajes/ContadorGoal.h>
#include <entrada_acciones_mensajes/ContadorResult.h>
#include <entrada_acciones_mensajes/ContadorFeedback.h>
class Contador{
protected:
ros::NodeHandle _nh;
actionlib::SimpleActionServer<entrada_acciones_mensajes::ContadorAction> _as;
int _contador;
public:
Contador():
_as(_nh, "/contador",
boost::bind(&Contador::cb, this, _1),
false),
_contador(0)
{
_as.start();
ROS_INFO("Servidor de la accion iniciado");
}
void cb(const entrada_acciones_mensajes::ContadorGoalConstPtr &goal){
ROS_INFO("Goal received");
int g =goal->objetivo;
ROS_INFO("Objetivo : %d",g);
_contador=0;
ros::Rate rate(1);
bool success = false;
bool preempted = false;
while (ros::ok()) {
_contador++;
if (_as.isPreemptRequested()) {
preempted = true;
break;
}
if (_contador > 30) {
break;
}
if (_contador > g) {
success = true;
break;
}
ROS_INFO("%d", _contador);
entrada_acciones_mensajes::ContadorFeedback feedback;
feedback.proporcion =
(double)_contador / (double)g;
_as.publishFeedback(feedback);
rate.sleep();
}
entrada_acciones_mensajes::ContadorResult result;
result.contador = _contador;
ROS_INFO("Send goal result to client");
if (preempted) {
ROS_INFO("Preempted");
_as.setPreempted(result);
}
else if (success) {
ROS_INFO("Success");
_as.setSucceeded(result);
}
else {
ROS_INFO("Aborted");
_as.setAborted(result);
}
}
};
int main (int argc, char **argv) {
ros::init(argc, argv, "nodo_acciones");
ROS_INFO("SERVIDOR ACCION");
Contador contador;
ros::spin();
return 0;
}
Con #include <ros/ros.h> cargamos la librería interna de ROS para C++.
Cargamos la librería actionlib que nos ayudará a crear el servidor de nuestra acción.
#include <ros/ros.h> #include <actionlib/server/simple_action_server.h>
En estas líneas cargamos el mensaje de la acción creado en el apartado anterior, dentro de nuestro paquete entrada_acciones_mensaje , se cargará ContadorAction, ContadorGoal, ContadorResult y ContadorFeedback
#include <entrada_acciones_mensajes/ContadorAction.h> #include <entrada_acciones_mensajes/ContadorGoal.h> #include <entrada_acciones_mensajes/ContadorResult.h> #include <entrada_acciones_mensajes/ContadorFeedback.h>
Creamos una clase llamada Contador, que contendrá nuestra acción. Primero instanciamos un servidor para la acción llamado _as , usando métodos de la clase actionlib , en nuestro caso actionlib::SimpleactionServer, a este servidor le decimos que espere datos tipo <entrada_acciones_mensajes::ContadorAction> ,este mensaje se creó automáticamente al crear el mensaje Counter.action en el paquete anterior. Es decir creamos un servidor para la acción(no lo iniciamos) y le decimos que el mesnaje a usar para la acción es ContadorAction.
actionlib::SimpleActionServer<entrada_acciones_mensajes::ContadorAction> _as;
Ahora creamos un contador, y creamos un constructor al que le pasamos como parámetros un nodeHandle _nh, el nombre de la accion que queremos crear y le pasamos una callback en nuestro caso llamada cb, que ahora explicaremos. Esta callback se ejecutará cada vez que le pasemos un goal a la acción.
Al iniciar el constructor el servidor _as, se iniciará ya que llamamos a su función start().
int _contador;
public:
Contador():
_as(_nh, "/contador",
boost::bind(&Contador::cb, this, _1),
false),
_contador(0)
{
_as.start();
ROS_INFO("Servidor de la accion iniciado");
}
esta funcion callback se encargará de:
- Obtener el valor goal (que nos lo pasará el cliente ) y pasarlo a una variable local
int g =goal->objetivo;
- Poner el contador a cero la primera vez
- Si se detecta que la acción está cancelada externamente (preempted) saldrá del bucle while. Si el contador llega al numero goal que hemos pasado se saldrá del bucle while.
- Si no se da la condición para salir del while, se incrementará el contador.
- Y muy importante, se crea una variable feedback del tipo entrada_acciones_mensajes::ContadorFeedback , la cual definimos como el cociente entre lo que llevamos de contador entre el objetivo. Esto se almacena en la variable proporción de nuestro mensaje
feedback.proporcion = (double)_contador / (double)g;
Para posteriormente publicarla con nuestro servidor _as para que pueda ser leída por el cliente
_as.publishFeedback(feedback);
Luego crearemos un archivo del tipo entrada_acciones_mensajes::ContadorResult llamado result donde guardaremos el valor del contador.
Este valor del resultado se llevará al cliente y se mostrará según la acción haya sido existosa, cancelada externamente (preempted), o abortada.
Pues bien, ya hemos creado el servidor de nuestra acción, pero para que funcione debemos modificar nuestro archivo CmakeLists.txt
En dicho archivo modificaremos, (recuerda que lo puedes ver en Github):
find_package(catkin REQUIRED COMPONENTS entrada_acciones_mensajes roscpp std_msgs actionlib actionlib_msgs )
Añadimos los paquetes de los que depende nuestro paquete
## System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS system)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
Añadimos estas líneas para habilitar el Boost Bind que hemos usado en el servidor de la acción.
add_executable(contador_servidor src/servidor.cpp)
target_link_libraries(contador_servidor ${catkin_LIBRARIES})
y por último añadimos el ejecutable y las librerías
Aquí acaba esta primera parte con la creación del mensaje y el servidor de la acción. No te pierdas la siguiente parte donde crearemos el cliente para nuestra acción. Hasta la próxima!!