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、结果展示:
参考:使用位姿真值 · 语雀