本文共 3368 字，大约阅读时间需要 11 分钟。

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的就是这些信息？

 

不对啊，MAVROS不是会自动进行坐标转换么？

 

我现在知道那个老师说的以mavros vins  pose 的形式发出去是什么了

他的程序本身就是借鉴的GAAS的，GAAS的SLAM的例程里发布的信息有下面这些其中一个就是

/mavros/vision_pose/pose

那个老师只需要把vins节点发出的pose信息转为了GAAS里 /mavros/vision_pose/pose 的信息一样的格式，其他部分就可以完全照搬GAAS的就可以了！！！！！！！它的transfer文件其实起的这个作用，我如果用T265一样的道理。GAAS其他部分的程序会把这个pose最后传给PX4。你不管用什么VSLAM算法都可以套用这么一个框架，只需要把SLAM节点改动一下就可以了。

 

 

而且我发现GAAS给的SLAM教程里面就是写的 /mavros/vision_pose/pose  查看pose消息就是查看 /mavros/vision_pose/pose！！！！！！我今天上午想到去看看果然真的是一样的！！！！！！！！！！！！！！！！！！！！！我们只需要让我们自己的SLAM节点发出的消息和这个格式一样，其他的应该可以直接套用GAAS的就可以了。

 

 

最后我发现其实就是MAVROS wiki里面定义的

MAVROS wiki的最后一段文字我细读了一下发现了一个东西

这不正是我哦想做的视觉SLAM输出位置信息么！！！！！！

真的好好读好官方文档太重要了！！！！！！！

 

=======================================================================================

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 =========================================================================================

 

 

转载地址：http://vzlni.baihongyu.com/