给无人机提供位姿信息 get_local_pose.py

PoseStamped适用于描述机器人的位姿信息，包括位置和方向，通常被用于机器人的导航和控制中。而Vector3Stamped适用于描述机器人的速度和加速度信息，通常被用于机器人的运动控制和姿态估计中。

在ROS中，ModelStates消息类型用于描述Gazebo仿真环境中所有模型的状态信息，包括位置、姿态、速度等信息。它是一个由多个ModelState消息组成的消息数组，每个ModelState消息表示一个模型的状态信息。

ModelStates消息类型通常用于获取Gazebo仿真环境中多个模型的状态信息，并将其传递给其他节点进行处理。例如，在机器人控制中，可以使用ModelStates消息获取无人机在Gazebo仿真环境中的位置和速度信息，并将其发布到ROS话题中，以供其他节点订阅使用。

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)]

这段代码首先从命令行参数中获取无人机类型和数量，并创建一个长度为vehicle_num的列表multi_pose_pub和multi_speed_pub，用于存储无人机位置和速度控制的发布者。然后，创建两个长度为vehicle_num的列表multi_local_pose和multi_speed，分别用于存储无人机的位置和速度信息。

其中，multi_local_pose和multi_speed分别是长度为vehicle_num的PoseStamped和Vector3Stamped对象列表，用于存储多个无人机的位置和速度信息。multi_pose_pub和multi_speed_pub分别是长度为vehicle_num的ROS话题发布者列表，用于发布多个无人机的位置和速度控制信息。

multi_local_pose[vehicle_id].header.stamp = rospy.Time().now()表示将第vehicle_id个无人机的位置信息的时间戳设置为当前的ROS时间，以表示该位置信息的时间戳是当前时间。

multi_local_pose[vehicle_id].header.frame_id = 'map'表示将第vehicle_id个无人机位置信息的坐标系设置为map，即将无人机的位置信息以map坐标系为参考系进行描述。这里的map是指地图坐标系，它是一个惯性坐标系，通常用于描述无人机的全局位置和姿态。

multi_local_pose[vehicle_id].pose = msg.pose[id]表示将第vehicle_id个无人机的位置信息设置为ModelStates消息中对应的位置信息。

ModelStates是gazebo_msgs中的一个消息类型，用于描述模型的状态信息，包括模型的位置、姿态、线速度和角速度等信息

在主函数中，首先通过rospy.init_node(vehicle_type+'_get_pose_groundtruth')初始化ROS节点，并创建一个gazebo_model_state_sub话题订阅者，用于接收/gazebo/model_states话题中的消息，并将其传递给gazebo_model_state_callback回调函数进行处理。

然后，使用一个循环不断发布无人机的位置和速度信息，并将其发布到对应的ROS话题中，以供其他节点订阅使用。具体来说，使用rospy.Publisher函数创建多个ROS话题发布者multi_pose_pub和multi_speed_pub，用于发布无人机的位置和速度信息。然后，在循环中，使用multi_pose_pub[i].publish(multi_local_pose[i])和multi_speed_pub[i].publish(multi_speed[i])分别将第i个无人机的位置和速度信息发布到对应的ROS话题中。

最后，使用rate = rospy.Rate(30)创建一个频率为30Hz的循环，以控制发布信息的频率，并使用rate.sleep()使节点按照设定的频率循环运行。rospy.is_shutdown()函数检查ROS节点是否已关闭，如果节点已关闭，则退出循环。