px4仿真使用fastlio的定位数据飞行

前提是已经配置好 px4 的仿真以及 fastlio 的环境，可以用 XTDrone 的教程配置，比较快一些。我是按照 XTDrone 的教程配置好了 livox_avia 的环境。可以参考我之前的博文：https://blog.csdn.net/a_xiaoning/article/details/148305871?spm=1001.2014.3001.5501

px4的模型与参数配置

模型修改

为了方便验证 px4 是否是接入了 fastlio 的数据才有定位能够起飞，因此找到对应的飞机模型文件，将 gps 的部分给屏蔽掉：

连接QGC

使用 XTDrone 提供的通信脚本也可以，但不如直接连接 QGC 方便，连接 QGC 后在本文中有几个我们需要做的：

1）修改EKF2配置参数，采用视觉定位数据(只是它起这个名字，用雷达也是一样的)

2）观察是否能切换到 Position 模式，能切到就说明有定位

3）看是否能解锁，起飞飞一段距离看会不会有异常

我使用的是 wsl ，虚拟机应该是差不多的，但我 QGC 是在 windows 中，因此不能直接使用本机 ip 来连接 udp 端口，需要使用 ifconfig 指令来获取一下 wsl 的 ip：

开启 px4 的 gazebo 脚本(这里我是自己改的名字，需要按自己的launch文件)：

roslaunch px4 outdoor_fastlio.launch

打开 QGC ，配置 udp 的链接方式：

之后就可以连接上仿真中的 px4 了，这时候由于关闭了 gps 模块，因此是切换不到 Position 以及 offboard 模式的：

可以自己试一下，切不到才是正常的。查看 mavlink 消息中，也没有 local_position 之类的本地坐标消息。

修改EKF2参数

修改 EKF2_AID_MASK 参数，选择 vision position fusion：

如果你是 XTDrone 的脚本，就需要收到修改 px4 的启动脚本，在启动脚本 rcS 文件中，添加此参数的修改：

此时正常修改完后，是要软重启的，但仿真没办法软重启，就关掉再重启一下 px4 仿真。

运行fastlio

编写转发脚本

在修改完上述步骤后，运行 fastlio，我们可以获取到 fastlio 的定位数据，但此时我们需要一个脚本来将 fastlio 的定位数据通过 mavros 来发送给 px4，但目前还有一个问题，就是 fastlio 的航向需要与 px4 的航向进行对齐，因为 fastlio 是 imu 与激光数据，并没有标准的航向数据，因此需要与飞机进行对齐，这个脚本网上也有许多人编写，需要注意的一点是：仿真中，我这边是没办法收到 px4 的 local position 数据的，也就是没办法用这个主题中的航向来进行对齐，实机中应该是有的，这一点是仿真不同的地方，因此我这里是获取了 imu 的主题数据，用 imu 的航向来进行对齐，之后将 fastlio 的定位数据旋转后，发送给 px4，以下是我的脚本，仅供参考：

# *******
#   监听 fastlio 的位置主题并发布给px4
# *******
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import Imu
import tf
import numpy as np
class FaslioToMavros:def __init__(self):# 初始化slam位姿与px4位姿self.q_slam = [0, 0, 0, 1]self.q_px4 = [0, 0, 0, 1]self.pose_body = np.zeros(3)self.yaw_px4 = 0   # 需要将px4的航向记录下来，用以对齐fastlio的航向self.yaw_px4_init = 0   # 存储上电时的px4初始航向self.q_px4_init = [0, 0, 0, 1]  # 存储px4初始位姿self.first_get_px4_yaw = False  # 记录是否接收到 px4 的航向信息# 初始化ROS节点rospy.init_node('fastlio_odom_republisher', anonymous=True)# 订阅 fastlio 的里程计主题rospy.Subscriber('/Odometry', Odometry, self.fastlio_odom_callback)# 订阅 px4 的里程计数据# rospy.Subscriber('iris_0/mavros/local_position/odom', Odometry, self.px4_odom_callback)# 订阅 px4 的里程计数据  发现在仿真中，没有gps时，px4不发送里程计数据出来，因此修改为 imu 数据rospy.Subscriber('iris_0/mavros/imu/data', Imu, self.px4_imu_callback)# 发布位姿数据给 px4 (需要将slam的里程计数据转化为mavros的主题)self.fastlio_pub = rospy.Publisher('iris_0/mavros/vision_pose/pose', PoseStamped, queue_size=10)# 设置发布频率为10Hzself.rate = rospy.Rate(10)self.run()# fastlio 里程计回调def fastlio_odom_callback(self, msg):self.q_slam = [msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w]self.pose_body = np.array([msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z])# print(f'get fastlio data')# px4 里程计回调def px4_odom_callback(self, msg):self.q_px4 = [msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w]euler = tf.transformations.euler_from_quaternion(self.q_px4)self.yaw_px4 = euler[2]   # 存储航向角if not self.first_get_px4_yaw:self.yaw_px4_init = self.yaw_px4self.q_px4_init = tf.transformations.quaternion_from_euler(0, 0, self.yaw_px4_init)self.first_get_px4_yaw = True  # 接收到px4航向角print(f'get px4 yaw')# px4 imu 回调def px4_imu_callback(self, msg):# 从IMU消息中获取四元数orientation_q = msg.orientationorientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]# 四元数转欧拉角euler = tf.transformations.euler_from_quaternion(orientation_list)self.yaw_px4 = euler[2]   # 存储航向角if not self.first_get_px4_yaw:self.yaw_px4_init = self.yaw_px4self.q_px4_init = tf.transformations.quaternion_from_euler(0, 0, self.yaw_px4_init)self.first_get_px4_yaw = True  # 接收到px4航向角print(f'get px4 yaw , yaw = {self.yaw_px4_init * 57.3}')# 主循环def run(self):while not rospy.is_shutdown():if self.first_get_px4_yaw:# 计算航向偏移，将旋转应用在雷达数据中rot_px4_to_slam = tf.transformations.quaternion_matrix(self.q_px4_init)[:3, :3]pose_adjust_enu = np.dot(rot_px4_to_slam, self.pose_body)  # 计算校正后的位置  由于是fastlio的消息，还是enu坐标系# 构建并发布slam位置信息fastlio_msg = PoseStamped()fastlio_msg.header.stamp = rospy.Time.now()fastlio_msg.header.frame_id = "map"  # 根据实际情况设置fastlio_msg.pose.position.x = pose_adjust_enu[0]fastlio_msg.pose.position.y = pose_adjust_enu[1]fastlio_msg.pose.position.z = pose_adjust_enu[2]fastlio_msg.pose.orientation.x = self.q_slam[0]fastlio_msg.pose.orientation.y = self.q_slam[1]fastlio_msg.pose.orientation.z = self.q_slam[2]fastlio_msg.pose.orientation.w = self.q_slam[3]self.fastlio_pub.publish(fastlio_msg)self.rate.sleep()
if __name__ == '__main__':try:# 创建并运行节点republisher = FaslioToMavros()except rospy.ROSInterruptException:# 处理ROS中断异常rospy.loginfo("Node interrupted")

验证定位数据

转发成功后，可以看到 QGC 中，飞机已经可以切换 position 模式：

并且在 mavlink 消息中可以看到有本地定位数据：

现在就可以解锁起飞了，使用虚拟摇杆控制飞机，可以在 rviz 中观察轨迹数据：