diff --git a/lib/localiser/localisation/tag/filter.ex b/lib/localiser/localisation/tag/filter.ex index 6a22be5..296a1b8 100644 --- a/lib/localiser/localisation/tag/filter.ex +++ b/lib/localiser/localisation/tag/filter.ex @@ -57,6 +57,8 @@ defmodule Localiser.Localisation.Tag.Filter do def handle_cast({:ingest, measurements}, state) do {:ok, {x, y}, new_filter_state} = state.filter_module.update(state.filter_state, measurements) + publish_particles(state.tag_id, new_filter_state, {x, y}) + new_room = find_room(state.rooms, x, y) new_room_id = if new_room, do: new_room.id, else: nil @@ -79,7 +81,7 @@ defmodule Localiser.Localisation.Tag.Filter do end end - # Room geometry changed — reload the rooms cache so containment checks stay accurate. + # Room geometry changed: reload the rooms cache so containment checks stay accurate. @impl true def handle_info({event, _}, state) when event in [:room_created, :room_updated] do {:noreply, %{state | rooms: load_rooms()}} @@ -91,6 +93,27 @@ defmodule Localiser.Localisation.Tag.Filter do def handle_info(_msg, state), do: {:noreply, state} + # Only publish if the filter exposes particle state (i.e. is the particle filter). + defp publish_particles(tag_id, filter_state, {x, y}) do + # Other filter implementations may not have a :particles key - silently skip. + with %{particles: particles, weights: weights} <- filter_state do + payload = %{ + tag_id: tag_id, + estimate: %{x: x, y: y}, + particles: Enum.zip(particles, weights) + |> Enum.map(fn {p, w} -> + {px, py} = particle_xy(p) + %{x: px, y: py, w: w} + end) + } + + Phoenix.PubSub.broadcast(Localiser.PubSub, "particles:#{tag_id}", {:particles_updated, payload}) + end + end + + defp particle_xy({x, y}), do: {x, y} + defp particle_xy({x, y, _vx, _vy}), do: {x, y} + defp load_rooms do Floors.list_floors_with_rooms() |> Enum.flat_map(& &1.rooms)