Notas de investigación de automóviles ROS: gestión del sistema de coordenadas ROS TF

TF se utiliza para administrar y consultar la transformación del sistema de coordenadas del robot. A través de TF, podemos obtener la relación posicional entre los dos sistemas de coordenadas de cualquier robot en 10 segundos

TF utiliza un modelo de emisión/escucha. Las coordenadas de cada nodo forman un árbol TF para guardar la transformación de coordenadas entre nodos. Si un nodo quiere obtener una determinada transformación del sistema de coordenadas, puede consultar a través del árbol TF

Herramienta de árbol visual tf en el paquete tf:

rosrun tf view_frames

Escuche el árbol tf durante 5 segundos y guarde el resultado, se generará un archivo pdf

rosrun tf tf_echo Sistema de coordenadas 1 Sistema de coordenadas 2

Obtenga la transformación tf entre dos sistemas de coordenadas, que se pueden detectar en tiempo real

Implementación de transmisión y escucha de tf (basado en el caso oficial de seguimiento de tortugas)

locutor de tf

#!/usr/bin/env python

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
	br = tf.TransformBroadcaster()
	br.snedTransform((msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world")

if __name__ == '__main__':
	rospy.init_node('turtle_tf_broadcaster')
	turtlename = rospy.get_param('~turtle')
	rospy.Subscriber('/%s/pose' % turtlename, turtlesim.msg.Pose, handle_turtle_pose, turtlename)
	rospy.spin()

1

br = tf.TransformBroadcaster()

Cree un emisor de coordenadas tf para transmitir coordenadas tf

2

br.snedTransform((msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world")

Crear contenido de transmisión coordinado tf. Dado que esta línea está en la función de devolución de llamada Pose, usamos (x, y, 0) obtenida en el tema Pose (porque el programa tortuga es un plano y no hay eje z) como las coordenadas de posición de transmisión. Theta en Pose se usa como coordenadas angulares, marca de tiempo (hora actual rospy.Time.now()) y dos sistemas de coordenadas para transmitir

oyente

#!/usr/bin/env python

import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
	rospy.init_node('turtle_tf_listener')
	listener = tf.TransformListener()

	rospy.wait_for_service('spawn')
	spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
	spawner(4, 2, 0, 'turtle2')

	turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)

	rate = rospy.Rate(10.0)
	while not rospy.is_shutdown():
		try:
			(trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
		except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
			continue
	
		angular = 4 * math.atan2(trans[1], trans[0])
		linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
		cmd = geometry_msgs.msg.Twist()
		cmd.linear.x = linear
		cmd.angular.z = angular
		turtle_vel.publish(cmd)

		rate.sleep()

listener = tf.TransformListener()

Crear un oyente tf

(trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))

Use el oyente para monitorear la transformación tf de verdad2 y tortuga1 en el momento actual (rospy.Time(0)). Obtener posición trans y rotacion rot

		angular = 4 * math.atan2(trans[1], trans[0])
		linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
		cmd = geometry_msgs.msg.Twist()
		cmd.linear.x = linear
		cmd.angular.z = angular
		turtle_vel.publish(cmd)

Calcule la velocidad lineal y la velocidad angular del comando de velocidad en función de las coordenadas de traslación y rotación obtenidas. Emita el comando de velocidad Twist para controlar la tortuga a seguir