From 1451898edec0866bad24419170bb239913feb9ea Mon Sep 17 00:00:00 2001 From: ANANTHAKRISHNAN U S Date: Tue, 17 Nov 2015 03:28:12 +0530 Subject: [PATCH 1/4] Added feature to toggle the camera feed using joystick button 3. --- src/drone_controller.py | 9 ++++++++- src/joystick_controller.py | 9 ++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/drone_controller.py b/src/drone_controller.py index 30d6d79..5c0945d 100644 --- a/src/drone_controller.py +++ b/src/drone_controller.py @@ -36,7 +36,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 +70,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) From 97e03a594527c088e1071acfc5d01434e8e37d6e Mon Sep 17 00:00:00 2001 From: ANANTHAKRISHNAN U S Date: Tue, 17 Nov 2015 18:02:14 +0530 Subject: [PATCH 2/4] Imported empty srv that is required for the prev commit 1451898edec0866bad24419170bb239913feb9ea --- src/drone_controller.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drone_controller.py b/src/drone_controller.py index 5c0945d..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 From 3f12883d91d9a9b424e80889962401e7c60c256f Mon Sep 17 00:00:00 2001 From: ANANTHAKRISHNAN U S Date: Tue, 17 Nov 2015 20:24:51 +0530 Subject: [PATCH 3/4] Added two more functions. One to start and one to stop Sending Commandsperiodically --- src/drone_controller.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/drone_controller.py b/src/drone_controller.py index 1a90681..542d8f4 100644 --- a/src/drone_controller.py +++ b/src/drone_controller.py @@ -47,11 +47,14 @@ def __init__(self): # Setup regular publishing of control packets self.command = Twist() - self.commandTimer = rospy.Timer(rospy.Duration(COMMAND_PERIOD/1000.0),self.SendCommand) + self.commandTimer = None # Land the drone if we are shutting down rospy.on_shutdown(self.SendLand) - + def StartSendCommand(self): + # To initialize the package. If it is automatically initialized two independent programs trying to control the same drone will create issues. + self.commandTimer = rospy.Timer(rospy.Duration(COMMAND_PERIOD/1000.0),self.SendCommand) + 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 @@ -86,3 +89,6 @@ def SendCommand(self,event): if self.status == DroneStatus.Flying or self.status == DroneStatus.GotoHover or self.status == DroneStatus.Hovering: self.pubCommand.publish(self.command) + def StopSendCommand(self): + # Stop sending commands at a particular frequency. + self.commandTimer.shutdown() From 5e710b2a93e94b9ed607ddb1eae97ac443d38535 Mon Sep 17 00:00:00 2001 From: ANANTHAKRISHNAN U S Date: Tue, 17 Nov 2015 20:41:10 +0530 Subject: [PATCH 4/4] Revert "Added two more functions. One to start and one to stop Sending Commandsperiodically" This reverts commit 3f12883d91d9a9b424e80889962401e7c60c256f. --- src/drone_controller.py | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/drone_controller.py b/src/drone_controller.py index 542d8f4..1a90681 100644 --- a/src/drone_controller.py +++ b/src/drone_controller.py @@ -47,14 +47,11 @@ def __init__(self): # Setup regular publishing of control packets self.command = Twist() - self.commandTimer = None + 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 StartSendCommand(self): - # To initialize the package. If it is automatically initialized two independent programs trying to control the same drone will create issues. - self.commandTimer = rospy.Timer(rospy.Duration(COMMAND_PERIOD/1000.0),self.SendCommand) - + 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 @@ -89,6 +86,3 @@ def SendCommand(self,event): if self.status == DroneStatus.Flying or self.status == DroneStatus.GotoHover or self.status == DroneStatus.Hovering: self.pubCommand.publish(self.command) - def StopSendCommand(self): - # Stop sending commands at a particular frequency. - self.commandTimer.shutdown()