Um programa publisher

Sumário

Publicando mensagens
O loop publish
1. Verificando o desligamento de nó
2. Controlando a taxa de publicação
Compilando pubvel
1. Declarando depedências do tipo de mensagem
Executando pubvel

O programa olá da seção anterior nos mostrou como compilar e executar um programa simples em ROS. Esse programa foi útil como uma introdução ao catkin mas, como todos os programas “Olá, Mundo!”, não faz nada com utilidade. Nessa seção, nós iremos olhar para um programa que interage um pouco mais com ROS. Especificamente, nós iremos olhar como enviar comandos de velocidade gerados aleatoriamente para uma tartaruga turtlesim, fazendo com que vagueie sem rumo. O Breve código fonte C++ para o programa, chamado de pubvel, aparece em Lista 3.4. Esse programa mostra todos os elementos necessários para publicar mensagens do código.

Lista 3.4

Um programa chamado `pubvel.cpp` que publica comandos de movimento gerados aleatoriamente para uma tartaruga `turtlesim`

// Esse programa publica mensagens
// de velocidade geradas aleatoriamente para o turtlesim.
#include <ros/ros.h>
#include <geometry_msgs/Twist.h> // For geometry_msgs : : Twist
#include <stdlib.h> // For rand () and RAND_MAX

int main(int argc , char ∗∗argv) {
// Inicializa o sistema ROS e torna-se um nó.
ros::i n i t (argc, argv, "publish_velocity") ;
ros::NodeHandle nh;

// Cria um objeto editor.
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>(
"turtle1/cmd_vel", 1000) ;

// Coloca o gerador de número aleatório.
srand (time (0)) ;

// Faz um loop em 2Hz até que o nó seja parado.
ros::Rate rate(2);
while(ros::ok()) {
// Cria e preenche a mensagem. Os outros quatro 
// campos, que são ignorados pelo turtlesim, são padronizados como 0.
geometry_msgs::Twist msg;
msg.linear.x = double (rand())/double(RAND_MAX) ;
msg.angular.z = 2∗double (rand())/double(RAND_MAX) − 1;

// Publica a mensagem.
pub.publish(msg) ;

// Envia uma mensagem para o rosout com os detalhes.
ROS_INFO_STREAM("Sending␣random␣velocity␣command: "
<< "␣linear=" << msg.linear.x
<< "␣angular=" << msg.angular.z ) ;

// ESpera até que seja o momento de outra iteração.
rate.sleep() ;
}

Publicando mensagens

As principais diferenças entre pubvel e hello derivam da necessidade de publicar mensagens.

Incluindo a declaração do tipo da menssagem

Você provavelmente vai se lembrar da Seção 2.7 que todo tópico ROS é associado a um tipo de mensagem. Cada tipo de mensagem tem um arquivo de cabeçalho C++ correspondente. Você precisará incluir (#include) esse cabeçalho para cada tipo de mensagem usada no seu programa, com um código desse tipo:

#include <package_name/type_name.h>

Observe que o nome do pacote deve ser o nome do pacote que contém o tipo da mensagem e não (necessariamente) o nome do seu próprio pacote. Em pubvel, nós queremos publicar mensagens do tipo geometry_msgs/Twist — um tipo chamado Twist pertencente a um pacote chamado geometry_msgs — então precisamos da seguinte linha:

#include <geometry_msgs/Twist.h>

O objetivo deste cabeçalho é definir uma classe C ++ que tem os mesmos membros de dados que os dos campos do tipo da mensagem fornecida. Esta classe é definida em um namespace nomeado depois pacote. O impacto prático dessa nomenclatura é que, ao se referir a classes de mensagem no código C ++, você usará dois pontos duplos (::) — também chamado de operador de resolução de escopo — para separar o nome do pacote do nome do tipo. No nosso exemplo pubvel, o cabeçalho define uma classe de nome geometry_msgs::Twist.

Criando um objeto público

O trabalho de realmente publicar as mensagens é feito por um objeto de classe ros::Publisher. Uma linha como esta cria o objeto de que precisamos:

ros::Publisher pub = node_handle.advertise<message_type>(topic_name, queue_size);

Vamos dar uma olhada em cada parte desta linha.

O node_handle é um objeto da classe ros::NodeHandle, uma que você criou perto do começo do seu programa. Estamos chamando o método “advertise” desse objeto.
A parte message_type dentro dos colchetes — formalmente chamada de modelo parâmetro — é o tipo de dados das mensagens que queremos publicar. Esse deve ser o nome da classe definida no cabeçalho discutido acima. No exemplo, nós usamos a classe geometry_msgs::Twist.
O topic_name é uma string que contém o nome do tópico sobre qual queremos publicar. Deve corresponder aos nomes dos tópicos mostrados pela lista rostopic ou rqt_graph, mas (normalmente) sem a barra inicial (/). Tiramos a barra inicial para fazer do nome do tópico um nome relativo; O Capítulo 5 explica a mecânica e os objetivos dos nomes relativos.No exemplo, o nome do tópico é turtle1/cmd_vel.

⚠️ Tenha cuidado com a diferença entre o nome do tópico e o nome da classe. Se você acidentalmente trocar esses dois, você obterá muitos erros de compilação potencialmente confusos.

O último parâmetro a anunciar é um número inteiro que representa o tamanho da fila de mensagens para essa publicação. Na maioria dos casos, um valor razoavelmente grande, digamos 1000, é adequado. Se o seu programa publicar rapidamente mais mensagens do que a fila pode conter, as mensagens não enviadas mais antigas serão descartadas.

⏩ Este parâmetro é necessário porque, na maioria dos casos, a mensagem deve ser transmitida para outro nó. Esse processo de comunicação pode ser demorado, especialmente se comparado ao tempo necessário para criar mensagens. O ROS atenua esse atraso fazendo com que o método publish (veja abaixo) armazene a mensagem em uma fila de “caixa de saída” e retorne imediatamente. Um tópico separado atrás das cenas na verdade transmite a mensagem. O valor inteiro dado aqui é o número de mensagens — e não, como você possa pensar, o número de bytes — que a lista de mensagens possa conter.

Curiosamente, a biblioteca cliente ROS é inteligente o suficiente para saber quando o nó editor e o nó assinante fazem parte do mesmo processo subjacente. Nesses casos, a mensagem é entregada diretamente ao assinante, sem usar nenhum transporte de rede. Este recurso é muito importante para fazer nodelets — ou seja, vários nós que podem ser carregados dinamicamente em um único processo — eficientes.

Se você quiser publicar mensagens sobre vários tópicos do mesmo nó, você precisará criar um objeto ros::Publisher separado para cada tópico.

⚠️ Esteja atento ao tempo de vida de seus objetos ros::Publisher . Criar o editor é uma operação cara, então geralmente é uma má ideia criar um objeto ros::Publisher novo toda vez que quiser publicar uma mensagem. Ao invés disso, crie um publisher para cada tópico, e use esse publisher durante toda a execução de seu programa. Em pubvel, conseguimos isso declarando o publisher fora do loop “while”.

Criando e preenchendo no objeto mensagem

Em seguida, criamos o próprio objeto de mensagem. Nós já referenciamos a classe mensagem quando criamos o objeto ros::Publisher. Objetos dessa classe têm um membro de dados acessível publicamente para cada campo no tipo de mensagem subjacente.

Nós usamos o rosmsg show (Seção 2.7 ) para ver que o tipo da mensagem geometry_msgs/Twist tem dois campos de nível superior (linear e angular), cada um dos quais contém três subcampos (x, y, e z). Cada um desses subcampos é um número de ponto flutuante de 64-bit, chamado de double pela maioria de compiladores C++. O código em Lista 3.4 cria um objeto geometry_msgs::Twist e atribui números pseudo-aleatórios a dois desses membros de dados:

geometry_msgs::Twist msg;
msg.linear.x = double(rand())/double(RAND_MAX);
msg.angular.z = 2*double(rand())/double(RAND_MAX) - 1;

Este código define a velocidade linear para um número entre 0 e 1, e a velocidade angular para um número entre -1 e 1. Porque o turtlesim ignora os outros quatro campos (msg.linear.y, msg.linear.z, msg.angular.x, e msg.angular.y), nós os deixamos com seu valor padrão, que passa a ser zero.

Claro, a maioria dos tipos de mensagem possui campos com tipos diferentes de float64. Felizmente, o mapeamento de tipos de campo ROS para tipos C ++ funciona exatamente da maneira que você espera. Um fato que pode não ser óbvio é que os campos com tipos array — mostrado por colchetes por rosmsg show — são realizados como vetores STL no código C ++.

Publicando a mensagem

Depois de todo esse trabalho preliminar, é muito simples realmente publicar a mensagem, usando o método do objeto ros::Publisher. No exemplo, é assim:

pub.publish(msg);

Este método adiciona a dada msg a fila de mensagens de saída do editor, de onde será enviado o mais rápido possível a todos os assinantes do tópico correspondente.

Formatando a saída

Embora não esteja diretamente relacionado à publicação de nossos comandos de velocidade, a linha ROS_INFO_STREAM em Lista 3.4 vale a pena dar uma olhada. Esta é uma ilustração mais completa do que ROS_INFO_STREAM pode fazer, porque mostra a habilidade de inserir dados exceto strings — nesse caso, os campos de mensagem específicos gerados aleatoriamente — na saída. A seção 4.3 tem mais informações sobre como o ROS_INFO_STREAM funciona

O loop publish

A seção anterior cobriu os detalhes da publicação de mensagens. Nosso exemplo pubvel repete os passos de publicação dentro de um loop while para publicar muitas mensagens diferentes conforme o tempo passa. O programa usa duas construções adicionais para formar esse loop.

Verificando o desligamento de nó

A condição do loop while do pubvel é:

ros::ok()

Informalmente, esta função verifica se nosso programa ainda está em “boa posição” como um nó ROS. Ele retornará true, até que o nó tenha algum motivo para encerrar. Existem algumas maneiras de fazer com que ros :: ok () retorne false:

Você pode usar rosnode kill no nó
Você pode enviar um sinal de interrupção (Ctrl-C) para o programa.

⏩ Curiosamente, ros::init() instala um manipulador para esse sinal, e o usa para iniciar um desligamento elegante. O impacto é que Ctrl-C pode ser usado para fazer o ros::ok() return false, mas não encerra imediatamente o programa. Isso pode ser importante se houver passos de limpeza — escrever log files, salvar resultados parciais, pubvel, nós conseguimos isso declarando o publisher fora do loop while. Dizer adeus, etc — Isso deve acontecer antes do programa terminar.

Você pode chamar, em algum lugar do próprio programa,
```
ros::shutdown()
```

Essa função pode ser uma maneira útil de sinalizar que o trabalho dos seus nós está completo nas profundezas do seu código.

Você pode iniciar outro nó com o mesmo nome. Isso geralmente acontece se você iniciar uma nova instância do mesmo programa

Controlando a taxa de publicação

O último novo elemento de pubvel é o seu uso de um objeto ros::Rate:.

ros::Rate rate(2);

Este objeto controla a rapidez com que o loop é executado. O parâmetro em seu construtor está em unidades de Hz, ou seja, em ciclos por segundo. Este exemplo cria um objeto de taxa projetado para regular um loop que executa duas iterações por segundo. Perto do fim de cada iteração de loop,chamamos o método “sleep” desse objeto.

rate.sleep();

Cada chamada a esse método causa um atraso no programa. A duração do atraso é calculada para prevenir o loop de iterar mais rápido que a taxa especificada. Sem esse tipo de controle, o programa iria publicar mensagens mais rápido do que o computador permite, o que poderia sobrecarregar as filas de publicação e assinatura e desperdiçar recursos de computação e rede.

(No computador do autor, uma versão não regulamentada deste programa atingiu o máximo de 6.300 mensagens por segundo.) Você pode confirmar que este regulamento está funcionando corretamente, usando rostopic hz. Para o pubvel, os resultados devem ser parecidos com isso:

average rate: 2.000
    min: 0.500s max: 0.500s std dev: 0.00006s window: 10

Podemos ver que nossas mensagens estão sendo publicadas a uma taxa de duas por segundo, com muito pouco desvio desse cronograma.

⚠️ Você deve estar pensando em uma alternativa para ros::Rate que use um simples, atraso fixo — talvez gerado pelo sleep ou usleep — em cada iteração de loop. A vantagem de objeto ros::Rate ao invés dessa abordagem é que ros::Rate pode explicar o tempo consumido por outras partes do loop. Se houver cálculos não triviais a serem feitos em cada iteração (como seria de esperar de um programa real), o tempo consumido por este cálculo é subtraído do atraso. Em casos extremos, em que o trabalho real do loop leva mais tempo do que a taxa solicitada, o atraso induzido pelo sleep() pode ser reduzido a zero.

Compilando `pubvel`

O processo de construir o pubvel é basicamente o mesmo que o de hello: Modifique CMakeLists.txt e package.xml, e então usecatkin_make para construir sua área de trabalho. No entanto, há uma diferença importante do hello.

Declarando depedências do tipo de mensagem

Porque pubvel usa um tipo de mensagem do pacote geometry_msg , devemos declarar uma dependência desse pacote. Isso assume a mesma forma que a dependência roscpp discutida na Seção 3.2 . Especificamente, devemos modificar a linha find_package em CMakeLists.txt para mencionar geometry_msg além de roscpp:

find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs)

Observe que estamos modificando a linha find_package existente, ao invés de criar uma nova. Em package.xml, devemos adicionar elementos para a nova dependência:

<build_depend>geometry_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>

⚠️ Se você pular (ou esquecer) esse passo, então catkin_make pode não ser capaz de encontrar o arquivo de cabeçalho geometry_msgs/Twist.h. Quando você encontrar erros de arquivos de cabeçalhos ausentes, é uma boa ideia verificar as dependências do seu package.

Executando `pubvel`

Finalmente, estamos prontos para executar pubvel. Como sempre, rosrun pode fazer o trabalho.

rosrun agitr pubvel

Você pode querer também executar um simulador turtlesim, assim você pode ver a tartaruga reagir aos comandos de movimento que pubvel publica:

rosrun turtlesim turtlesim_node

A Figura 3.1 mostra um exemplo dos resultados.

Figura 3.1: Uma tartaruga turtlesim reagindo a comandos de velocidade aleatórios dopubvel.