transfer.py就是把vins的坐标系转为PX4的坐标系,其实也是个ROS功能包,包含代码分析。(最后发现是改成GAAS里pose的消息形式)

本文主要是介绍transfer.py就是把vins的坐标系转为PX4的坐标系,其实也是个ROS功能包,包含代码分析。(最后发现是改成GAAS里pose的消息形式),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

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节

 

 

可以结合我这篇博文来看

https://blog.csdn.net/sinat_16643223/article/details/107783847?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522159664474519195264549442%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=159664474519195264549442&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~blog~first_rank_v1~rank_blog_v1-1-107783847.pc_v1_rank_blog_v1&utm_term=%E5%9D%90%E6%A0%87%E7%B3%BB&spm=1018.2118.3001.4187

 

 

 

下面是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节点改动一下就可以了。

https://github.com/generalized-intelligence/GAAS/tree/master/software

 

 

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

https://gaas.gitbook.io/guide/software-realization-build-your-own-autonomous-drone/wu-ren-ji-zi-dong-jia-shi-xi-lie-part-3-zai-wu-gps-huan-jing-xia-tong-guo-slam-shi-xian-wei-zhi-gu-j

 

 

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

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

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

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

http://wiki.ros.org/mavros_extras#vision_pose_estimate

 

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

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

 

 

这篇关于transfer.py就是把vins的坐标系转为PX4的坐标系,其实也是个ROS功能包,包含代码分析。(最后发现是改成GAAS里pose的消息形式)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/507683

相关文章

Go标准库常见错误分析和解决办法

《Go标准库常见错误分析和解决办法》Go语言的标准库为开发者提供了丰富且高效的工具,涵盖了从网络编程到文件操作等各个方面,然而,标准库虽好,使用不当却可能适得其反,正所谓工欲善其事,必先利其器,本文将... 目录1. 使用了错误的time.Duration2. time.After导致的内存泄漏3. jsO

springboot循环依赖问题案例代码及解决办法

《springboot循环依赖问题案例代码及解决办法》在SpringBoot中,如果两个或多个Bean之间存在循环依赖(即BeanA依赖BeanB,而BeanB又依赖BeanA),会导致Spring的... 目录1. 什么是循环依赖?2. 循环依赖的场景案例3. 解决循环依赖的常见方法方法 1:使用 @La

使用C#代码在PDF文档中添加、删除和替换图片

《使用C#代码在PDF文档中添加、删除和替换图片》在当今数字化文档处理场景中,动态操作PDF文档中的图像已成为企业级应用开发的核心需求之一,本文将介绍如何在.NET平台使用C#代码在PDF文档中添加、... 目录引言用C#添加图片到PDF文档用C#删除PDF文档中的图片用C#替换PDF文档中的图片引言在当

C#使用SQLite进行大数据量高效处理的代码示例

《C#使用SQLite进行大数据量高效处理的代码示例》在软件开发中,高效处理大数据量是一个常见且具有挑战性的任务,SQLite因其零配置、嵌入式、跨平台的特性,成为许多开发者的首选数据库,本文将深入探... 目录前言准备工作数据实体核心技术批量插入:从乌龟到猎豹的蜕变分页查询:加载百万数据异步处理:拒绝界面

Java实现文件图片的预览和下载功能

《Java实现文件图片的预览和下载功能》这篇文章主要为大家详细介绍了如何使用Java实现文件图片的预览和下载功能,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... Java实现文件(图片)的预览和下载 @ApiOperation("访问文件") @GetMapping("

用js控制视频播放进度基本示例代码

《用js控制视频播放进度基本示例代码》写前端的时候,很多的时候是需要支持要网页视频播放的功能,下面这篇文章主要给大家介绍了关于用js控制视频播放进度的相关资料,文中通过代码介绍的非常详细,需要的朋友可... 目录前言html部分:JavaScript部分:注意:总结前言在javascript中控制视频播放

SpringKafka消息发布之KafkaTemplate与事务支持功能

《SpringKafka消息发布之KafkaTemplate与事务支持功能》通过本文介绍的基本用法、序列化选项、事务支持、错误处理和性能优化技术,开发者可以构建高效可靠的Kafka消息发布系统,事务支... 目录引言一、KafkaTemplate基础二、消息序列化三、事务支持机制四、错误处理与重试五、性能优

SpringIntegration消息路由之Router的条件路由与过滤功能

《SpringIntegration消息路由之Router的条件路由与过滤功能》本文详细介绍了Router的基础概念、条件路由实现、基于消息头的路由、动态路由与路由表、消息过滤与选择性路由以及错误处理... 目录引言一、Router基础概念二、条件路由实现三、基于消息头的路由四、动态路由与路由表五、消息过滤

Spring Boot 3.4.3 基于 Spring WebFlux 实现 SSE 功能(代码示例)

《SpringBoot3.4.3基于SpringWebFlux实现SSE功能(代码示例)》SpringBoot3.4.3结合SpringWebFlux实现SSE功能,为实时数据推送提供... 目录1. SSE 简介1.1 什么是 SSE?1.2 SSE 的优点1.3 适用场景2. Spring WebFlu

基于SpringBoot实现文件秒传功能

《基于SpringBoot实现文件秒传功能》在开发Web应用时,文件上传是一个常见需求,然而,当用户需要上传大文件或相同文件多次时,会造成带宽浪费和服务器存储冗余,此时可以使用文件秒传技术通过识别重复... 目录前言文件秒传原理代码实现1. 创建项目基础结构2. 创建上传存储代码3. 创建Result类4.