ROS中发布里程计消息（Odometry）

根据阿克曼转向结构的车辆实现里程计消息的发布，本文参考博客如下，

古月居： https://www.guyuehome.com/332
ROS WiKi：http://wiki.ros.org/cn/navigation/Tutorials/RobotSetup/Odom

1.首先理解里程计是什么？

在ROS中，里程计消息保存了机器人空间里评估的位置和速度信息。它首先是一个坐标系的变化，表示的是从Odom坐标系到base_link的坐标变化关系。其次里程计信息中含有车辆的速度信息，可以用于其他用途。里程计的坐标系原点，笔者理解为程序启动后，起始时刻的位置信息，在车辆运动后，位置和速度会发生变化，其中的TF变换关系就是当前时刻位置与起始时刻位置三维变换。要完成里程计的发布，首先应发布base_link(车辆基座标系)到里程计坐标系的TF变化。其次，发布odom信息，包含车辆的速度信息。（如有错误，请留言指正）

2.里程计发布流程

1.对TF变换以及Odom话题初始化发布者
2.获取base_link相对于Odom坐标系的位置和旋转关系
3.发布从Odom到base_link坐标的TF变换，可参考本人另一篇博客：ROS中TF变换详解
4.发布Odom话题

3.发布里程计TF变换

在本节中，编写示例代码，用于在ROS上发布nav_msgs/Odometry消息，程序由C++和python两种语言书写

2.1c++发布TF变换

在编写程序之前，需要在package.xml文件中添加依赖信息，如下：

<depend package="tf"/>
<depend package="nav_msgs"/>

2.2 python发布TF变换

首先导入库函数

from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions

初始化节点等信息

	mpu = JY901()  # 初始化IMU
    rospy.init_node("IMU_pub")  # 初始化节点
    pub = rospy.Publisher("IMU", Imu, queue_size=100)  #IMU话题发布者
    odom_pub = rospy.Publisher("odom", Odometry, queue_size=100)  # 里程计话题发布者
    rate = rospy.Rate(100)  # 发送速度
    send_data = Imu()  # 要发送IMU数据
    send_Od_data = Odometry() #  要发送的Odom数据
    br = tf2_ros.TransformBroadcaster()  # 定义TF变换广播者
    trans = TransformStamped()  # 定义广播者要广播的数据类型
    x_base_link = 0
    y_base_link = 0
    z_base_link = 0
    Vx = 0
    Vy = 0
    Vz = 0
    time_now = rospy.Time.now().to_sec()
    print("start******")

思路：每次读取IMU，记录读取时间间隔。根据以及读取到的IMU数据计算出测量间隙之间的位移量，即可求得base_link相对于Odom的位移。JY901带有磁力计，通过IIC读取偏航角，转换为弧度，计算出base_link与Odom之间的旋转关系，最后用tf工具转换为欧拉角，发布TF变换和Odom话题

	Q0 = mpu.read_Q0()
	Q1 = mpu.read_Q1()
	Q2 = mpu.read_Q2()
	Q3 = mpu.read_Q3() # 从IMU读取转换好的四元数
	vx = mpu.read_velocity_x()
	vy = mpu.read_velocity_y()
	vz = mpu.read_velocity_z()  # 读取base_link三轴的角速度
	ax = mpu.read_acceleration_x()
	ay = mpu.read_acceleration_y()
	az = mpu.read_acceleration_z()  # 读取base_link三轴的加速度信息
	# data = mpu.read_yaw()  # 读取base_link的偏航角（与正东方向夹角）
	# 添加msgs的head数据

	# 发布IMU话题
	send_data.header.stamp = rospy.Time.now()
	send_data.header.frame_id = "base_link"
	# 添加四元数
	send_data.orientation.x = Q0
	send_data.orientation.y = Q1
	send_data.orientation.z = Q2
	send_data.orientation.w = Q3
	# 添加angular_velocity 角速度
	send_data.angular_velocity.x = vx
	send_data.angular_velocity.y = vy
	send_data.angular_velocity.z = vz
	# # 添加linear_acceleration
	send_data.linear_acceleration.x = ax
	send_data.linear_acceleration.y = ay
	send_data.linear_acceleration.z = az - 9.8
	# publish topic
	pub.publish(send_data)
	
	
	# 发布TF变换
	time_last = time_now
	time_now = rospy.Time.now().to_sec()
	delta_t = time_now - time_last  # 计算时间间隔
	delta_x = vx*delta_t
	delta_y = vy*delta_t
	delta_z = vz*delta_t
	
	Vx = Vx + ax*delta_t
	Vy = Vy + ay*delta_t
	Vz = Vz + az*delta_t
	delta_x = 0.5*Vx*delta_t
	delta_y = 0.5*Vy*delta_t
	delta_z = 0.5*Vz*delta_t
	# TF transform  odom->base_link
	trans.header.stamp = rospy.Time.now()
	trans.header.frame_id = "odom"
	trans.child_frame_id = "base_link"
	trans.transform.translation.x = x_base_link
	trans.transform.translation.y = y_base_link
	trans.transform.translation.z = z_base_link
	# 从偏航角解算出对应的四元数，只考虑平面运动
	q = tf_conversions.transformations.quaternion_from_euler(0, 0, mpu.read_yaw()*pi/180.0)
	trans.transform.rotation.x = q[0]
	trans.transform.rotation.x = q[1]
	trans.transform.rotation.x = q[2]
	trans.transform.rotation.x = q[3]
	br.sendTransform(trans)
	# 发布里程计信息
	send_Od_data.header.frame_id = "odom"
	send_Od_data.header.stamp = rospy.Time.now()
	send_Od_data.child_frame_id = "base_link"
	
	send_Od_data.pose.pose.position.x = x_base_link
	send_Od_data.pose.pose.position.y = y_base_link
	send_Od_data.pose.pose.position.z = z_base_link
	
	send_Od_data.twist.twist.angular.x = vx
	send_Od_data.twist.twist.angular.y = vy
	send_Od_data.twist.twist.angular.z = vz
	odom_pub.publish(send_Od_data)
	
	rate.sleep()