获取Parrot-Sphinx仿真环境中Bebop 2的飞行信息

本篇博客介绍了在使用Parrot-Sphinx进行Bebop 2的飞行仿真时，如何实时读取飞行信息。关于Parrot-Sphinx的安装和初步使用，参考：ROS + Parrot-Sphinx进行Bebop无人机仿真。

测试环境：

• Ubuntu 18.04
• ROS Melodic

使用步骤:

1. 启动firmwared服务
   打开一个终端，输入：

   $ sudo systemctl start firmwared.service
   

   并检查是否启动成功：

   $ fdc ping
   

   若启动成功，将会得到PONG的响应。

2. 运行Sphinx
   打开一个终端，运行sphinx，如：

   $ cd ~/ROS/hai_ws/src/bebop2_toolbox/bebop2_sphinx/
   $ ls
   $ sphinx --datalog ./worlds/empty.world ./drones/bebop2_eth0.drone
   

   注意：为了使sphinx在运行时提供实时的数据记录，需要添加–datalog的选项，即：

   $ sphinx --datalog my.world my.drone
   

   上面执行的命令中，.world文件用的是sphinx默认的一个empty.world，而.drone文件进行了一些修改：

   <?xml version="1.0" encoding="UTF-8"?>
   <drone
     name="Bebop1"
     firmware="/home/hai/ROS/hai_ws/src/bebop2_toolbox/bebop2_sphinx/firmware/ardrone3-milos_pc.ext2_latest.zip"
     hardware="milosboard">
     <machine_params
       low_gpu="false"
       with_front_cam="false"
       with_hd_battery="false"
       with_flir="false"
       flir_pos="tilted"/>
     <pose>1 0 0.2 0 0 0</pose>
     <interface>eth1</interface>
     <!-- 'wlan0' may need to be replaced the actual wifi interface name -->
     <stolen_interface>eth0:eth0:192.168.42.1/24</stolen_interface>
   </drone>
   

   注意其中的name定义了运行该文件后在gazebo中生成的无人机的名称。

3. 查看无人机的飞行数据
   打开一个终端，查看可见的ros topic：

   $ rostopic list 
   /rosout
   /rosout_agg
   

   发现没有无人机飞行相关的信息。在终端中继续运行：

   $ parrot-gz topic -l
   

   可以看到一个话题列表（以下仅含部分）：

   $ parrot-gz topic -l
   /gazebo/default/Bebop1/body/ultrasound/scan
   /gazebo/default/Bebop1/body/vertical_camera/image
   /gazebo/default/Bebop1/body/wrench
   /gazebo/default/atmosphere
   /gazebo/default/model/info
   /gazebo/default/model/modify
   

   其中/gazebo/default/pose/info话题包含了仿真器中歌模型的实时位姿信息，可以对其进行查看：

   $ parrot-gz topic -e /gazebo/default/pose/info
   

   获得实时信息：

   time {
     sec: 566
     nsec: 820000000
   }
   pose {
     name: "Bebop1"
     id: 10
     position {
       x: 0.99899889677977471
       y: -4.2870200141800114e-05
       z: 0.0048740620753592936
     }
     orientation {
       x: 1.3384541576419549e-06
       y: -5.7508756664778854e-05
       z: 0.017040173236204895
       w: 0.9998548040525842
     }
   }
   pose {
     name: "Bebop1::body"
     id: 11
     position {
       x: 0
       y: 0
       z: 0.045
     }
     orientation {
       x: 0
       y: 0
       z: 0
       w: 1
     }
   }
   

4. 查看更多的话题信息
   在终端中输入：

   $ parrot-gz topic -h
   

   可以获取一个简短的帮助文件信息，通过设置不同的选项对相关话题的信息进行查看，如可在终端中输入$ parrot-gz topic -z /gazebo/default/pose/info查看上述位姿信息的发送频率，约为60Hz左右。在终端中输入$ parrot-gz topic -i /gazebo/default/pose/info可以得到该话题的类型等信息：

   Type: gazebo.msgs.PosesStamped
   Publishers:
   	127.0.0.1:33917
   Subscribers:
   	127.0.0.1:46613
   

5. 为了方便的通过ros topic来获取需要记录的飞行信息，可以将该/gazebo/default/pose/info信息转换为ros topic信息后再进行发出。主要代码如下：

   my_logger.pose_gazebo_sub = node->Subscribe("/gazebo/default/pose/info", &data_logger::read_pose, &my_logger);
   
   geometry_msgs::PoseStamped pose_temp;
   
   // Reading Follower pose from Gazebo
   void data_logger::read_pose(ConstPosesStampedPtr &msg)
   {
   	pose_temp.header.frame_id = "world";
   	// pose_temp.header.seq = 0;
   	const ::gazebo::msgs::Time &time_now = msg->time();
   	pose_temp.header.stamp.sec = time_now.sec();
   	pose_temp.header.stamp.nsec = time_now.nsec();
   
   	for (int i =0; i < msg->pose_size(); ++i)
       	{
   		const ::gazebo::msgs::Pose &pose_now = msg->pose(i);
   		if (pose_now.name() == "Bebop1")
   		{
   			const ::gazebo::msgs::Vector3d &position = pose_now.position();
   			const ::gazebo::msgs::Quaternion q = pose_now.orientation();
   
   			pose_temp.pose.position.x = position.x();
   			pose_temp.pose.position.y = position.y();
   			pose_temp.pose.position.z = position.z();
   			pose_temp.pose.orientation.w = q.w();
   			pose_temp.pose.orientation.x = q.x();
   			pose_temp.pose.orientation.y = q.y();
   			pose_temp.pose.orientation.z = q.z();
   
   			this->pose_pub.publish(pose_temp);
   		}
   	}
   }
   

注意！！！

1. 使用parrot-gz命令查看仿真信息时，实际上使用的是gazebo-7命令。而在ROS Melodic中默认的版本是gazebo-9，因此上述第五步的转换中将会出现因为版本冲突造成的错误。

参考资料：https://forum.developer.parrot.com/t/how-to-get-drone-status-and-parameters/8697/10