九、键盘控制无人机 · 中（multirotor_communication.py解读）

笔记来源于开源项目：基于PX4和ROS的无人机仿真平台

来源于开源项目：GAAS

---

目录

一、解读启动通信的multirotor_communication.py脚本

1、库函数以及消息服务

2、class Communication类中的init之构造方法

3、上面订阅话题中和mavros通讯相应的回调函数（3个）

4、上面订阅话题中和cmd外部程序相应的回调函数（7个）

cmd_callbacck回调函数：

其他回调函数：

5、class Communication类中的start之构造方法

6、main函数

二、打开程序，查看节点，话题，服务

---

 

一、解读启动通信的multirotor_communication.py脚本

代码有点长，我们一步一步分析：

1、库函数以及消息服务

import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from geometry_msgs.msg import PoseStamped, Pose, Twist
from gazebo_msgs.srv import GetModelState
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import String
import time
from pyquaternion import Quaternion
import math
from multiprocessing import Process
import sys

• rospy是ros接口api的库
• tf这个库资料中也没有太多介绍，在这个文件里面可以没有特别的用到（我不是很清楚），是不是和求解坐标的时候位置变换有一定的关系，就是对于其他的相关程序而言
• yaml这个是Python一个捣鼓.yaml配置文件的一个库，但是我还是没有找到ta在这个程序里面的用到的地方
• time、math、sys这几个就不说了啦
• pyquaternion是Python里面计算四元数的一个库，你可以把ta理解为像numpy一样的计算工具。这里面的Quaternion，相关的用法是Quaternion(w, x, y, z)，参数可以是实数也可以是字符串，通过指定4个实数标量元素来创建四元数
• multiprocessing是Python的标准模块，它既可以用来编写多进程，也可以用来编写多线程。这里面的Process是用来创建进程的模块
• from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
  from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
  from geometry_msgs.msg import PoseStamped, Pose, Twist
  from gazebo_msgs.srv import GetModelState
  from nav_msgs.msg import Odometry
  from sensor_msgs.msg import Imu, NavSatFix
  from std_msgs.msg import String
• 这一共是好几个个msg文件，4个srv文件，我们先放在这里，都是后面制造话题和服务用的

2、class Communication类中的init之构造方法

class Communication:
    def __init__(self, vehicle_type, vehicle_id):
        self.vehicle_type = vehicle_type
        self.vehicle_id = vehicle_id
        self.imu = None
        self.local_pose = None
        self.current_state = None
        self.current_heading = None 
        self.hover_flag = 0
        self.target_motion = PositionTarget()
        self.global_target = None
        self.arm_state = False
        self.offboard_state = False
        self.motion_type = 0
        self.flight_mode = None
        self.mission = None
        self.transition_state = None
        self.transition = None   
        '''
        ros subscribers
        '''
        self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
        self.mavros_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/state", State, self.mavros_state_callback)
        self.imu_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)

        self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback)
        self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
        self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
        self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
        self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
        self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback)
        self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback) 
        ''' 
        ros publishers
        '''
        self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
        self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+'_'+self.vehicle_id+'/ground_truth/odom', Odometry, queue_size=10)
        '''
        ros services
        '''
        self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
        self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode)
        self.gazeboModelstate = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)

        print(self.vehicle_type+'_'+self.vehicle_id+": "+"communication initialized")

• 定义成员变量。注意到没有，当时我们执行这个Python文件的时候，会带上两个参数，而这两个参数就是 def __init__(self, vehicle_type, vehicle_id) 中的 vehicle_type 、 vehicle_id 指的就是机型以及它的id编号。然后其他的成员变量会在下面的函数中连接到gazebo之后才会给他们定义，现在只是给他们一个初始值
• ros subscribers：创建Subscriber。和mavros通讯相关的话题，和飞机的位姿、状态、imu数据有关，都订阅名为“飞机的机型+飞机的编号+/mavros/+....+对应的话题”的topic，然后注册相应的回调函数。和外部控制命令相关的话题，和控制飞机命令的名称、速度、坐标、加速度有关，都订阅名为“/xtdrone/+飞机的机型+飞机的编号+/cmd....+对应的话题”的topic，然后注册相应的回调函数。
• 注意：这里面的flu代表车身坐标系—前-左-天坐标（自我为中心：The Vehicle Frame —Front-Left-Up，FLU）；enu代表局部坐标系—东-北-天坐标（看地球的东西南北：The Local Frame – East-North-Up，ENU）
• ros publishers：创建Publisher。发布和mavros通信相关的期望点的话题，发布和真实测绘程外部命令控制的话题
• ros services：创建Server，请求被服务。请求名为“飞机的机型+飞机的编号+/mavros/+....+对应的服务”的server，得到提供和是否解锁、飞机的设置模式的服务；创建'gazebo/get_model_state'的服务，查看仿真里面模型的状态
• 到此为止，所有的topic以及server都已经被创建，所有的回调函数都在下面的程序中，一共有10个回调函数。还有两个发布和三个被服务
• 最后，当初始化完毕后，输出：机型+飞机编号+communication initialized。表示初始化完毕了。

3、上面订阅话题中和mavros通讯相应的回调函数（3个）

    def local_pose_callback(self, msg):
        self.local_pose = msg

    def mavros_state_callback(self, msg):
        self.mavros_state = msg.mode

    def imu_callback(self, msg):
        self.current_heading = self.q2yaw(msg.orientation)

这里面还有用到一个q2yaw的函数，这个函数也只在这里用了一次，代码如下：

    def q2yaw(self, q):
        if isinstance(q, Quaternion):
            rotate_z_rad = q.yaw_pitch_roll[0]
        else:
            q_ = Quaternion(q.w, q.x, q.y, q.z)
            rotate_z_rad = q_.yaw_pitch_roll[0]

        return rotate_z_rad

• 我们先来解释一下这个q2yaw函数，我们可以看到def imu_callback(self, msg):self.current_heading = self.q2yaw(msg.orientation)这里面出现了这个函数，而且里面还有一个参数msg.orientation。另外这个msg在上面的代码中指的是sensor_msgs.msg文件中的Imu这个定义的消息，打开ros wiki找到这个传感器的msg文件打开：
• 单词解释：orientation指的是方位，current_heading指的是当前的头方向

Header header

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z 

• 而这里面的orientation就是我们这里用到的，直面翻译是翻滚轴的xyz方向，然后我们结合这个再来看一看q2yaw函数：
• if：isinstance这个是Python的一个内置函数，判断类型（和type差不多，但是可以承接父类，要是类型一样就返回True，反之False）。这里的意思就是要是q（msg.orientation）和Quaternion（import上面讲过的四元数还记不记得）的类型一样的话，rotate_z_rad = q.yaw_pitch_roll[0]：z轴旋转的弧度就是这个四元数里面偏航倾斜翻滚的第一个位置上的数
• else：如果这个q（msg.orientation）和Quaternion的类型不一样的话，那么先把这个q里面的四元数先变成Quaternion的类型，处理过之后，再输出z轴旋转的弧度就是这个处理过之后的四元数里面偏航倾斜翻滚的第一个位置上的数
• 好的，我们已经了解完这个函数了，是不是还是没有明白到晒意思，查了一下百度发现好复杂，就先不了解下去了
• 综合意思就是通过这些回调函数读取的msg的消息，然后对之前的成员变量进行定义，值就是读取msg的值

4、上面订阅话题中和cmd外部程序相应的回调函数（7个）

    def cmd_pose_flu_callback(self, msg):
        self.coordinate_frame = 9
        self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
 
    def cmd_pose_enu_callback(self, msg):
        self.coordinate_frame = 1
        self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
        
    def cmd_vel_flu_callback(self, msg):
        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
        if self.hover_flag == 0:
            self.coordinate_frame = 8
            self.motion_type = 1     
            self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)       
 
    def cmd_vel_enu_callback(self, msg):
        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
        if self.hover_flag == 0:
            self.coordinate_frame = 1
            self.motion_type = 1
            self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)

    def cmd_accel_flu_callback(self, msg):
        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
        if self.hover_flag == 0:
            self.coordinate_frame = 8
            self.motion_type = 2
            self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
            
    def cmd_accel_enu_callback(self, msg):
        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
        if self.hover_flag == 0:
            self.coordinate_frame = 1 
            self.motion_type = 2
            self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
    def cmd_callback(self, msg):
        if msg.data == '':
            return

        elif msg.data == 'ARM':
            self.arm_state =self.arm()
            print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state))

        elif msg.data == 'DISARM':
            self.arm_state = not self.disarm()
            print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state))

        elif msg.data[:-1] == "mission" and not msg.data == self.mission:
            self.mission = msg.data
            print(self.vehicle_type+'_'+self.vehicle_id+": "+msg.data)

        elif not msg.data == self.flight_mode:
            self.flight_mode = msg.data
            self.flight_mode_switch()

cmd_callbacck回调函数：

订阅的命令的发布指令string（这是一个字符串），因此这个回调函数是针对这个命令字符串的：

• if：如果这个msg.data的字符串是为空，那就啥也不干
• elif msg.data == 'ARM':如果这个命令是“ARM”，那么：arm_state的状态就是就是arm（）这个函数的输出（这个函数的输出是True 或者False，如果解锁的话就输出True），然后在终端输出：飞机机型+飞机编号+Armed+arm_state的状态。但是，这个arm（）函数到底输出什么呢，我们来看看这个函数：

    def arm(self):
        if self.armService(True):
            return True
        else:
            print(self.vehicle_type+'_'+self.vehicle_id+": arming failed!")
            return False

可见，这里面调用了服务armServer，那我们再来看一看这个armServer：

        self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool)

这个是向这个“机型+编号+/mavros/cmd/arming”请求服务，这个CommandBool是mavros_msgs.srv中的一个服务。

也就是说，我现在在终端外部函数控制的指令是ARM的话，那么我就去找arm（）这个函数返回的值，如果返回的是True，那么就解锁了。但是要找这个arm（）函数的返回值，我就要去请求armServer服务，看ta返回的是什么，如果返回的是True，那么我这个arm（）函数就返回True，反之False，而且输出解锁失败。

• elif msg.data == 'DISARM':这个和上面的‘ARM’其实差不多

    def disarm(self):
        if self.armService(False):
            return True
        else:
            print(self.vehicle_type+'_'+self.vehicle_id+": disarming failed!")
            return False

• elif msg.data[:-1] == "mission" and not msg.data == self.mission:如果外部程序发过来的命令的是 “mission”，而且之前不是这个模式的话，那么就把mission这个成员变量定义为msg.data，设置为mission模式，同样输出相关的文字到终端去
• elif not msg.data == self.flight_mode:如果发过来的这个指令不是flight_mode原来的那个模式的话（这个flight_mode也是成员变量，初始值是None），那么酒吧模式变成发过来指令的那个模式，然后通过函数flight_mode_switch（）进行模式的转变，这个函数如下所示：

    def flight_mode_switch(self):
        if self.flight_mode == 'HOVER':
            self.hover_flag = 1
            self.hover()
            print(self.vehicle_type+'_'+self.vehicle_id+":"+self.flight_mode)
        elif self.flightModeService(custom_mode=self.flight_mode):
            print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode)
            return True
        else:
            print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode+"failed")
            return False

如果发过来的指令是 ‘HOVER’，那么悬停，然后判断标示hover_flag就设置为1，然后通过hover（）函数进行hover模式，hover（）模式如下：

    def hover(self):
        self.coordinate_frame = 1hover
        self.motion_type = 0
        self.target_motion = self.construct_target(x=self.local_pose.pose.position.x,y=self.local_pose.pose.position.y,z=self.local_pose.pose.position.z)

这里涉及到了函数construct_target（），我们先放着，反正我们知道这个函数的输出的期望点就是现在的这个点，这样才可以盘旋。同时，成员变量也相应的改变设置。

如果我们收到的指令不是 ‘HOVER’，那么我们就去请求服务 flightModeService(custom_mode=self.flight_mode) ，这个服务是这样的：

        self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode)

这个SetMode也是mavros_msgs.srv中的一个服务，去请求这个服务之后，得到的结果，然后按照这个得到结果设置模式，同时返回True，那么flight_mode成员变量就变成了这个模式。如果出错的话就会说，哎呀，我的模式设置失败了。

好的这个cmd_callback（self，msg）回调函数解释完毕。

其他回调函数：

这里面都涉及到两个函数：hover_state_transition以及construct_target

• 我们先来了解一下函数construct_target：

    def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0):
        target_raw_pose = PositionTarget()
        target_raw_pose.coordinate_frame = self.coordinate_frame

        if self.coordinate_frame == 1:
            target_raw_pose.position.x = x
            target_raw_pose.position.y = y
            target_raw_pose.position.z = z

            target_raw_pose.velocity.x = vx
            target_raw_pose.velocity.y = vy
            target_raw_pose.velocity.z = vz
            
            target_raw_pose.acceleration_or_force.x = afx
            target_raw_pose.acceleration_or_force.y = afy
            target_raw_pose.acceleration_or_force.z = afz
            
        else:
            target_raw_pose.position.x = -y
            target_raw_pose.position.y = x
            target_raw_pose.position.z = z

            target_raw_pose.velocity.x = -vy
            target_raw_pose.velocity.y = vx
            target_raw_pose.velocity.z = vz
            
            target_raw_pose.acceleration_or_force.x = afx
            target_raw_pose.acceleration_or_force.y = afy
            target_raw_pose.acceleration_or_force.z = afz

        target_raw_pose.yaw = yaw
        target_raw_pose.yaw_rate = yaw_rate

        if(self.motion_type == 0):
            target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
                            + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
                            + PositionTarget.IGNORE_YAW
        if(self.motion_type == 1):
            target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
                            + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
                            + PositionTarget.IGNORE_YAW
        if(self.motion_type == 2):
            target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
                            + PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
                            + PositionTarget.IGNORE_YAW

        return target_raw_pose

这里有几个已经带有初始值的参数：x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0

一共是11个：x，y，z表示空间位置；vx，vy，vz表示三个方向的速度；afx，afy，afz表示三个方向的加速度；yaw表示的是偏航的角度，yaw_rate表示偏航转速

        target_raw_pose = PositionTarget()
        target_raw_pose.coordinate_frame = self.coordinate_frame

这两行的意思是：这里面的PositionTarget是mavros_msgs.msg文件中的一个话题消息，话题的消息如下：

# Message for SET_POSITION_TARGET_LOCAL_NED
#
# Some complex system requires all feautures that mavlink
# message provide. See issue #402.

std_msgs/Header header

uint8 coordinate_frame
uint8 FRAME_LOCAL_NED = 1
uint8 FRAME_LOCAL_OFFSET_NED = 7
uint8 FRAME_BODY_NED = 8
uint8 FRAME_BODY_OFFSET_NED = 9

uint16 type_mask
uint16 IGNORE_PX = 1	# Position ignore flags
uint16 IGNORE_PY = 2
uint16 IGNORE_PZ = 4
uint16 IGNORE_VX = 8	# Velocity vector ignore flags
uint16 IGNORE_VY = 16
uint16 IGNORE_VZ = 32
uint16 IGNORE_AFX = 64	# Acceleration/Force vector ignore flags
uint16 IGNORE_AFY = 128
uint16 IGNORE_AFZ = 256
uint16 FORCE = 512	# Force in af vector flag
uint16 IGNORE_YAW = 1024
uint16 IGNORE_YAW_RATE = 2048

geometry_msgs/Point position
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 acceleration_or_force
float32 yaw
float32 yaw_rate

然后中间的大部分程序都是对这些变量按照函数的这些参数设定，我们暂不讨论

总之就是按照函数的输入参数，再给出相应的期望点，需要结合下面的回电函数再看看这个函数会好理解一点

• 我们再来理解一下函数：hover_state_transition

    def hover_state_transition(self,x,y,z,w):
        if abs(x) > 0.005 or abs(y)  > 0.005 or abs(z)  > 0.005 or abs(w)  > 0.005:
            self.hover_flag = 0

这个函数的名字叫做盘旋状态的变迁，只要x,y,z,w这四个值中的任意一个参数的绝对值大于0.005，那么成员变量盘旋标示hover_flag就为0，到底是什么意思还要配合下面的回调函数来看：

• cmd_pose_flu_callback(self, msg):这个名字叫做 <在车身坐标系下的位置命令反馈函数> 如果coordinate_frame = 9,好像是坐标设计构成是9，那么就到construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)的位置去
• cmd_pose_enu_callback(self, msg):这个名字叫做 <在大地坐标系下的位置命令反馈函数> 如果coordinate_frame = 1，那么和上面一样
• .......接下来的几个反馈函数都是对期望点的反馈，涉及到位置，速度，加速度，主要是中间的有些成员变量的设置值不是太明白，但是对了解和向着比赛的方向而改变的话，不构成大障碍，在这里不过多解读

5、class Communication类中的start之构造方法

    def start(self):
        rospy.init_node(self.vehicle_type+'_'+self.vehicle_id+"_communication")
        rate = rospy.Rate(100)
        '''
        main ROS thread
        '''
        while not rospy.is_shutdown():
            self.target_motion_pub.publish(self.target_motion)
            
            if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
                if(self.disarm()):
                    self.flight_mode = "DISARMED"
        
            try:
                response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')
            	odom = Odometry()
            	odom.header = response.header
            	odom.pose.pose = response.pose
            	odom.twist.twist = response.twist
            	self.odom_groundtruth_pub.publish(odom)
            except rospy.ServiceException, e:
                print "Gazebo model state service call failed: %s"%e

            rate.sleep()

ROS节点初始化，节点的名字叫做：机型+编号+_communication

设置循环的频率为100

如果ROS一直是保持状态的的话：

• self.target_motion_pub.publish(self.target_motion)：发布target_motion目标移动的的消息，这个self.target_motion = PositionTarget()在上面说过，这是一个msg文件消息，然后来看一下：这个创建的target_motion_pub（publisher）

        self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)

话题：机型+编号+/mavros/setpoint_raw/local的topic；消息的类型是PositionTarget

• if：如果飞行模式是着陆‘LAND’  而且飞机的z方向上的位置是小于0.15的，再如果是锁定的状态，那就继续解锁，不是解锁的状态就调整为解锁状态
• try：response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')，请求服务调用，输入这个参数的时候服务返回的值是response。然后把消息odom = Odometry()对应的变量进行设置，再把这个变量发布出去，通过：odom_groundtruth_pub，这个Publisher如下：

        self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+'_'+self.vehicle_id+'/ground_truth/odom', Odometry, queue_size=10)

• except：输出gazebo中的模型服务调用失败

6、main函数

if __name__ == '__main__':
    communication = Communication(sys.argv[1],sys.argv[2])
    communication.start()

先对这个Communication类进行初始化的设置，后面的sys.argv是读取所跟的参数的意思，其中机型和飞机的编号通过这两个参数进行设置，然后其他的所有的变量都通过订阅话题和服务来设置，包括位置、速度、加速度、角度、飞行模式、飞机状态......

后面是调用这个start（）函数，先对ros节点初始化，就是这个communication的节点，然后解锁飞机状态，把期望点的消息话题都发布出去，然后向gazebo中的服务请求，得到飞机的各个话题数据包，然后再将这些信息发布出去。

只要外部的控制程序命令一发出来，gazebo中的飞机就可以和外部程序建立通讯。

 

二、打开程序，查看节点，话题，服务

 

打开indoor1.launch和multirotor_communication.py的话题：

多出来的：

/xtdrone/iris_0/cmd
/xtdrone/iris_0/cmd_accel_enu
/xtdrone/iris_0/cmd_accel_flu
/xtdrone/iris_0/cmd_pose_enu
/xtdrone/iris_0/cmd_pose_flu
/xtdrone/iris_0/cmd_vel_enu
/xtdrone/iris_0/cmd_vel_flu
/xtdrone/iris_0/ground_truth/odom

服务:(这个iris_0前面打开了indoor.launch就有了)

zhu@zhu-sf315-51g-50y6:~$ rosservice list
/gazebo/apply_body_wrench
/gazebo/apply_joint_effort
/gazebo/clear_body_wrenches
/gazebo/clear_joint_forces
/gazebo/delete_light
/gazebo/delete_model
/gazebo/get_joint_properties
/gazebo/get_light_properties
/gazebo/get_link_properties
/gazebo/get_link_state
/gazebo/get_loggers
/gazebo/get_model_properties
/gazebo/get_model_state
/gazebo/get_physics_properties
/gazebo/get_world_properties
/gazebo/pause_physics
/gazebo/reset_simulation
/gazebo/reset_world
/gazebo/set_joint_properties
/gazebo/set_light_properties
/gazebo/set_link_properties
/gazebo/set_link_state
/gazebo/set_logger_level
/gazebo/set_model_configuration
/gazebo/set_model_state
/gazebo/set_parameters
/gazebo/set_physics_properties
/gazebo/spawn_sdf_model
/gazebo/spawn_urdf_model
/gazebo/unpause_physics
/gazebo_gui/get_loggers
/gazebo_gui/set_logger_level
/gazebo_gui/set_parameters
/iris_0/mavros/cmd/arming
/iris_0/mavros/cmd/command
/iris_0/mavros/cmd/command_int
/iris_0/mavros/cmd/land
/iris_0/mavros/cmd/set_home
/iris_0/mavros/cmd/takeoff
/iris_0/mavros/cmd/trigger_control
/iris_0/mavros/cmd/trigger_interval
/iris_0/mavros/cmd/vtol_transition
/iris_0/mavros/ftp/checksum
/iris_0/mavros/ftp/close
/iris_0/mavros/ftp/list
/iris_0/mavros/ftp/mkdir
/iris_0/mavros/ftp/open
/iris_0/mavros/ftp/read
/iris_0/mavros/ftp/remove
/iris_0/mavros/ftp/rename
/iris_0/mavros/ftp/reset
/iris_0/mavros/ftp/rmdir
/iris_0/mavros/ftp/truncate
/iris_0/mavros/ftp/write
/iris_0/mavros/get_loggers
/iris_0/mavros/home_position/req_update
/iris_0/mavros/log_transfer/raw/log_request_data
/iris_0/mavros/log_transfer/raw/log_request_end
/iris_0/mavros/log_transfer/raw/log_request_list
/iris_0/mavros/mission/clear
/iris_0/mavros/mission/pull
/iris_0/mavros/mission/push
/iris_0/mavros/mission/set_current
/iris_0/mavros/mount_control/configure
/iris_0/mavros/param/get
/iris_0/mavros/param/pull
/iris_0/mavros/param/push
/iris_0/mavros/param/set
/iris_0/mavros/set_logger_level
/iris_0/mavros/set_message_interval
/iris_0/mavros/set_mode
/iris_0/mavros/set_stream_rate
/iris_0/mavros/setpoint_position/mav_frame
/iris_0/mavros/setpoint_trajectory/mav_frame
/iris_0/mavros/setpoint_trajectory/reset
/iris_0/mavros/setpoint_velocity/mav_frame
/iris_0/mavros/vehicle_info_get
/iris_0/stereo_camera/left/image_raw/compressed/set_parameters
/iris_0/stereo_camera/left/image_raw/compressedDepth/set_parameters
/iris_0/stereo_camera/left/image_raw/theora/set_parameters
/iris_0/stereo_camera/left/set_camera_info
/iris_0/stereo_camera/left/set_parameters
/iris_0/stereo_camera/right/image_raw/compressed/set_parameters
/iris_0/stereo_camera/right/image_raw/compressedDepth/set_parameters
/iris_0/stereo_camera/right/image_raw/theora/set_parameters
/iris_0/stereo_camera/right/set_camera_info
/iris_0/stereo_camera/right/set_parameters
/iris_0_communication/get_loggers
/iris_0_communication/set_logger_level
/rosout/get_loggers
/rosout/set_logger_level
/rqt_gui_py_node_11408/get_loggers
/rqt_gui_py_node_11408/set_logger_level