00001
00002
00003 import rospy
00004 import numpy as np
00005 from crazyflie_driver.srv import *
00006 from crazyflie_driver.msg import TrajectoryPolynomialPiece
00007
00008 def arrayToGeometryPoint(a):
00009 return geometry_msgs.msg.Point(a[0], a[1], a[2])
00010
00011 class Crazyflie:
00012 def __init__(self, prefix, tf):
00013 self.prefix = prefix
00014 self.tf = tf
00015
00016 rospy.wait_for_service(prefix + "/set_group_mask")
00017 self.setGroupMaskService = rospy.ServiceProxy(prefix + "/set_group_mask", SetGroupMask)
00018 rospy.wait_for_service(prefix + "/takeoff")
00019 self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff)
00020 rospy.wait_for_service(prefix + "/land")
00021 self.landService = rospy.ServiceProxy(prefix + "/land", Land)
00022 rospy.wait_for_service(prefix + "/stop")
00023 self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop)
00024 rospy.wait_for_service(prefix + "/go_to")
00025 self.goToService = rospy.ServiceProxy(prefix + "/go_to", GoTo)
00026 rospy.wait_for_service(prefix + "/upload_trajectory")
00027 self.uploadTrajectoryService = rospy.ServiceProxy(prefix + "/upload_trajectory", UploadTrajectory)
00028 rospy.wait_for_service(prefix + "/start_trajectory")
00029 self.startTrajectoryService = rospy.ServiceProxy(prefix + "/start_trajectory", StartTrajectory)
00030 rospy.wait_for_service(prefix + "/update_params")
00031 self.updateParamsService = rospy.ServiceProxy(prefix + "/update_params", UpdateParams)
00032
00033 def setGroupMask(self, groupMask):
00034 self.setGroupMaskService(groupMask)
00035
00036 def takeoff(self, targetHeight, duration, groupMask = 0):
00037 self.takeoffService(groupMask, targetHeight, rospy.Duration.from_sec(duration))
00038
00039 def land(self, targetHeight, duration, groupMask = 0):
00040 self.landService(groupMask, targetHeight, rospy.Duration.from_sec(duration))
00041
00042 def stop(self, groupMask = 0):
00043 self.stopService(groupMask)
00044
00045 def goTo(self, goal, yaw, duration, relative = False, groupMask = 0):
00046 gp = arrayToGeometryPoint(goal)
00047 self.goToService(groupMask, relative, gp, yaw, rospy.Duration.from_sec(duration))
00048
00049 def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
00050 pieces = []
00051 for poly in trajectory.polynomials:
00052 piece = TrajectoryPolynomialPiece()
00053 piece.duration = rospy.Duration.from_sec(poly.duration)
00054 piece.poly_x = poly.px.p
00055 piece.poly_y = poly.py.p
00056 piece.poly_z = poly.pz.p
00057 piece.poly_yaw = poly.pyaw.p
00058 pieces.append(piece)
00059 self.uploadTrajectoryService(trajectoryId, pieceOffset, pieces)
00060
00061 def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
00062 self.startTrajectoryService(groupMask, trajectoryId, timescale, reverse, relative)
00063
00064 def position(self):
00065 self.tf.waitForTransform("/world", "/cf" + str(self.id), rospy.Time(0), rospy.Duration(10))
00066 position, quaternion = self.tf.lookupTransform("/world", "/cf" + str(self.id), rospy.Time(0))
00067 return np.array(position)
00068
00069 def getParam(self, name):
00070 return rospy.get_param(self.prefix + "/" + name)
00071
00072 def setParam(self, name, value):
00073 rospy.set_param(self.prefix + "/" + name, value)
00074 self.updateParamsService([name])
00075
00076 def setParams(self, params):
00077 for name, value in params.iteritems():
00078 rospy.set_param(self.prefix + "/" + name, value)
00079 self.updateParamsService(params.keys())