Cómo escribir un suscriptor y editor en ROS

Los Tutoriales de ROS cubren cómo se puede escribir un suscriptor y un editor en C ++ y en Python con cierto detalle. Entiendo que estos dos tutoriales pueden ser confusos y que la única forma de mejorar el trabajo con ROS es ensuciarse las manos y escribir el código usted mismo.

En esta respuesta, repasaré el proceso de escribir un suscriptor (para obtener datos de los sensores integrados de un Parrot AR Drone 2.0) y un editor (para publicar comandos de control en un Parrot AR Drone 2.0).

Sin embargo, antes de comenzar, hay algunos aspectos del AR Drone 2.0 con los que el lector de esta respuesta debería sentirse cómodo. El AR Drone tiene un controlador interno que estabiliza el drone (realiza el mantenimiento de la posición) y ayuda en el despegue y el aterrizaje. El controlador interno toma los comandos de control correspondientes a la velocidad de cabeceo, balanceo, guiñada y vertical y los convierte en comandos de control básicos de tasas de rotación y empuje y, por lo tanto, controla el cuadrotor

Ahora, hay un paquete ros de controlador de código abierto (ardrone_autonomy) que

(1) ayuda a tu computadora a conectarse al AR Drone a través de wifi

(2) publica la información de los sensores y cámaras integrados en los temas / ardrone / navdata, / ardrone / navdata_gps, / ardrone / imu, / ardrone / mag, / ardrone / front / image_raw y / ardrone / bottom / image_raw topics.

(3) se suscribe al tema / ardrone / takeoff, / ardrone / land y / cmd_vel y envía los datos publicados por el mismo tema al drone.

Habiendo obtenido un conocimiento previo necesario, comencemos con el código para un suscriptor que se suscribe al tema / ardrone / navdata ———

#include “ros / ros.h” // conveniencia
#include “ardrone_autonomy / Navdata.h” // incluye el mensaje Navdata en el paquete ardrone_autonomy

El mensaje (del tema / ardrone / navdata) a incluir se puede encontrar en el comando de terminal “rostopic type / ardrone / navdata”

ardrone_autonomy :: Navdata drone_navdata; // usando esta instancia de clase para almacenar navdata obtenida después de suscribirse al tema

// a continuación se muestra la definición de la función de devolución de llamada que se llamará cuando llegue un nuevo mensaje sobre el tema / ardrone / navdata. El nuevo mensaje se almacena en la instancia de clase drone_navdata para facilitar su uso en otras partes del código.

Los detalles sobre el mensaje se pueden obtener del comando de terminal “rosmsg show ardrone_autonomy / Navdata”

void poseCallback (const ardrone_autonomy :: Navdata :: ConstPtr & pose_message)
{
drone_navdata.vx = pose_message-> vx;
drone_navdata.vy = pose_message-> vy;
drone_navdata.vz = pose_message-> vz;
drone_navdata.ax = pose_message-> ax;
drone_navdata.ay = pose_message-> ay;
drone_navdata.az = pose_message-> az;
drone_navdata.rotX = pose_message-> rotX;
drone_navdata.rotY = pose_message-> rotY;
drone_navdata.rotZ = pose_message-> rotZ;
drone_navdata.magX = pose_message-> magX;
drone_navdata.magY = pose_message-> magY;
drone_navdata.magZ = pose_message-> magZ;
drone_navdata.altd = pose_message-> altd;
drone_navdata.tm = pose_message-> tm;
drone_navdata.header = pose_message-> header;
drone_navdata.batteryPercent = pose_message-> batteryPercent;
}

int main () {
ros :: init (argc, argv, “waypuoint_nav”);
ros :: NodeHandle n; // Nodehandle es como una clase donde estás operando
ros :: Suscriptor pose_subscriber = n.subscribe (“/ ardrone / navdata”, 200, poseCallback); // suscríbase al tema / ardrone / navdata con el maestro para obtener un mensaje de tipo ardrone_autonomy / Navdata

while (ros :: ok ())
{
int var = getchar ();
interruptor (var)
{
caso ‘p’:
print_subscriptions_data ();
descanso;
}
}
}

void print_subscriptions_data ()
{
ros :: Rate loop_rate (10); // esto le permite especificar una frecuencia en la que le gustaría recorrer el siguiente ciclo. (10 entre paréntesis es equivalente a una tasa de 10 Hz)
while (ros :: ok ())
{
ROS_INFO (“Escuché que la altitud es: [% s]”, drone_navdata.altd); // imprime “Escuché que la altitud es: (altitud_valor)

ros :: spinOnce (); // porque el tema recibe mensajes continuamente, a menos que se mencione esto, los valores almacenados en la instancia de clase drone_navdata no cambiarán desde el primer valor recibido por el tema
loop_rate.sleep (); // usamos el objeto ros :: Rate para dormir durante el tiempo restante que nos permite alcanzar nuestra tasa de suscripción de 10Hz
}
}

Tenga en cuenta que los comentarios al lado de cada línea de código explican cuál es el propósito de esa línea.

A continuación veremos cómo publicar comandos de despegue y aterrizaje en / ardrone / takeoff y / ardrone / land temas respectivamente. Además, para publicar comandos de velocidad (también conocido como control) en el tema / cmd_vel para poder controlar el dron, se puede crear un editor de la siguiente manera:

#include “ros / ros.h”
#include “std_msgs / Empty.h” // incluye un mensaje vacío del paquete std_msgs (para comandos de despegue y aterrizaje)
#include “geometry_msgs / Twist.h” // incluye el mensaje Twist del paquete geometry_msgs (para comandos de velocidad)
#include “geometry_msgs / Vector3.h” // incluye el mensaje Twist del paquete Vector3 (para comandos de velocidad)

std_msgs :: Empty emp_msg; // Este es un objeto de mensaje. Lo rellena con datos y luego lo publica. (para comandos de despegue y aterrizaje)
geometry_msgs :: Twist vel_msg; // Esto también es un objeto de mensaje. Lo rellena con datos y luego lo publica. (para comandos de velocidad)

int main () {
ros :: init (argc, argv, “waypuoint_nav”);
ros :: NodeHandle n;
ros :: Publisher T_pub_empty = n.advertise (“/ ardrone / takeoff”, 1);
ros :: Publisher L_pub_empty = n.advertise (“/ ardrone / land”, 1);
ros :: Publisher velocity_publisher = n.advertise (“/ cmd_vel”, 100);

// dile al maestro que vamos a publicar mensajes de los tipos std_msgs / Empty, std_msgs / Empty y geometry_msgs / Twist sobre los temas / ardrone / takeoff, / ardrone / land y / cmd_vel respectivamente

while (ros :: ok ())
{
int p = getchar ();
interruptor (var):
caso ‘t’:
quitarse();
descanso;
caso ‘l’:
tierra();
descanso;
caso ‘m’:
mover (1,1, -1,1, -1, -1);
descanso;
caso ‘f’:
move_forward_continuously ();
descanso;
}
}

despegue nulo ()
{
T_pub_empty.publish (emp_msg);
ROS_INFO (“ARdrone lanzado”);
}
tierra vacía ()
{
L_pub_empty.publish (emp_msg);
ROS_INFO (“ARdrone aterrizó”);
}
movimiento nulo (lx, ly, lz, ax, ay, az)
{
// rellenando los datos de velocidad lineal en el objeto a continuación
vel_msg.linear.x = lx;
vel_msg.linear.y = ly;
vel_msg.linear.z = lz;
// rellenando los datos de velocidad angular en el objeto a continuación
vel_msg.angular.x = ax;
vel_msg.angular.y = ay;
vel_msg.angular.z = az;
// publicando el objeto a continuación
velocity_publisher.publish (vel_msg);
}
nulo move_forward_continuously ()
{ros :: Rate loop_rate (10);
while (ros :: ok ())
{
mover (1,0,0,0,0,0);

ros :: spinOnce ();
loop_rate.sleep ();
}

Tenga en cuenta que el código anterior no está completo. No se han mencionado muchas declaraciones de funciones. No es más que darle al lector una idea muy centrada sobre cómo publicar comandos y suscribirse a datos con respecto a un AR Drone.

Todo el código anterior es parte de un repositorio github [1] que consiste en paquetes ROS y nodos para realizar maniobras básicas con el AR Drone. (más documentación se puede encontrar allí)

Notas al pie

[1] kaustubhsridhar / AR.Drone-basic-control

Todos los tutoriales están presentes en la wiki oficial de ROS. Intenta buscar en Google primero.

Use este tutorial para escribir un suscriptor y editor en C ++, y esto para Python.