diff --git a/src/drone_controller.py b/src/drone_controller.py
index 30d6d79..00750e5 100644
--- a/src/drone_controller.py
+++ b/src/drone_controller.py
@@ -8,73 +8,74 @@
# It also tracks the drone state based on navdata feedback
# Import the ROS libraries, and load the manifest file which through will give us access to the project dependencies
-import roslib; roslib.load_manifest('ardrone_tutorials')
+import roslib
+roslib.load_manifest('ardrone_tutorials')
import rospy
# Import the messages we're interested in sending and receiving
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
+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
+COMMAND_PERIOD = 100 # ms
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
- 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)
+ 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
+ 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)
diff --git a/src/drone_status.py b/src/drone_status.py
index 124d215..a454581 100644
--- a/src/drone_status.py
+++ b/src/drone_status.py
@@ -3,14 +3,15 @@
# 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
\ No newline at end of file
+ Emergency = 0
+ Inited = 1
+ Landed = 2
+ Flying = 3
+ Hovering = 4
+ Test = 5
+ TakingOff = 6
+ GotoHover = 7
+ Landing = 8
+ Looping = 9
diff --git a/src/drone_video_display.py b/src/drone_video_display.py
index 0531139..59c3cdc 100644
--- a/src/drone_video_display.py
+++ b/src/drone_video_display.py
@@ -8,12 +8,13 @@
# 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 will give us access to the project dependencies
-import roslib; roslib.load_manifest('ardrone_tutorials')
+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
+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
@@ -26,137 +27,141 @@
# 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
+CONNECTION_CHECK_PERIOD = 250 # ms
+GUI_UPDATE_PERIOD = 20 # ms
+DETECT_RADIUS = 4 # the radius of the circle drawn when a tag is detected
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):
- # 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)
-
- # 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
-
- # 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
-
- 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)
-
- 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)
+ 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):
+ # 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)
+
+ # 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
+
+ # 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
+
+ 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)
+
+ 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)
diff --git a/src/joystick_controller.py b/src/joystick_controller.py
index 25798b6..39af5c3 100755
--- a/src/joystick_controller.py
+++ b/src/joystick_controller.py
@@ -6,7 +6,8 @@
# This controller implements the base DroneVideoDisplay class, the DroneController class and subscribes to joystick messages
# Import the ROS libraries, and load the manifest file which through will give us access to the project dependencies
-import roslib; roslib.load_manifest('ardrone_tutorials')
+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
@@ -21,67 +22,72 @@
# define the default mapping between joystick buttons and their corresponding actions
ButtonEmergency = 0
-ButtonLand = 1
-ButtonTakeoff = 2
+ButtonLand = 1
+ButtonTakeoff = 2
# define the default mapping between joystick axes and their corresponding directions
-AxisRoll = 0
-AxisPitch = 1
-AxisYaw = 3
-AxisZ = 4
+AxisRoll = 0
+AxisPitch = 1
+AxisYaw = 3
+AxisZ = 4
# define the default scaling to apply to the axis inputs. useful where an axis is inverted
-ScaleRoll = 1.0
-ScalePitch = 1.0
-ScaleYaw = 1.0
-ScaleZ = 1.0
+ScaleRoll = 1.0
+ScalePitch = 1.0
+ScaleYaw = 1.0
+ScaleZ = 1.0
# handles the reception of joystick packets
+
+
def ReceiveJoystickMessage(data):
- if data.buttons[ButtonEmergency]==1:
- rospy.loginfo("Emergency Button Pressed")
- controller.SendEmergency()
- elif data.buttons[ButtonLand]==1:
- rospy.loginfo("Land Button Pressed")
- controller.SendLand()
- elif data.buttons[ButtonTakeoff]==1:
- rospy.loginfo("Takeoff Button Pressed")
- controller.SendTakeoff()
- else:
- controller.SetCommand(data.axes[AxisRoll]/ScaleRoll,data.axes[AxisPitch]/ScalePitch,data.axes[AxisYaw]/ScaleYaw,data.axes[AxisZ]/ScaleZ)
+ if data.buttons[ButtonEmergency] == 1:
+ rospy.loginfo("Emergency Button Pressed")
+ controller.SendEmergency()
+
+ elif data.buttons[ButtonLand] == 1:
+ rospy.loginfo("Land Button Pressed")
+ controller.SendLand()
+
+ elif data.buttons[ButtonTakeoff] == 1:
+ rospy.loginfo("Takeoff Button Pressed")
+ controller.SendTakeoff()
+
+ else:
+ controller.SetCommand(data.axes[AxisRoll] / ScaleRoll, data.axes[AxisPitch] / ScalePitch, data.axes[AxisYaw] / ScaleYaw, data.axes[AxisZ] / ScaleZ)
# 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_joystick_controller')
-
- # Next load in the parameters from the launch-file
- ButtonEmergency = int ( rospy.get_param("~ButtonEmergency",ButtonEmergency) )
- ButtonLand = int ( rospy.get_param("~ButtonLand",ButtonLand) )
- ButtonTakeoff = int ( rospy.get_param("~ButtonTakeoff",ButtonTakeoff) )
- AxisRoll = int ( rospy.get_param("~AxisRoll",AxisRoll) )
- AxisPitch = int ( rospy.get_param("~AxisPitch",AxisPitch) )
- AxisYaw = int ( rospy.get_param("~AxisYaw",AxisYaw) )
- AxisZ = int ( rospy.get_param("~AxisZ",AxisZ) )
- ScaleRoll = float ( rospy.get_param("~ScaleRoll",ScaleRoll) )
- ScalePitch = float ( rospy.get_param("~ScalePitch",ScalePitch) )
- ScaleYaw = float ( rospy.get_param("~ScaleYaw",ScaleYaw) )
- ScaleZ = float ( rospy.get_param("~ScaleZ",ScaleZ) )
-
- # Now we construct our Qt Application and associated controllers and windows
- app = QtGui.QApplication(sys.argv)
- display = DroneVideoDisplay()
- controller = BasicDroneController()
-
- # subscribe to the /joy topic and handle messages of type Joy with the function ReceiveJoystickMessage
- subJoystick = rospy.Subscriber('/joy', Joy, ReceiveJoystickMessage)
-
- # executes the QT application
- display.show()
- status = app.exec_()
-
- # and only progresses to here once the application has been shutdown
- rospy.signal_shutdown('Great Flying!')
- sys.exit(status)
\ No newline at end of file
+if __name__ == '__main__':
+ import sys
+ # Firstly we setup a ros node, so that we can communicate with the other packages
+ rospy.init_node('ardrone_joystick_controller')
+
+ # Next load in the parameters from the launch-file
+ ButtonEmergency = int(rospy.get_param("~ButtonEmergency", ButtonEmergency))
+ ButtonLand = int(rospy.get_param("~ButtonLand", ButtonLand))
+ ButtonTakeoff = int(rospy.get_param("~ButtonTakeoff", ButtonTakeoff))
+ AxisRoll = int(rospy.get_param("~AxisRoll", AxisRoll))
+ AxisPitch = int(rospy.get_param("~AxisPitch", AxisPitch))
+ AxisYaw = int(rospy.get_param("~AxisYaw", AxisYaw))
+ AxisZ = int(rospy.get_param("~AxisZ", AxisZ))
+ ScaleRoll = float(rospy.get_param("~ScaleRoll", ScaleRoll))
+ ScalePitch = float(rospy.get_param("~ScalePitch", ScalePitch))
+ ScaleYaw = float(rospy.get_param("~ScaleYaw", ScaleYaw))
+ ScaleZ = float(rospy.get_param("~ScaleZ", ScaleZ))
+
+ # Now we construct our Qt Application and associated controllers and windows
+ app = QtGui.QApplication(sys.argv)
+ display = DroneVideoDisplay()
+ controller = BasicDroneController()
+
+ # subscribe to the /joy topic and handle messages of type Joy with the function ReceiveJoystickMessage
+ subJoystick = rospy.Subscriber('/joy', Joy, ReceiveJoystickMessage)
+
+ # executes the QT application
+ display.show()
+ status = app.exec_()
+
+ # and only progresses to here once the application has been shutdown
+ rospy.signal_shutdown('Great Flying!')
+ sys.exit(status)
diff --git a/src/keyboard_controller.py b/src/keyboard_controller.py
index ae3cf11..bab88a4 100755
--- a/src/keyboard_controller.py
+++ b/src/keyboard_controller.py
@@ -6,7 +6,8 @@
# 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 will give us access to the project dependencies
-import roslib; roslib.load_manifest('ardrone_tutorials')
+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
@@ -19,116 +20,115 @@
# 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
+ 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
# 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
-# We add a keyboard handler to the DroneVideoDisplay to react to keypresses
- def keyPressEvent(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():
- # 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)
+ def __init__(self):
+ super(KeyboardController, self).__init__()
+
+ self.pitch = 0
+ self.roll = 0
+ self.yaw_velocity = 0
+ self.z_velocity = 0
+# We add a keyboard handler to the DroneVideoDisplay to react to keypresses
+ def keyPressEvent(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():
+ # 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')
+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)
- controller = BasicDroneController()
- display = KeyboardController()
+ # Now we construct our Qt Application and associated controllers and windows
+ app = QtGui.QApplication(sys.argv)
+ controller = BasicDroneController()
+ display = KeyboardController()
- display.show()
+ display.show()
- # executes the QT application
- status = app.exec_()
+ # 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)
\ No newline at end of file
+ # and only progresses to here once the application has been shutdown
+ rospy.signal_shutdown('Great Flying!')
+ sys.exit(status)