把T265定点过程中的坐标变换彻底弄清楚
但是我看普罗米修斯的px4_pos_estimator.cpp的代码感觉里面没有做坐标变换
https://gitee.com/maxibooksiyi/Prometheus/blob/master/Modules/control/src/px4_pos_estimator.cpp
那我去板载计算机上打印看看 /t265/odom/sample 和 /mavros/vision_pose/pose 看看经过px4_pos_estimate之后有什么变化?
实际我看了下似乎真的不需要坐标变换,下面左边是/t265/odom/sample 右边是 /mavros/vision_pose/pose ,原始的odom信息似乎就已经符合东北天坐标系了!!!!!莫非是realsense2-camera已经考虑到这一点了?确实px4_pos_estimator.cpp里我看代码也没有做任何的坐标变换。
(下面这个图的数据是我再上电后把无人机向前移动了几厘米)
(下面的数据是我再上电后把无人机向前移动了几厘米,向左移动了一段距离,可以看到数据都是符合东北天坐标系的)
我顺带把/tf消息也打出来了,也是符合东北天坐标系的。所以不需要坐标系变换啊,难道那个APM教程或者vision_to_mavros说错了。你可以再去分析一下PX4给的VIO的代码,里面是不是也没有做任何坐标系变换。
那等于也就是实质做了个消息类型转变,px4_pos_estimator.cpp在T265定点中就起了这么一个作用。把nav_msgs::Odometry类型的消息转为geometry_msgs::PoseStamped类型的消息,并改个名字,好让MAVROS接收。
至少可以确定,T265向前(摄像头前方)是X变大,向左是Y变大,向上是Z变大,
那我去回看一下我之前做APMT265树莓派无人机的时候的录屏(之前的录屏似乎没有移动无人机,所以看不出来),看看那个时候打印的/tf消息和打印的/mavros/vison_pose/pose消息是不是就是一样的,你当然还可以进一步看一下vision_to_mavros的源码,看看它到底有没有做坐标系变换。
我看了下vision_to_mavros的代码似乎也是没有做坐标系变换的,似乎就是做了个消息类型的转变,如它的注释所说 Create PoseStamped message to be sent
https://github.com/thien94/vision_to_mavros/blob/master/src/vision_to_mavros.cpp
vision_to_mavros它是监听tf消息(所以没有写subscribe订阅,似乎/tf不是用来订阅的?),然后发布geometry_msgs::PoseStamped类型的vision_pose
vision_to_mavros中间这么一大坨都是用来打印终端消息的。
不过我好像在vision_to_mavros里面看到了有做坐标系转换。
普罗米修斯跑T265定点时的节点图
px4_command跑T265定点时的节点图
下面是跑px4_command时打印的消息,最左边是/tf (vision_to_mavros接收的) 中间/visionPose(vision_to_mavros发布的,也是px4_pos_extimate订阅的) 最右边是/mavros/vision_pose/pose(px4_pos_extimate发布的)
这里对比可以看到vision_to_mavros确实做了坐标系变换,但是px4_pos_extimate又将其变回去了。可能原本我的APM树莓派T265默认正方向朝北,这里默认正方向朝东,所以阿木的这个就实际不需要多T265输出的原始位置数据做坐标系变换,是不是这个道理。确实第二个/visionPose特就是经过vision_to_mavros变换过后的,就变成y是朝向机头的了,在东北天坐标系下就是朝北,这个就是符合我的APMT265树莓派无人机的,所以那个时候我每次起飞机头要朝向正北方向,这样才符合现实的磁场角度。因为当时我无人机飞控用的现实的磁场,如果对不上,飞机自身发送期望指令就会有问题,飞机实际偏向一个方向,但是无人机认为偏向另一个方向,再负反馈控制就会有问题,这个细节我之前分析过,后面有时间再写一遍。
之前有个弄错了,阿木应该是没有改vision_to_mavros的核心代码的,虽然他们代码行数少些好像删去的是一些打印终端消息的。
之前我说阿木改了vision_to_mavros是在这篇博文里面,我现在去改过来 https://blog.csdn.net/sinat_16643223/article/details/113139495
下面的位置是我把无人机向前移动几厘米
现在再看vision_to_mavros这里注释的第一行,说是变成y向前知道什么意思了吧。它发布的/visionPose就是这样!!!!!我的APMT265树莓派无人机就是这样。
对了找找vision_to_mavros是在哪里写的发布/mavros/vision_pose/pose的
再看下面的坐标系的图
对于机体是朝前,对于世界是朝北。也就是x是朝北的,现在看明白那个图了。
对于世界就是东北天坐标系。
机体的x和世界的y对应上
这就是那句话的意思。
之所以这么写,因为对于机体或者摄像头而言,是没有北的概念的,只有前,而对于世界坐标系而言,是东北天坐标系,是没有前的概念的,只有东北天。
划红线的地方的意思就是 机体的x和世界的y对应上
我现在非常彻底清除为什么上面的坐标系图那么画了!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!之前的东西都对得上了。
我之前的APMT265树莓派无人机默认机头也就是摄像头前方朝北,而阿木的无人机默认机头方向也就是摄像头方向朝东,这就是区别,所以为什么我需要用vision_to_mavros,而他们不需要做坐标系转换。
现在还有两个地方没弄懂,vision_to_mavros是在哪里发布的/mavros/vision_pose/pose消息,我只看到发布/visionPose消息。第二个是px4_command的px4_pos_extimate讲道理是对订阅的视觉位置数据做了坐标系变换了,为什么我没有看到呢。
又发布,我弄错了,只是阿木的怎么把消息变成/visionPose了呢,难道他们改了名字,我去看看板载计算机上的代码,是的,确实就是改了vision_to_mavros.cpp发布消息的名称。
看了下阿木的px4_command里面的vision_to_mavros确实修改了发布的消息名称,和原版的vision_to_mavros相比。
节点名称的改名和发布消息的改名原来是放在Launch文件里面改的。这下面是官方vision_to_mavros的,这也可以明白为什么看节点图看到的节点名称是t265_to_mavros而不是vision_to_mavros。也可以明白为什么代码里写发布消息只写了/vision_pose,而不是/mavros/vision_pose/pose了。阿木应该是在launch文件里面把他们vision_to_mavros节点发布的消息最后改为/visionPose,这样再看上面px4_command的T265定点的节点图就清楚了。这么来讲阿木对vision_to_mavros还是有些改动的,只是坐标系变换部分没有改动。(launch文件里面可以改发布的话题消息名称,但是后来发现阿木是在vision_to_mavros.cpp里面改的,由于P200的TX2上有两个vision_to_mavros.cpp,我之前找错了。)
当然上面的分析可以让我们很好理解,我之前做APMT265树莓派的时候,为什么vision_to_mavros发布出来的消息是/mavros/vision_pose/pose,而代码里面写的发布消息的名称却是/vision_pose,因为在lunach文件中修改了。这是不是侧面可以体现出,消息的名称是可以随便改的,消息的本质类型就是那样,名字可以随便取,所以普罗米修斯里面就把/camera改为/t265了,是这样吧。
后来看了下,原来这种改名就是所说的重映射!!!!之前看到重映射以为是什么很高级的东西,现在才明白。<remap>
而且可能是ros里面的基础操作,普罗米修斯把/camera改为/t265也是基础操作,应该为了防止重名,并不是什么为了防止别人拿去能直接用。。。。
px4_command中的px4_pos_estimate.cpp里面对视觉位置数据做坐标系变换的地方找到了,虽然上面有个备注时上面vicon,但是实际那些变量pos_drone_vio就是来自于视觉位置数据的回调函数里面的!!!!!!那么现在OK了。我知道px4_command在T265定点的时候怎么做坐标系变换的了。再看上面那三个话题消息的打印,都可以解释清楚了。对vision_to_mavros的坐标转换也清楚了,也清楚为什么普罗米修斯在T265定点的时候不需要做坐标转换了。是的,都清楚了。
https://github.com/amov-lab/px4_command/blob/master/src/px4_pos_estimator.cpp
目前就剩最后一个问题 在px4_command定点的时候,阿木在哪里把vision_to_mavros发布的话题消息/vision_xyz改为/visionPose的,应该是在t265_tf_to_mavros.launch这个launch文件里面,但是我目前还没有找到明确写了改了这个的。
这个问题我找到答案了,他们就是在vision_to_mavros.cpp里面改的话题消息名称,只是我之前找到另一个vision_to_mavros.cpp去了,他们实际用的这个vision_to_mavros.cpp,所以launch文件里面的改不改无所谓。那么现在最后一个问题也彻底解决了!!!!!!!!!!!!!!!!!
现在可能能真正自己写个T265定点了,就做个消息类型转换。
昨天本身对消息也有了进一步认识。
就像肖昆自己写个transfer.py确实不难。
关于阿木为什么默认机头朝东好好想想,可能东北天,x就是东,这样方便发布指点坐标,好看些。要是机头朝北确实也不方便。
或者px4 有没有规定?应该没有。
我发现vision_to_mavros这一段话你还没有真正理解,根本可能是对TF没有理解。(结合这篇博文来看 https://blog.csdn.net/sinat_16643223/article/details/113889533 )
结合它下面的解释可能就好些,就是从相机坐标系到世界坐标系的变换,你就可以理解为相机相对于世界的位姿。就像下面cartographer也是发布的/tf,意思就是机器人相对地图的位姿,不就是位置么。感觉就是一个意思,换种表达方式。
https://blog.csdn.net/xiekaikaibing/article/details/80118134
而且我还看到local_position/pose还可以以TF的类型发出去
t265节点为什么要发布TF消息?
还有我发现普罗米修斯里面px4_pos_eatimate阅激光雷达的也是订阅的/tf,这里面应该有一定道理。可能都是用/tf来表示的
https://gitee.com/maxibooksiyi/Prometheus/blob/master/Modules/control/src/px4_pos_estimator.cpp
https://blog.csdn.net/qq_38649880/article/details/88372390
https://blog.csdn.net/xiekaikaibing/article/details/80118134