Como escrever um listener em python para o Pioneer P3-AT

Por renvieir - julho 05, 2016

Uma das coisas mais legais do Ros é que você pode usar tanto a linguagem C++, como o python que é uma linguagem mais fácil.

Então nesse post vamos aprender como escrever um programa para ler os dados da odometria do robô Pioneer que está rodando o RosAria

O Código

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import *
from tf.transformations import euler_from_quaternion
from math import *

def get_yaw(odom_data):
    orientation = odom_data.pose.pose.orientation
    quaternion = (
      orientation.x,
      orientation.y,
      orientation.z,
      orientation.w)

    return euler_from_quaternion(quaternion)[-1]

def odometry_callback(odom_data):
  x, y = odom_data.pose.pose.position.x, odom_data.pose.pose.position.y
  theta = get_yaw(odom_data)
  rospy.loginfo('x, y, theta: \n%s', [x, y, theta])

rospy.init_node('p3at_node', anonymous=True)
rospy.Subscriber('/sim_p3at/odom', Odometry, odometry_callback)
rospy.spin()

Basicamente nós precisamos iniciar um node, linha 25.
Depois se inscrever no canal que queremos ouvir, linha 26, passando o nome do canal, o tipo de dados que será passado e uma função para tratar os dados.
E dar o comando pra ficar rodando o programa indefinidamente, linha 27.

A função odometry_callback, linha 20, pega os dados da odometria e retorna a pose do robô (x, y, theta).

A função get_yaw serve apenas para pegar o ângulo do robô, visto que na odometria esse ângulo não é passado diretamente

É isso, para mais informações leiam as referências

Referências

Writing Publisher and Subscriber in python
Quaternion transformations in python

Pesquisar este blog

Robotica de Campo