diff --git a/src/drone_controller.py b/src/drone_controller.py index 30d6d79..1a90681 100644 --- a/src/drone_controller.py +++ b/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 diff --git a/src/joystick_controller.py b/src/joystick_controller.py index 25798b6..d0f93cc 100755 --- a/src/joystick_controller.py +++ b/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) \ No newline at end of file + sys.exit(status)