def tfTransformPose(tf, target_frame, pose, timeout=rospy.Duration(1)):
tf.waitForTransform(target_frame, pose.header.frame_id, pose.header.stamp,
timeout)
return tf.transformPose(target_frame, pose)
self.phi_restriction()
return self.add_phi + self.cf_v
if __name__ == "__main__":
Z = 1.0
H = 0
cf_v8 = np.array([0.0, 0.0, 0.0])
swarm = Crazyswarm()
timeHelper = swarm.timeHelper
allcfs = swarm.allcfs
cf8 = allcfs.crazyfliesById[8]
allcfs.takeoff(targetHeight=Z, duration=1.0 + 0.5)
tf = tf.TransformListener()
tf.waitForTransform("/world", "/vicon/cftarget_2/cftarget_2",
rospy.Time(0), rospy.Duration(0.1))
timeHelper.sleep(2)
t_now = rospy.Time.now()
t_now2 = rospy.Time.now()
dt = 0
position_last, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_2/cftarget_2", rospy.Time(0))
Op_point_8 = np.array([position_last[0], position_last[1], Z])
Op_point_group = {8: Op_point_8}
##def init(self,ID,target_ID,offset,master_point,a,b,h,phi_factor)
Agent8 = cfAct(8, [8], np.array([0, 0, 0]), False, 1, 0.6,
0.5 * np.array([[0, 0, 0]]), 0.05)
while (True):
t_last = rospy.Time.now()
def tfLookup(tf, target_frame, source_frame, time, timeout=rospy.Duration(1)):
tf.waitForTransform(target_frame, source_frame, time, timeout)
return tfPoseToGeometry(
tf.lookupTransform(target_frame, source_frame, time))
if __name__ == "__main__":
Z = 0.7
H = 0
cf_v2 = np.array([0.0, 0.0, 0.0])
cf_v4 = np.array([0.0, 0.0, 0.0])
cf_v5 = np.array([0.0, 0.0, 0.0])
cf_v7 = np.array([0.0, 0.0, 0.0])
cf_v1 = np.array([0.0, 0.0, 0.0])
cf_v8 = np.array([0.0, 0.0, 0.0])
swarm = Crazyswarm()
timeHelper = swarm.timeHelper
allcfs = swarm.allcfs
allcfs.takeoff(targetHeight=Z, duration=1.0 + 0.5)
tf = tf.TransformListener()
tf.waitForTransform("/world", "/vicon/cftarget_1/cftarget_1",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_2/cftarget_2",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_3/cftarget_3",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_4/cftarget_4",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_5/cftarget_5",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_6/cftarget_6",
rospy.Time(0), rospy.Duration(0.1))
timeHelper.sleep(2)
t_now = rospy.Time.now()
t_now2 = rospy.Time.now()
dt = 0
position_last_1, quaternion = tf.lookupTransform(