XTDrone-使用位姿真值-配置教程

Gazebo提供了无人机位姿姿态的真实值，作用类似于实物系统中的动作捕捉系统。位姿真值主要有两个作用，一是对于集群编队等任务，提供了可靠准确的全局定位；二是用于分析SLAM算法的精度。下面简述使用方法。

1、利用roslaunch命令启动一架无人机（以iris为例）

roslaunch px4 outdoor3.launch

注意：启动之前需要修改outdoor文件中的几个关键部分，确保是iris无人机，具体修改见下图：

文件目录：/home/ray/PX4_Firmware/launch

2、启动通信脚本，该脚本会中转Gazebo的真值位姿话题

cd  ~/XTDrone/communication
python3 multirotor_communication.py iris 0

3、启动获取位姿真值的脚本，脚本命令行参数的“1”表示1架无人机

cd ~/XTDrone/sensing/pose_ground_truth/
python3 get_local_pose.py iris 1 

说明：已修改了get_local_pose.py脚本，增加了在控制台输出所获取车辆的位资信息。

修改后的代码如下所示：

import rospy
from geometry_msgs.msg import PoseStamped, Vector3Stamped
import sys
from gazebo_msgs.msg import ModelStates

# 从命令行参数获取车辆类型和数量
vehicle_type = sys.argv[1]
vehicle_num = int(sys.argv[2])

# 初始化发布器数组，用于发布姿态和速度信息
multi_pose_pub = [None]*vehicle_num
multi_speed_pub = [None]*vehicle_num

# 初始化多个姿态和速度消息
multi_local_pose = [PoseStamped() for i in range(vehicle_num)]
multi_speed = [Vector3Stamped() for i in range(vehicle_num)]

# 回调函数，处理模拟器状态消息
def gazebo_model_state_callback(msg):
    for vehicle_id in range(vehicle_num):
        # 获取指定车辆的索引
        id = msg.name.index(vehicle_type+'_'+str(vehicle_id))
        
        # 填充姿态消息
        multi_local_pose[vehicle_id].header.stamp = rospy.Time().now()
        multi_local_pose[vehicle_id].header.frame_id = 'map'
        multi_local_pose[vehicle_id].pose = msg.pose[id]
        
        # 填充速度消息
        multi_speed[vehicle_id].header.stamp = rospy.Time().now()
        multi_speed[vehicle_id].header.frame_id = 'map'
        multi_speed[vehicle_id].vector = msg.twist[id]

if __name__ == '__main__':
    rospy.init_node(vehicle_type+'_get_pose_groundtruth')
    
    # 订阅模拟器状态消息
    gazebo_model_state_sub = rospy.Subscriber("/gazebo/model_states", ModelStates, gazebo_model_state_callback, queue_size=1)
    
    # 创建多个发布器，用于发布每个车辆的姿态和速度信息
    for i in range(vehicle_num):
        multi_pose_pub[i] = rospy.Publisher(vehicle_type+'_'+str(i)+'/mavros/vision_pose/pose', PoseStamped, queue_size=1)
        multi_speed_pub[i] = rospy.Publisher(vehicle_type+'_'+str(i)+'/mavros/vision_speed/speed', Vector3Stamped, queue_size=1)
        print("获取 " + vehicle_type + "_" + str(i) + " 的真实姿态信息")
    
    rate = rospy.Rate(30)  # 设置发布频率为30Hz
    while not rospy.is_shutdown():
        for i in range(vehicle_num):
            # 发布姿态和速度信息
            multi_pose_pub[i].publish(multi_local_pose[i])
            multi_speed_pub[i].publish(multi_speed[i])
            
            # 打印姿态和速度信息
            print(f"Vehicle {vehicle_type}_{i} Pose: {multi_local_pose[i].pose}")
            print(f"Vehicle {vehicle_type}_{i} Speed: {multi_speed[i].vector}")
        
        try:
            rate.sleep()  # 维持发布频率
        except rospy.ROSInterruptException:
            continue  # 处理 ROS 中断异常

4、结果展示：

参考：使用位姿真值 · 语雀