Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
111 changes: 56 additions & 55 deletions src/drone_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 <depend package=... /> 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)
21 changes: 11 additions & 10 deletions src/drone_status.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Emergency = 0
Inited = 1
Landed = 2
Flying = 3
Hovering = 4
Test = 5
TakingOff = 6
GotoHover = 7
Landing = 8
Looping = 9
Loading