Added feature to toggle the camera feed using joystick.

src/drone_controller.py

@@ -15,6 +15,7 @@
 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
+import std_srvs.srv
 
 # An enumeration of Drone Statuses
 from drone_status import DroneStatus
@@ -36,7 +37,11 @@ def __init__(self):
 		self.pubLand    = rospy.Publisher('/ardrone/land',Empty)
 		self.pubTakeoff = rospy.Publisher('/ardrone/takeoff',Empty)
 		self.pubReset   = rospy.Publisher('/ardrone/reset',Empty)
-
+
+		# Allow controller to toggle camera feed
+		rospy.wait_for_service('/ardrone/togglecam')
+		self.callChangeCam = rospy.ServiceProxy('/ardrone/togglecam',std_srvs.srv.Empty)
+
 		# Allow the controller to publish to the /cmd_vel topic and thus control the drone
 		self.pubCommand = rospy.Publisher('/cmd_vel',Twist)
 
@@ -66,6 +71,9 @@ def SendEmergency(self):
 		# Send an emergency (or reset) message to the ardrone driver
 		self.pubReset.publish(Empty())
 
+	def ChangeCam(self):
+		self.callChangeCam()
+
 	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

src/joystick_controller.py

@@ -24,6 +24,9 @@
 ButtonLand      = 1
 ButtonTakeoff   = 2
 
+#define the default mapping to change the camera feed
+ButtonChangeCam = 3
+
 # define the default mapping between joystick axes and their corresponding directions
 AxisRoll        = 0
 AxisPitch       = 1
@@ -47,6 +50,9 @@ def ReceiveJoystickMessage(data):
 	elif data.buttons[ButtonTakeoff]==1:
 		rospy.loginfo("Takeoff Button Pressed")
 		controller.SendTakeoff()
+	elif data.buttons[ButtonChangeCam]==1:
+		rospy.loginfo("Camera Changed")
+		controller.ChangeCam()
 	else:
 		controller.SetCommand(data.axes[AxisRoll]/ScaleRoll,data.axes[AxisPitch]/ScalePitch,data.axes[AxisYaw]/ScaleYaw,data.axes[AxisZ]/ScaleZ)
 
@@ -61,6 +67,7 @@ def ReceiveJoystickMessage(data):
 	ButtonEmergency = int (   rospy.get_param("~ButtonEmergency",ButtonEmergency) )
 	ButtonLand      = int (   rospy.get_param("~ButtonLand",ButtonLand) )
 	ButtonTakeoff   = int (   rospy.get_param("~ButtonTakeoff",ButtonTakeoff) )
+	ButtonChangeCam = int (   rospy.get_param("~ButtonChangeCam",ButtonChangeCam) )
 	AxisRoll        = int (   rospy.get_param("~AxisRoll",AxisRoll) )
 	AxisPitch       = int (   rospy.get_param("~AxisPitch",AxisPitch) )
 	AxisYaw         = int (   rospy.get_param("~AxisYaw",AxisYaw) )
@@ -84,4 +91,4 @@ def ReceiveJoystickMessage(data):
 
 	# and only progresses to here once the application has been shutdown
 	rospy.signal_shutdown('Great Flying!')
-	sys.exit(status)
+	sys.exit(status)