Como escrever um subscriber em C++ para o Pioneer P3-AT

Programa em C++ para ler os dados de odometria do Pioneer P3-AT (subscriber).

Introdução

Nesse post iremos demonstrar como ler as mensagens que o ROS escuta quando executamos um node. Fugindo um pouco de termo técnicos, o que queremos é quando dermos algum comando (node) para o robô executar através dos sensores (odometria, sonar, laser) , mostrar como ler esses dados que os sensores retornam.

Imagine o seguinte problema, usando pacote RosAria - pacote para controlar o robô - queremos ler os dados da odometria do robô e imprimir esses dados.

Lembrando, o pacote RosAria está instalado e o código foi devidamente testado em um robô pioneer P3AT.

Workspace:

O arquivo com o código deve ser criado dentro do $workspace_name/src/rosaria.

Usamos catkin para criarmos um espaço de trabalho, para ver como criar um espaço de trabalho usando o catkin e usar o ROS clique aqui.

Passo 1: Adicionando as bibliotecas

Primeiramente é necessário colocar as bibliotecas: 

1 2 3 4 5

#include <ros/ros.h> #include <tf/tf.h> #include <geometry_msgs/Twist.h> #include <geometry_msgs/Pose.h> #include <nav_msgs/Odometry.h>

Passo 2: Criando a função para ler a odometria do P3AT.

Adicione essa função no seu código.
O que essa função faz é printar no terminal através do ROS_INFO da biblioteca ros.h os valores que a odometria está retornando.  Essa função fica fora da main.

1 2 3 4 5 6 7 8 9 10 11

void chatterCallback(const nav_msgs::Odometry::ConstPtr& msg){ ROS_INFO("Seq: [%d]", msg->header.seq); ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", msg->pose.pose.position.x,msg->pose.pose.position.y, msg->pose.pose.position.z); ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); ROS_INFO("Vel-> Linear: [%f], Angular: [%f]", msg->twist.twist.linear.x,msg->twist.twist.angular.z); }

Passo 3: Criando a Main e chamando a função.

Na função main do código ficará assim: 

1 2 3 4 5 6 7 8 9 10 11 12 13

int main(int argc, char **argv){ ros::init(argc, argv, "subP3AT"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("/RosAria/pose",1000, chatterCallback); ros::spin(); return 0; }

Passo 4: Criando o node

É necessário adicionar 3 linhas no arquivo CMakeLists.txt que está na pasta $workspace_name/src/rosaria.
As linhas são:

1 2 3

add_executable(subOdom subscriber_odom.cpp) add_dependencies(subOdom nav_msgs_generate_messages_cpp) target_link_libraries(subOdom ${catkin_LIBRARIES})

O nome do nosso node será subOdom que está dentro da pasta rosaria no workspace.

Após esse passo é necessário compilar o catkin novamente:

1	$catkin_make

Ele deve criar o node subOdom

Passo 5: Executando o node no robô:

Após rodar $roscore rode o seguinte comando para iniciar os drivers do rosaria:

1	$rosrun rosaria RosAria

E então somente o node que criamos para ler os dados da odometria:

1	$rosrun rosaria subOdom

Bom é isso.