transfer.py就是把vins的坐标系转为PX4的坐标系,其实也是个ROS功能包,包含代码分析。
transfer.py
这个脚本干什么工作呢,这个脚本就是之前我跟大家说的(他可能前面讲了看来基础要听听),它把vins它的那个世界坐标系跟px4它定义的北东地,mavros对应的是东北天,它的那个坐标系做了一个转换,然后同时以mavros vins pose 的形式发出去了,这样的话PX4就能接受到你vins发出的数据,就干这样一件事。
这个时候你打开QGC的analyze,
你去监控它的这个叫做local_position_NED这样的一个数据
你会发现X,Y,Z都已经出来了,就这个数据就是vins发给飞机的数据,然后下一步你要检查它是不是真的是NED坐标系下的,就是你观察下磁罗盘(地面站上有现实,视频里有),现在是朝北,那你就往北往前,往北往后,然后往东往西往上往下,看看它对应的是不是X的正向往北,Y的正向是东,然后Z的正向是地,就是向下,这都对应上了,你再晃来晃去晃来晃去,你发现它跟你的这个一个是尺度,就你往前走一米是不是真的是X变大了一米,往上走一米是不是Z变小了一米,就这些都对应上了之后,然后再飞行,就是才能保证你的这个飞机就是大概率在空中不会出现问题,这个是在飞行之前一定要检查好的,这个是起飞前检查。(现在我回看这个我可以理解了,之前最开始看的时候还觉得怎么这么麻烦,现在很理解,是的,要确保vins给无人机的坐标位置是对的,不然无人机肯定乱啊,你实际往后飞,但是给的坐标是往前,飞控要反向控制那就往后,那这样飞机肯定失控啊,这个很好理解了现在,对吧,这个确实要检查好,这可能就跟你光流模块方向没装对一样的,这里还可以查看实际vins给无人机发的位置信息,我觉得这个挺好的,这个非常棒。还有一个没弄明白,为什么后面每次飞摆的方向要和第一次的方向一样呢)
第一期13节
可以结合我这篇博文来看
下面是transfer.py的代码,仔细看它其实也是一个ROS功能包,里面有subscriber,有publisher,所以运行transfer.py其实是启动了一个ROS节点,这个节点的功能专门负责这个坐标系变换的,这样就好理解了。不过老师还说了转换后是以mavros vins pose 的形式发出去的,也就是MAVROS可以直接处理了,也就是实际传给PX4的就是这些信息?
=======================================================================================
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
def callback(data):
#rospy.loginfo(str(data.pose.position.x)+','+str(data.pose.position.y)+','+str(data.pose.position.z))
global posx, posy, posz, qx, qy, qz, qw
posx = data.pose.pose.position.x
posy = -data.pose.pose.position.y
posz = data.pose.pose.position.z
qx = data.pose.pose.orientation.x
qy = data.pose.pose.orientation.y
qz = data.pose.pose.orientation.z
qw = data.pose.pose.orientation.w
posx, posy, posz, qx, qy ,qz ,qw = 0, 0, 0, 0, 0, 0, 1
rospy.init_node('transfer')
rospy.Subscriber("/vins_estimator/camera_pose", Odometry, callback)
# spin() simply keeps python from exiting until this node is stopped
position_pub = rospy.Publisher("/mavros/vision_pose/pose", PoseStamped, queue_size=2) #creates a publisher object
setpoint_msg = PoseStamped()
rate = rospy.Rate(60) #the rate will take care of updating the publisher at a specified speed - 50hz in this case
#loop_count = 0
while True:
setpoint_msg.header.stamp = rospy.Time.now()
#setpoint_msg.header.seq = loop_count
#setpoint_msg.header.frame_id = 'fcu'
#rospy.loginfo(str(posx)+','+str(posy)+','+str(posz))
setpoint_msg.pose.position.x = posx
setpoint_msg.pose.position.y = posy
setpoint_msg.pose.position.z = posz
setpoint_msg.pose.orientation.x = qx
setpoint_msg.pose.orientation.y = qy
setpoint_msg.pose.orientation.z = qz
setpoint_msg.pose.orientation.w = qw
position_pub.publish(setpoint_msg)
#loop_count = loop_count + 1
rate.sleep()
#creates a message object of type pose stamped #http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html
=========================================================================================