HOKAY Physics can now send info about its bodies and ground to Foxglove.

A little jank, since it's just TF poses, but apparently making a real Scene
with 3d elements is a lot more work.
M lib/goatherd/goatglove/msg.ex +54 -7
@@ 69,6 69,20 @@ defmodule Goatherd.Goatglove.Msg do
   end
 
   @doc """
+  https://github.com/foxglove/schemas/blob/main/schemas/jsonschema/Vector2.json
+  """
+  @spec vec2(Graphmath.Vec2.vec2()) :: map()
+  def vec2(v) do
+    {x, y} = v
+
+    %{
+      "x" => x,
+      "y" => y
+    }
+  end
+
+
+  @doc """
   https://github.com/foxglove/schemas/blob/main/schemas/jsonschema/Vector3.json
   """
   @spec vec3(Graphmath.Vec3.vec3()) :: map()

          
@@ 107,21 121,34 @@ defmodule Goatherd.Goatglove.Msg do
 
   @doc """
   Create a Grid at the ground plane (in the LTP frame)
+
+  TODO: For some reason the pixel interpretation isn't quite right here,
+  the colors are wrong and it seems to draw 6 cells instead of 4.
+  But it does something, period, which is all I need right now.
   """
-  def ground(_alt, t) do
-    p = Math.pose()
+  @spec ground(number(), integer()) :: map()
+  def ground(alt, t) do
+    p = Math.pose(Graphmath.Vec3.create(0, 0, alt))
     r = 255
     g = 0
     b = 0
     a = 255
+    pixel = <<r::size(8), g::size(8), b::size(8), a::size(8)>>
+    pixel_size = 4 # bytes
+    rows = 2
+    columns = 2
+    data =  pixel <> pixel <> pixel <> pixel
+    encoded = :base64.encode_to_string(data)
+    # This doesn't end up *quite* right for some reason,
     %{
       "timestamp" => timestamp(t),
       "frame_id" => "ltp",
       "pose" => pose(p),
-      "column_count" => 1,
-      "cell_size" => 5,
-      "row_stride" => 1,
-      "cell_stride" => 1,
+      "column_count" => columns,
+      "cell_size" => vec2(Graphmath.Vec2.create(5.0, 5.0)),
+
+      "row_stride" => pixel_size * rows,         # x cells per row
+      "cell_stride" => pixel_size,               # 4 bytes per cell
       "fields" => [
         %{
           "name" => "red",

          
@@ 144,7 171,27 @@ defmodule Goatherd.Goatglove.Msg do
           "type" => 1        # UINT8
         },
       ],
-      "data" => <<r::size(8), g::size(8), b::size(8), a::size(8)>>
+      "data" => encoded,
     }
   end
+
+  @spec cube_prim(Goatherd.Math.pose(), integer()) :: map()
+  def cube_prim(pose, t) do
+    size = 1.0
+    color = %{"r" => 1.0, "g" => 0.0, "b" => 0.0, "a" => 1.0}
+    %{
+      "pose" => pose(pose),
+      "size" => size,
+      "color" => color,
+    }
+  end
+
+  @spec body(Goatherd.Physics.body(), integer()) :: map()
+  def body(body, t) do
+    # Making a proper 3d Scene for foxglove is complicated so we just
+    # show the thing as a TF node for now.
+    {offset, rot} = body.pose
+    tf = Tfex.Tf.new(:body, :ltp, offset, rot)
+    tf_to_frametransform(tf, t)
+  end
 end

          
M lib/goatherd/goatglove/server.ex +8 -13
@@ 65,6 65,8 @@ defmodule Goatherd.Goatglove.Server do
       new_state()
       |> add_channel(new_channel("/tf", "foxglove.FrameTransform", &Msg.tf/2))
       |> add_channel(new_channel("/ground_plane", "foxglove.Grid", &Msg.ground/2))
+      # |> add_channel(new_channel("/bodies", "foxglove.CubePrimitive", &Msg.body/2))
+      |> add_channel(new_channel("/bodies", "foxglove.FrameTransform", &Msg.body/2))
 
     services = []
 

          
@@ 120,15 122,13 @@ defmodule Goatherd.Goatglove.Server do
         {:push, msgs, state}
 
       {:physics, world} -> 
-        # msgs = serialize_physics(world, system_time)
-        #   # And filter out unsubscribed message types.
-        #   # We have no filter_map() apparently :/ see
-        #   # https://wiki.alopex.li/RustaceanCheatsheetForElixir
-        # msgs_with_id = msgs 
-        #   |> Enum.map(fn {name, msg} -> {get_subscription_id(name), msg} end)
-        #   |> Enum.filter(fn x -> not is_nil(x) end)
-        # {:push, serialized_msgs, state}
         msgs = serialize_topic(state, "/ground_plane", world.ground, system_time)
+        msgs = world.bodies
+          |> Enum.reduce(msgs, 
+              fn body, acc -> 
+                serialize_topic(state, "/bodies", body, system_time) ++ acc
+              end)
+        Logger.info("Sending physics messages: #{inspect(msgs)}")
         {:push, msgs, state}
 
       {:tick, t} ->

          
@@ 406,11 406,6 @@ defmodule Goatherd.Goatglove.Server do
     end
   end
 
-  @spec serialize_physics(Goatherd.Physics.world(), integer()) :: map()
-  defp serialize_physics(world, system_time) do
-    ground_plane = Msg.ground(world.ground, system_time)
-    [{"/ground_plane", ground_plane}]
-  end
 
   # IFF the client is subscribed to the topic name, it takes
   # a message for that topic name and turns it into a list of serialized

          
M lib/goatherd/math.ex +5 -0
@@ 20,6 20,11 @@ defmodule Goatherd.Math do
     pose(Vec3.create(), Quatern.identity())
   end
 
+  @spec pose(Vec3.vec3()) :: pose()
+  def pose(vec) do
+    {vec, Quatern.identity()}
+  end
+
   @spec pose(Vec3.vec3(), Quatern.quatern()) :: pose()
   def pose(vec, quat) do
     {vec, quat}

          
M lib/goatherd/physics.ex +1 -1
@@ 65,7 65,7 @@ defmodule Goatherd.Physics do
   defp new_world() do
     # Our actual object in the world
     body = %{
-      pose: Math.pose(),
+      pose: Math.pose(Vec3.create(2.0, 2.0, 2.0)),
       vel: Vec3.create(0.0, 0.0, -1.0), # m/s
       angular_vel: Quatern.identity(),  # tau/s (I think?)
       mass: 1.0,