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(