def interactive_recogniser(dynamics, observation, prior, weight, gestures):
pf = pfilter.ParticleFilter(initial=prior,
observe_fn=observation,
n_particles=200,
dynamics_fn=dynamics,
weight_fn=weight,
resample_proportion=0.01)
recogniser = Recogniser(pf, gestures)
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()
def init_model():
return pfilter.ParticleFilter(SpinState().to_vec(), )