controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from sensor_msgs.msg import Joy
00005 from crazyflie_driver.srv import UpdateParams
00006 from std_srvs.srv import Empty
00007 
00008 class Controller():
00009     def __init__(self, use_controller, joy_topic):
00010         rospy.wait_for_service('update_params')
00011         rospy.loginfo("found update_params service")
00012         self._update_params = rospy.ServiceProxy('update_params', UpdateParams)
00013 
00014         rospy.loginfo("waiting for emergency service")
00015         rospy.wait_for_service('emergency')
00016         rospy.loginfo("found emergency service")
00017         self._emergency = rospy.ServiceProxy('emergency', Empty)
00018 
00019         if use_controller:
00020             rospy.loginfo("waiting for land service")
00021             rospy.wait_for_service('land')
00022             rospy.loginfo("found land service")
00023             self._land = rospy.ServiceProxy('land', Empty)
00024 
00025             rospy.loginfo("waiting for takeoff service")
00026             rospy.wait_for_service('takeoff')
00027             rospy.loginfo("found takeoff service")
00028             self._takeoff = rospy.ServiceProxy('takeoff', Empty)
00029         else:
00030             self._land = None
00031             self._takeoff = None
00032 
00033         # subscribe to the joystick at the end to make sure that all required
00034         # services were found
00035         self._buttons = None
00036         rospy.Subscriber(joy_topic, Joy, self._joyChanged)
00037 
00038     def _joyChanged(self, data):
00039         for i in range(0, len(data.buttons)):
00040             if self._buttons == None or data.buttons[i] != self._buttons[i]:
00041                 if i == 0 and data.buttons[i] == 1 and self._land != None:
00042                     self._land()
00043                 if i == 1 and data.buttons[i] == 1:
00044                     self._emergency()
00045                 if i == 2 and data.buttons[i] == 1 and self._takeoff != None:
00046                     self._takeoff()
00047                 if i == 4 and data.buttons[i] == 1:
00048                     value = int(rospy.get_param("ring/headlightEnable"))
00049                     if value == 0:
00050                         rospy.set_param("ring/headlightEnable", 1)
00051                     else:
00052                         rospy.set_param("ring/headlightEnable", 0)
00053                     self._update_params(["ring/headlightEnable"])
00054                     print(not value)
00055 
00056         self._buttons = data.buttons
00057 
00058 if __name__ == '__main__':
00059     rospy.init_node('crazyflie_demo_controller', anonymous=True)
00060     use_controller = rospy.get_param("~use_crazyflie_controller", False)
00061     joy_topic = rospy.get_param("~joy_topic", "joy")
00062     controller = Controller(use_controller, joy_topic)
00063     rospy.spin()


crazyflie_demo
Author(s): Wolfgang Hoenig
autogenerated on Wed Jun 12 2019 19:20:46