if nvalid >= 4: pf.update() estimate = pf.getEstimate() pt = Point() pt.x = estimate[0] pt.y = estimate[1] pt.z = estimate[2] position_pub.publish(pt) br = tf.TransformBroadcaster() br.sendTransform((pt.x, pt.y, pt.z), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "base_link", "world") if __name__ == "__main__": rospy.init_node('dwm_pf') anchor_positions = get_anchors_pos() pf = pfilter.ParticleFilter(200, 0.1, (10, 10, 2)) position_pub = rospy.Publisher(rospy.get_namespace() + "position", Point, queue_size=10) rospy.Subscriber("ranging", RangeArray, callback) rospy.spin()
data.ranges[anchor], 0.3) pf.update() estimate = pf.getEstimate() pt = Point() pt.x = estimate[0] pt.y = estimate[1] pt.z = estimate[2] position_pub.publish(pt) br = tf.TransformBroadcaster() br.sendTransform((pt.x, pt.y, pt.z), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "base_link", "world") if __name__ == "__main__": rospy.init_node('dwm_pf') anchor_positions = get_anchors_pos() pf = pfilter.ParticleFilter(200, 0.1, (10, 10, 2)) position_pub = rospy.Publisher("crazyflie_position", Point, queue_size=10) rospy.Subscriber("ranging", RangeArray, callback) rospy.spin()