这里有一段python代码，可用于操控ardrone 2.0。实验室曾经借鉴用过，并添加了部分功能。如今复习一下，顺便理理python的相关知识点。
#!/usr/bin/env python # A basic drone controller class for the tutorial "Up and flying with the AR.Drone and ROS | Getting Started" # https://github.com/mikehamer/ardrone_tutorials_getting_started # This class implements basic control functionality which we will be using in future tutorials. # It can command takeoff/landing/emergency as well as drone movement # It also tracks the drone state based on navdata feedback # Import the ROS libraries, and load the manifest file which through <depend package=... /> will give us access to the project dependencies import roslib; roslib.load_manifest('ardrone_tutorials') import rospy # Import the messages we're interested in sending and receiving # 模块的导入最好遵循from module import .. 或者import module 而不是from module import *.用前者可最大程度避免命名空间冲突的问题 # 例如import a, a中有一个类c_a,其中有方法a_foo(), 那么调用规则为a.c_a.a_foo() # 如果from a import * or from a import c_a 那么可直接调用c_a.a_foo() # 这里因为模块数量简单，数量较少，所以可以直接from import.而且这里不是import* 而是有针对性的如Empty Navdata from geometry_msgs.msg import Twist # for sending commands to the drone from std_msgs.msg import Empty # for land/takeoff/emergency from ardrone_autonomy.msg import Navdata # for receiving navdata feedback # An enumeration of Drone Statuses from drone_status import DroneStatus # Some Constants COMMAND_PERIOD = 100 #ms #BasicDroneController类，第一个类，集合了各种publisher，用于统一向ardrone发送消息。 class BasicDroneController(object): def __init__(self): # Holds the current drone status self.status = -1 # Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received # 和roscpp不同，rospy不需要通过rosspinOnce来显式调用call_back。每一个rospy.Subscriber都会独立启动一个线程来负责消息的订阅。 self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata) # Allow the controller to publish to the /ardrone/takeoff, land and reset topics self.pubLand = rospy.Publisher('/ardrone/land',Empty) self.pubTakeoff = rospy.Publisher('/ardrone/takeoff',Empty) self.pubReset = rospy.Publisher('/ardrone/reset',Empty) # Allow the controller to publish to the /cmd_vel topic and thus control the drone self.pubCommand = rospy.Publisher('/cmd_vel',Twist) # Setup regular publishing of control packets self.command = Twist() self.commandTimer = rospy.Timer(rospy.Duration(COMMAND_PERIOD/1000.0),self.SendCommand) # Land the drone if we are shutting down rospy.on_shutdown(self.SendLand) def ReceiveNavdata(self,navdata): # Although there is a lot of data in this packet, we're only interested in the state at the moment self.status = navdata.state def SendTakeoff(self): # Send a takeoff message to the ardrone driver # Note we only send a takeoff message if the drone is landed - an unexpected takeoff is not good! if(self.status == DroneStatus.Landed): self.pubTakeoff.publish(Empty()) def SendLand(self): # Send a landing message to the ardrone driver # Note we send this in all states, landing can do no harm self.pubLand.publish(Empty()) def SendEmergency(self): # Send an emergency (or reset) message to the ardrone driver self.pubReset.publish(Empty()) def SetCommand(self,roll=0,pitch=0,yaw_velocity=0,z_velocity=0): # Called by the main program to set the current command self.command.linear.x = pitch self.command.linear.y = roll self.command.linear.z = z_velocity self.command.angular.z = yaw_velocity def SendCommand(self,event): # The previously set command is then sent out periodically if the drone is flying if self.status == DroneStatus.Flying or self.status == DroneStatus.GotoHover or self.status == DroneStatus.Hovering: self.pubCommand.publish(self.command)
#!/usr/bin/env python # An enumeration of drone statuses for the tutorial "Up and flying with the AR.Drone and ROS | Getting Started" # https://github.com/mikehamer/ardrone_tutorials_getting_started #飞行器机器人状态类 class DroneStatus(object): Emergency = 0 Inited = 1 Landed = 2 Flying = 3 Hovering = 4 Test = 5 TakingOff = 6 GotoHover = 7 Landing = 8 Looping = 9
#!/usr/bin/env python # A basic video display window for the tutorial "Up and flying with the AR.Drone and ROS | Getting Started" # https://github.com/mikehamer/ardrone_tutorials_getting_started # This display window listens to the drone's video feeds and updates the display at regular intervals # It also tracks the drone's status and any connection problems, displaying them in the window's status bar # By default it includes no control functionality. The class can be extended to implement key or mouse listeners if required # Import the ROS libraries, and load the manifest file which through <depend package=... /> will give us access to the project dependencies import roslib; roslib.load_manifest('ardrone_tutorials') import rospy # Import the two types of messages we're interested in from sensor_msgs.msg import Image # for receiving the video feed from ardrone_autonomy.msg import Navdata # for receiving navdata feedback # We need to use resource locking to handle synchronization between GUI thread and ROS topic callbacks from threading import Lock # An enumeration of Drone Statuses from drone_status import DroneStatus # The GUI libraries from PySide import QtCore, QtGui # Some Constants CONNECTION_CHECK_PERIOD = 250 #ms GUI_UPDATE_PERIOD = 20 #ms DETECT_RADIUS = 4 # the radius of the circle drawn when a tag is detected # 这里利用qt做界面展示。我们的新类DroneVideoDisplay继承于QtGui.QMainWindow class DroneVideoDisplay(QtGui.QMainWindow): # 字典数据结构保存飞行器状态和对应字符串的关系 StatusMessages = { DroneStatus.Emergency : 'Emergency', DroneStatus.Inited : 'Initialized', DroneStatus.Landed : 'Landed', DroneStatus.Flying : 'Flying', DroneStatus.Hovering : 'Hovering', DroneStatus.Test : 'Test (?)', DroneStatus.TakingOff : 'Taking Off', DroneStatus.GotoHover : 'Going to Hover Mode', DroneStatus.Landing : 'Landing', DroneStatus.Looping : 'Looping (?)' } DisconnectedMessage = 'Disconnected' UnknownMessage = 'Unknown Status' def __init__(self): # 构造基类，调用基类构造函数。还有一种用法QtGui.QMainWindow.__init__(self)。 # Construct the parent class super(DroneVideoDisplay, self).__init__() # Setup our very basic GUI - a label which fills the whole window and holds our image self.setWindowTitle('AR.Drone Video Feed') self.imageBox = QtGui.QLabel(self) self.setCentralWidget(self.imageBox) # Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata) # Subscribe to the drone's video feed, calling self.ReceiveImage when a new frame is received self.subVideo = rospy.Subscriber('/ardrone/image_raw',Image,self.ReceiveImage) # 多线程问题，该类将会从ros读取数据的同时，负责显示，这两类操作必须互斥，同理还有窗口底部的状态字符串标签。 # 所以必须为两者上锁 # Holds the image frame received from the drone and later processed by the GUI self.image = None self.imageLock = Lock() self.tags = [] self.tagLock = Lock() # Holds the status message to be displayed on the next GUI update self.statusMessage = '' # Tracks whether we have received data since the last connection check # This works because data comes in at 50Hz but we're checking for a connection at 4Hz self.communicationSinceTimer = False self.connected = False # 下面的显示线程和重绘线程均按照QTimer设置的时钟频率被调用 # A timer to check whether we're still connected self.connectionTimer = QtCore.QTimer(self) self.connectionTimer.timeout.connect(self.ConnectionCallback) self.connectionTimer.start(CONNECTION_CHECK_PERIOD) # A timer to redraw the GUI self.redrawTimer = QtCore.QTimer(self) self.redrawTimer.timeout.connect(self.RedrawCallback) self.redrawTimer.start(GUI_UPDATE_PERIOD) # 很巧的实现，每隔一个周期，就将状态置为未连接，只有连接上了，才将其置为已连接 # Called every CONNECTION_CHECK_PERIOD ms, if we haven't received anything since the last callback, will assume we are having network troubles and display a message in the status bar def ConnectionCallback(self): self.connected = self.communicationSinceTimer self.communicationSinceTimer = False # 双重锁。最外一层锁image,里面一层锁tags。这里均是在显示状态下上锁，避免和ros读取数据线程冲突 def RedrawCallback(self): if self.image is not None: # We have some issues with locking between the display thread and the ros messaging thread due to the size of the image, so we need to lock the resources self.imageLock.acquire() try: # Convert the ROS image into a QImage which we can display image = QtGui.QPixmap.fromImage(QtGui.QImage(self.image.data, self.image.width, self.image.height, QtGui.QImage.Format_RGB888)) if len(self.tags) > 0: self.tagLock.acquire() try: painter = QtGui.QPainter() painter.begin(image) painter.setPen(QtGui.QColor(0,255,0)) painter.setBrush(QtGui.QColor(0,255,0)) for (x,y,d) in self.tags: r = QtCore.QRectF((x*image.width())/1000-DETECT_RADIUS,(y*image.height())/1000-DETECT_RADIUS,DETECT_RADIUS*2,DETECT_RADIUS*2) painter.drawEllipse(r) painter.drawText((x*image.width())/1000+DETECT_RADIUS,(y*image.height())/1000-DETECT_RADIUS,str(d/100)[0:4]+'m') painter.end() finally: self.tagLock.release() finally: self.imageLock.release() # We could do more processing (eg OpenCV) here if we wanted to, but for now lets just display the window. self.resize(image.width(),image.height()) self.imageBox.setPixmap(image) # Update the status bar to show the current drone status & battery level self.statusBar().showMessage(self.statusMessage if self.connected else self.DisconnectedMessage) # 读取数据，对image 和 tag 上锁，避免与显示线程冲突 def ReceiveImage(self,data): # Indicate that new data has been received (thus we are connected) self.communicationSinceTimer = True # We have some issues with locking between the GUI update thread and the ROS messaging thread due to the size of the image, so we need to lock the resources self.imageLock.acquire() try: self.image = data # Save the ros image for processing by the display thread finally: self.imageLock.release() def ReceiveNavdata(self,navdata): # Indicate that new data has been received (thus we are connected) self.communicationSinceTimer = True # Update the message to be displayed msg = self.StatusMessages[navdata.state] if navdata.state in self.StatusMessages else self.UnknownMessage self.statusMessage = '{} (Battery: {}%)'.format(msg,int(navdata.batteryPercent)) self.tagLock.acquire() try: if navdata.tags_count > 0: self.tags = [(navdata.tags_xc[i],navdata.tags_yc[i],navdata.tags_distance[i]) for i in range(0,navdata.tags_count)] else: self.tags = [] finally: self.tagLock.release() if __name__=='__main__': import sys rospy.init_node('ardrone_video_display') app = QtGui.QApplication(sys.argv) display = DroneVideoDisplay() display.show() status = app.exec_() rospy.signal_shutdown('Great Flying!') sys.exit(status)
#!/usr/bin/env python # The Keyboard Controller Node for the tutorial "Up and flying with the AR.Drone and ROS | Getting Started" # https://github.com/mikehamer/ardrone_tutorials # This controller extends the base DroneVideoDisplay class, adding a keypress handler to enable keyboard control of the drone # Import the ROS libraries, and load the manifest file which through <depend package=... /> will give us access to the project dependencies import roslib; roslib.load_manifest('ardrone_tutorials') import rospy # Load the DroneController class, which handles interactions with the drone, and the DroneVideoDisplay class, which handles video display from drone_controller import BasicDroneController from drone_video_display import DroneVideoDisplay # Finally the GUI libraries from PySide import QtCore, QtGui # 键盘映射，为了更好的可读性 # Here we define the keyboard map for our controller (note that python has no enums, so we use a class) class KeyMapping(object): PitchForward = QtCore.Qt.Key.Key_E PitchBackward = QtCore.Qt.Key.Key_D RollLeft = QtCore.Qt.Key.Key_S RollRight = QtCore.Qt.Key.Key_F YawLeft = QtCore.Qt.Key.Key_W YawRight = QtCore.Qt.Key.Key_R IncreaseAltitude = QtCore.Qt.Key.Key_Q DecreaseAltitude = QtCore.Qt.Key.Key_A Takeoff = QtCore.Qt.Key.Key_Y Land = QtCore.Qt.Key.Key_H Emergency = QtCore.Qt.Key.Key_Space # 继承于DroneVideDisplay类 # 个人觉着，但从实现上而言，不用继承于DroneVideoDisplay.只需继承QMainWindow就好 # 但是这里是需要在显示的同时进行操作，也就是说，同一个mainwindow将接受键盘事件和显示图像。 # 父类的函数在不同层级的子类被override，interesting. # Our controller definition, note that we extend the DroneVideoDisplay class class KeyboardController(DroneVideoDisplay): def __init__(self): # 初始化基类 super(KeyboardController,self).__init__() self.pitch = 0 self.roll = 0 self.yaw_velocity = 0 self.z_velocity = 0 # 这里的keyPressEvent是一个override函数，因为该类的父类的父类是QMainWindow.此类中有方法keyPressEvent.所以这里必须要实现自己的版本 # We add a keyboard handler to the DroneVideoDisplay to react to keypresses def keyPressEvent(self, event): key = event.key() # 此处的controller实则为全局变量，是BasicDroneController的对象，可访问， # 但我觉着可以尝试将BasicDroneController作为KeyboardController的成员变量会更好 # 或者直接多继承BasicDroneController # If we have constructed the drone controller and the key is not generated from an auto-repeating key if controller is not None and not event.isAutoRepeat(): # Handle the important cases first! if key == KeyMapping.Emergency: controller.SendEmergency() elif key == KeyMapping.Takeoff: controller.SendTakeoff() elif key == KeyMapping.Land: controller.SendLand() else: # Now we handle moving, notice that this section is the opposite (+=) of the keyrelease section if key == KeyMapping.YawLeft: self.yaw_velocity += 1 elif key == KeyMapping.YawRight: self.yaw_velocity += -1 elif key == KeyMapping.PitchForward: self.pitch += 1 elif key == KeyMapping.PitchBackward: self.pitch += -1 elif key == KeyMapping.RollLeft: self.roll += 1 elif key == KeyMapping.RollRight: self.roll += -1 elif key == KeyMapping.IncreaseAltitude: self.z_velocity += 1 elif key == KeyMapping.DecreaseAltitude: self.z_velocity += -1 # finally we set the command to be sent. The controller handles sending this at regular intervals controller.SetCommand(self.roll, self.pitch, self.yaw_velocity, self.z_velocity) def keyReleaseEvent(self,event): key = event.key() # If we have constructed the drone controller and the key is not generated from an auto-repeating key if controller is not None and not event.isAutoRepeat(): # Note that we don't handle the release of emergency/takeoff/landing keys here, there is no need. # Now we handle moving, notice that this section is the opposite (-=) of the keypress section if key == KeyMapping.YawLeft: self.yaw_velocity -= 1 elif key == KeyMapping.YawRight: self.yaw_velocity -= -1 elif key == KeyMapping.PitchForward: self.pitch -= 1 elif key == KeyMapping.PitchBackward: self.pitch -= -1 elif key == KeyMapping.RollLeft: self.roll -= 1 elif key == KeyMapping.RollRight: self.roll -= -1 elif key == KeyMapping.IncreaseAltitude: self.z_velocity -= 1 elif key == KeyMapping.DecreaseAltitude: self.z_velocity -= -1 # finally we set the command to be sent. The controller handles sending this at regular intervals controller.SetCommand(self.roll, self.pitch, self.yaw_velocity, self.z_velocity) # Setup the application if __name__=='__main__': import sys # Firstly we setup a ros node, so that we can communicate with the other packages rospy.init_node('ardrone_keyboard_controller') # Now we construct our Qt Application and associated controllers and windows app = QtGui.QApplication(sys.argv) # 此处，display将会显示图像，同时接受键盘事件 controller = BasicDroneController() display = KeyboardController() display.show() # executes the QT application status = app.exec_() # and only progresses to here once the application has been shutdown rospy.signal_shutdown('Great Flying!') sys.exit(status)
整个程序的架构图如下
