ros中tf和odom的创建，转换与发布

在gazebo仿真移动机器人时，车轮差速控制器会根据车轮的运动计算机器人移动的距离和速度，发布一个里程计 tf（tf由 一个坐标系frame以及这个坐标系到其他任意一个坐标系的转换关系 这两个部分组成。里程计frame系通常命名为 odom，也就是frame_id="odom"）和一个Odometry类型的topic，这个topic包含的内容是里程计frame的位置和速度，也就是根据车轮运动而计算出来的机器人的位置和速度。

在机器人xacro模型文件中，我们定义了一个机器人的基准点，这个基准点通常命名为"base_link"。需要建立里程计frame到base_link的tf转换，并将这个tf发布，在rviz中设定fixed frame为odom，才能在rviz中正确看到机器人的运动。ros navigation package也需要Odometry类型的消息和里程计tf。

如果不使用车轮里程计，也可及将gps坐标值作为里程计frame的位置，并根据IMU计算Odometry topic中的速度，供ros navigation package使用。里程计 frame的位置是gps的坐标，也可以初始化一个点，使用gps的相对坐标。

import sys
from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QTextEdit, QPushButton
from PyQt5.QtCore import QThread, pyqtSignal
import rospy
import time
import numpy as np
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped, Point, Pose, Twist, Vector3


def update_velocity_position(acceleration, velocity, position, dt=0.1):
	theta = position[5]
	# 随体坐标系速度转odom标系速度
	_velocity = np.array([velocity[0], velocity[1], velocity[5]])
	velocity_odom = np.array([[np.cos(theta), -np.sin(theta), 0],
	                          [np.sin(theta), np.cos(theta), 0],
	                          [0, 0, 1]]) @ _velocity
	
	velocity_ = velocity + acceleration * dt
	_velocity_ = np.array([velocity_[0], velocity_[1], velocity_[5]])
	velocity_odom_ = np.array([[np.cos(theta), -np.sin(theta), 0],
	                          [np.sin(theta), np.cos(theta), 0],
	                          [0, 0, 1]]) @ _velocity_
	position_delta = (velocity_odom + velocity_odom_) * dt / 2
	position[0] += position_delta[0]
	position[1] += position_delta[1]
	position[5] += position_delta[2]
	
	normalize_angle = np.vectorize(lambda angle: (angle + np.pi) % (2*np.pi) - np.pi)
	position[3:] = normalize_angle(position[3:])
	velocity = velocity_.copy()
	
	return velocity, position


class ROSPublisherThread(QThread):
	message_published = pyqtSignal(str)

	def __init__(self, parent=None):
		super(ROSPublisherThread, self).__init__(parent)
		self.content_list = []
		
		self.tf_broadcaster = tf.TransformBroadcaster()
		self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=10)
		
		# 设置初始速度，初始位置
		self.position = np.array([3.0, 0, 0, 0, 0, 0])  # X Y Z phi psi yaw unit m rad
		self.velocity = np.array([0, 0, 0, 0, 0, 0])  # linear velocity, angular velocity
		self.acceleration = np.array([0, 0, 0, 0, 0, 0.0])

	def run(self):
		while True:
			for index, content in enumerate(self.content_list):
				self.acceleration[index] = float(content)
				
			self.velocity, self.position = update_velocity_position(self.acceleration, self.velocity, self.position, 0.1)
			# 创建一个TransformStamped消息
			transform_msg = TransformStamped()
			transform_msg.header.stamp = rospy.Time.now()
			transform_msg.header.frame_id = 'odom'
			transform_msg.child_frame_id = 'base_link'
			
			# 设置坐标变换的平移部分
			transform_msg.transform.translation.x = self.position[0]
			transform_msg.transform.translation.y = self.position[1]
			transform_msg.transform.translation.z = self.position[2]
			# 设置坐标变换的旋转部分（四元数表示）
			euler_angles = self.position[3:] # 以弧度表示的Roll、Pitch、Yaw
			quaternion = tf.transformations.quaternion_from_euler(*euler_angles)
			transform_msg.transform.rotation.x = quaternion[0]
			transform_msg.transform.rotation.y = quaternion[1]
			transform_msg.transform.rotation.z = quaternion[2]
			transform_msg.transform.rotation.w = quaternion[3]
			# 发布坐标变换
			self.tf_broadcaster.sendTransformMessage(transform_msg)
			
			odom_msg = Odometry()
			odom_msg.header.stamp = rospy.Time.now()
			odom_msg.header.frame_id = 'odom'
			odom_msg.child_frame_id = 'base_link'
			# 设置位置信息
			odom_msg.pose.pose = Pose(Point(x=self.position[0], y=self.position[1], z=self.position[2]), quaternion)
			# 设置速度信息
			odom_msg.twist.twist = Twist(Vector3(self.velocity[0], self.velocity[1], self.velocity[2]), Vector3(self.velocity[3], self.velocity[4], self.velocity[5]))
	
			time.sleep(0.1)
			
			print("publish{:.1f} {:.1f} {:.1f} {:.1f} {:.1f} {:.1f}".format(self.position[0], self.position[1], self.position[2]
			                                                                , self.position[3], self.position[4], self.position[5]))


class MyROSApp(QWidget):
	def __init__(self):
		super().__init__()
		rospy.init_node('my_ros_app', anonymous=True)

		self.layout = QVBoxLayout()

		self.text_edits = [QTextEdit() for _ in range(6)]

		for text_edit in self.text_edits:
			text_edit.setText('0')
			self.layout.addWidget(text_edit)

		self.send_button = QPushButton('Send to ROS')
		self.send_button.clicked.connect(self.start_ros_thread)
		self.layout.addWidget(self.send_button)
		
		self.update_button = QPushButton("Update ROS Topic")
		self.update_button.clicked.connect(self.update_content)
		self.layout.addWidget(self.update_button)

		self.setLayout(self.layout)

		self.ros_thread = ROSPublisherThread()
		self.ros_thread.message_published.connect(self.on_message_published)

	def start_ros_thread(self):
		self.ros_thread.start()
	
	def update_content(self):
		self.ros_thread.content_list = [text_edit.toPlainText() for text_edit in self.text_edits]

	def on_message_published(self, message):
		print(message)


if __name__ == '__main__':
	app = QApplication(sys.argv)
	my_app = MyROSApp()
	my_app.show()
	sys.exit(app.exec_())