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,