目录
- 1.首先理解里程计是什么?
- 2.里程计发布流程
- 3.发布里程计TF变换
- 2.1c++发布TF变换
- 2.2 python发布TF变换
根据阿克曼转向结构的车辆实现里程计消息的发布,本文参考博客如下,
古月居: 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()
rospy.init_node("IMU_pub")
pub = rospy.Publisher("IMU", Imu, queue_size=100)
odom_pub = rospy.Publisher("odom", Odometry, queue_size=100)
rate = rospy.Rate(100)
send_data = Imu()
send_Od_data = Odometry()
br = tf2_ros.TransformBroadcaster()
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()
vx = mpu.read_velocity_x()
vy = mpu.read_velocity_y()
vz = mpu.read_velocity_z()
ax = mpu.read_acceleration_x()
ay = mpu.read_acceleration_y()
az = mpu.read_acceleration_z()
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
send_data.angular_velocity.x = vx
send_data.angular_velocity.y = vy
send_data.angular_velocity.z = vz
send_data.linear_acceleration.x = ax
send_data.linear_acceleration.y = ay
send_data.linear_acceleration.z = az - 9.8
pub.publish(send_data)
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
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()
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)