Skip to content

SLAM messages: map, pose and control for a mapping sensor - #325

Open
follesoe wants to merge 30 commits into
masterfrom
sonar3d-slam
Open

follesoe wants to merge 30 commits into
masterfrom
sonar3d-slam

Conversation

@follesoe

@follesoe follesoe commented Nov 6, 2026 •

Copy link
Copy Markdown
Member

Messages for a SLAM on the drone: it builds a 3D map from a mapping sensor (the Water Linked Sonar 3D-15 first) and tracks the drone in it.

  • Telemetry on the SLAM's own stream: the pose estimate with its covariance (SlamPoseTel), map updates and the whole map (SlamMapUpdateTel, SlamMapTel), the latest scan (SlamScanTel) and the SLAM's state, sensors and recording (SlamStatusTel).
  • Requests to start and stop it, change the sensor's settings, reset the map, fetch the map and record (SetSlamActiveReq, SetSlamSensorSettingsReq, ResetSlamReq, GetSlamMapReq, SetSlamRecordingReq).
  • Loop closures (SlamLoopClosure) on the map update whose correction they make, and all of them in the whole map, so a viewer can show what a loop corrected.
  • Sensor-neutral: poses are the platform's (the drone's), and each sensor has an id and its mount on the drone, so a stereo camera can map alongside the sonar later. The map frame says how it lies in the world (gravity, north, origin depth).
  • Fixes the frame in the DvlVelocity comment: y is right, not left.
  • Not SLAM: ResetPubFrequencyReq/Rep and ResetLogFrequencyReq/Rep reset a message's publishing or logging rate to the drone's default (empty: every message), so a client that raised a rate (as the SLAM's recording does for the dive log) can put it back.

Related: BluEye-Robotics/libblunux#527
Related: BluEye-Robotics/p2_drone#1216
Related: BluEye-Robotics/p2-django#732
Related: BluEye-Robotics/meta-blueye#535

🤖 Generated with Claude Code

follesoe and others added 11 commits November 3, 2026 18:28
The sonar SLAM publishes the sensor's pose for every shot (SlamPoseTel), the
changes to its point-cloud map (SlamMapUpdateTel) and the latest shot
(SlamShotTel) on a publisher of its own, and answers GetSlamMapReq with the
whole map and ResetSlamReq by starting a new one.

The map is keyframes and submaps: a submap's points are in its anchor
keyframe's frame, so a corrected trajectory moves them without sending them
again. Map updates are numbered, so a client fetches the map once and applies
the updates that follow it. Points are packed little-endian float32
(PointCloud) for use as typed arrays.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
A viewer draws the sonar's field of view from the shot it is showing.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
…ream

SlamShot becomes SlamScan and says which sensor it comes from (SlamSensor:
type, field of view, range), so a stereo camera can feed the same SLAM. The
whole map is published on the SLAM publisher (SlamMapTel) when a client asks
for it: GetSlamMapRep only acknowledges, so the request can go through the
drone's own replier while the map stays off the telemetry socket.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
SetSlamActiveReq starts the SLAM (its sensor switched on, a new map
begun) or stops it (the sensor off, the map kept); SetSlamActiveRep
says whether a SLAM answered and the state after. SlamStatusTel, on the
SLAM publisher once a second and on a change, carries the state and the
current map's id.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
…ange

SlamSensorSettings holds one message per kind of sensor in a oneof, so a
client shows the settings of the sensor the SLAM has: SlamSonar3dSettings
carries a 3D sonar's frequency mode (Sonar3dFrequencyMode: low, wide and
long; high, narrow, short and finer) and maximum range.
SetSlamSensorSettingsReq changes them and its reply has the sensor's
settings after; SlamStatusTel carries the current ones.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
SlamPoseEstimate's pose is the sensor's. vehicle_pose is the drone's, back
through how the sensor is mounted, and mount_tilt the tilt of a tilting
mount used for it (degrees, positive up); both are unset when the mount
is not known.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
SlamPose's body is the one its field names (the sensor, or the drone for
vehicle_pose). A 3D sonar's settings in a report use unspecified and 0
for what the sonar does not say, as a request uses them to leave a
setting. Starting a SLAM that is running changes nothing; ResetSlamReq
begins a new map.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
SetSlamRecordingReq/Rep starts and stops a SLAM recording, as the map
alone or with the sensor's data; SlamStatusTel reports it
(SlamRecordingState). ResetPubFrequencyReq and ResetLogFrequencyReq put
a message's publish and log rates back to the drone's defaults.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
SlamMapFrame says how a map's frame lies in the world: gravity aligned,
what x points along (the first scan's heading or the drone's compass),
the magnetic declination the drone applied, the origin's depth and the
map's start. SlamMap and SlamMapUpdate carry it, so a recording cut short
has it too. SlamKeyframe.vehicle_pose is the drone's pose at the keyframe.
A SLAM recording holds DroneInfoTel, naming its dive log.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Every SLAM pose (estimate, keyframe, scan) is now the platform's: the
drone's body frame when the sensor's mount is known, else the sensor
itself (SlamMapFrame.platform). Each carries the sensor's id and its
pose on the platform (mount); submaps hold one sensor's points in the
platform's frame at their anchor. SlamStatusTel lists its sensors
(SlamSensorStatus: description, settings, mount, tilt) in place of one
sensor's settings, and SlamPoseEstimate has the covariance of the
scan's match. This leaves room for a second sensor on the same drone.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
The drone's DVL nodes pass the DVL's frame through: x forward, y right,
z down. Dive logs show it: the pilot's sway (positive right) and the
DVL's y velocity have the same sign.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
@follesoe follesoe added this to the Blunux v5.3 milestone Nov 6, 2026
@follesoe follesoe added enhancement New feature or request new message New message definition labels Nov 6, 2026
@follesoe follesoe self-assigned this Nov 6, 2026
@follesoe
follesoe requested a balanced review from Copilot November 6, 2026 23:17

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟡 Changes recommended

The pose documentation mislabels the platform pose as a sensor pose, which could cause consumers to omit the mount transform for tilted or offset sensors.

1 open issue
What changed in this PR

Adds sensor-neutral SLAM protocol definitions for mapping sensors on Blueye drones, covering map data, platform poses, telemetry, and control.

Changes:

  • Defines SLAM poses, covariance, point clouds, map updates, sensor metadata, and coordinate frames.
  • Adds SLAM telemetry and requests for activation, settings, map retrieval/reset, and recording.
  • Adds publishing/logging frequency reset requests and corrects the DVL velocity frame comment from “y left” to “y right.”
File Description
protobuf_definitions/​telemetry.proto Adds SLAM pose, map, scan, and status telemetry.
protobuf_definitions/​req_rep.proto Adds SLAM control and frequency reset request/response messages.
protobuf_definitions/​message_formats.proto Defines shared SLAM data types and corrects the DVL frame comment.

Review effort: Balanced

Comment thread protobuf_definitions/telemetry.proto Resolved
follesoe and others added 4 commits November 7, 2026 00:37
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Pushes to sonar3d-slam publish 3.4.0-sonar3d-slam.<sha> under the
sonar3d-slam tag, for the SLAM pull requests to build against; latest
and the blueye-ts bump stay master's. The branch comes out of the
trigger before merging.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
SlamLoopClosure names the keyframe that closed a loop, the earlier one it
matched near, and how far the correction moved and turned it. A map update
carries the loops whose correction moved its keyframes; the whole map
carries every loop.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
So another sensor (a 2D imaging multibeam, say) joins the SLAM without
changing what is there:
- SlamSensor names its guest port device (device_id), so a client can
  show the device itself.
- SlamScan has its host time, to line it up with the sensor's own stream.
- SetSlamSensorSettingsReq can name the sensor (sensor_id); without one
  the settings go to the sensor of their kind, as before.
- The estimate's match fields no longer assume a 3D sonar's points:
  match_rms is the matched points' residual against the map.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟡 Changes recommended

The branch versioning can generate invalid SemVer for all-digit SHAs with leading zeros, causing pnpm version to fail and blocking publication.

2 open issues

Review effort: Balanced

Comment thread .github/workflows/ci-typescript.yaml Resolved
SlamPoseTel's comment says the estimate is the platform's (the drone's)
pose. A branch prerelease prefixes the commit hash with "g": an
all-digit hash with a leading zero is not a valid SemVer identifier.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🔵 Needs a closer look

The shared SLAM wire contract depends on four inaccessible implementation changes, requiring domain-owner review of producer/consumer agreement on transforms, covariance conventions, map sequencing, and recording semantics.

No open issues 🎉

2 resolved since last review

Review effort: Balanced

@follesoe
follesoe requested a review from jp-pino November 10, 2026 08:17
From Juan's review:
- SlamSubmap holds one kind of content in a oneof (points today, same
  field numbers), and the map-part and removal rules say nothing of points.
- GetSlamMapReq says which map and sequence the client holds; the reply
  says whether the missed updates come again or the whole map, in how
  many parts.
- SlamStatusTel carries the map's frame.
- ResetSlamReq's map_id 0 resets only when there is no map yet.
- PointCloud says what count validates; doc fixes for recordings,
  covariance and the pose's platform.
- The SLAM's enums and messages move to slam.proto.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
@follesoe

Copy link
Copy Markdown
Member Author

Thanks Juan. All three main points are in: protocol 69e4406, be-slam 402b47da (p2_drone#1216), and viewer c7dc324d (p2-django#732, on the 3.4.0-sonar3d-slam.g69e44062 prerelease).

  1. Submaps: SlamSubmap now has oneof content { PointCloud points = 3; }.
    • I kept the existing numbers (points 3, sensor_id 4) rather than renumbering, so it stays wire-compatible with the recordings we have.
    • removed_submap_ids and the map-part rule no longer mention points. Every repeated field is concatenated in part order, and every other field is taken from the part that sets it. The viewer now joins parts generically by that rule.
  2. Catch-up: GetSlamMapReq/Rep as you proposed.
    • be-slam keeps its latest 256 updates (at most 8 MB) and republishes the missed ones; otherwise it sends the whole map, with part_count in the reply.
    • The viewer drives its sync from the reply and falls back to the whole map if a catch-up stalls. It treats a reply with neither field (an older be-slam) as the whole map.
  3. Frame in the status: SlamStatusTel.frame = 7, sent every second; the viewer uses it until it holds the map. SlamPoseTel now says the pose is the platform's (SlamMapFrame.platform).

Smaller notes:

  • Recordings: the SlamRecordingState and SlamMapTel comments now say a recording can hold several maps, each ended with the whole map (in parts).
  • Covariance: reworded as you suggested.
  • ResetSlamReq.map_id 0: now resets only when there is no map yet, in be-slam and the viewer (which already sends the id it holds).
  • PointCloud.count: positions are exactly count × 12 bytes, and intensities empty or count bytes. be-slam and the viewer now drop a cloud that disagrees instead of truncating it.
  • replace / removed_submap_ids: be-slam doesn't send them yet, and nothing is planned short term. I kept them because the viewer handles them and raster content will need a replace rule anyway. I'm happy to drop them until they're used if you'd rather.
  • SlamMap vs SlamMapUpdate: I left them separate. A snapshot would carry update-only fields (moved_poses, removed_submap_ids) that mean nothing there. With the part rule the viewer's join is already generic, and both feed the same store.
  • slam.proto: done. The Tel messages stay in telemetry.proto, the Req/Rep pairs in req_rep.proto, and Quaternion and PointCloud in message_formats.proto. CLAUDE.md lists the new file.
  • CI trigger: noted. I'll drop it before merging, and re-pin p2-django to a master build after.

@follesoe
follesoe requested a review from jp-pino November 10, 2026 14:21

@jp-pino jp-pino left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, this looks good. I went through 69e4406 and be-slam's 402b47da: the submap oneof, the catch-up (the reply logic in map_server.cpp follows the request/reply comments exactly), the part rule, the frame in the status, slam.proto and the smaller notes are all as discussed. Keeping SlamMap and SlamMapUpdate separate and keeping replace/removed_submap_ids are fine by me.

One small comment, not blocking: SlamMap.map_id and GetSlamMapRep.map_id say "0 before the first scan", but be-slam gives a new map its id at reset, before its first scan (Slam::reset), so after a reset the id is non-zero with no keyframes yet. Maybe "0 when the SLAM has no map", which also matches the new ResetSlamReq wording.

Approving on the understanding that the sonar3d-slam trigger in ci-typescript.yaml comes out before merging.

@sindrehan

Copy link
Copy Markdown
Member

I generated blueye.protocol from this branch (69e4406) and built the SDK docs against it. It generates cleanly, the docs build with --strict, and the SDK unit tests pass. Some notes from the Python side:

1. SlamSensor.type becomes type_ in Python

type is reserved by proto-plus, so the field gets a trailing underscore. There is precedent: we have type_ three times and range_ twice already. Since nothing is released yet, would sensor_type work? The rename is wire-compatible, though it changes the TS and JSON names.

2. Where is the SLAM publisher?

The Slam*Tel comments say "on the SLAM publisher, not the main telemetry socket", but the proto never says where that is. As I understand the control system side, it is PUB 5561 (ws 9989) and REP 5562 (ws 9990), with requests forwarded by the drone's request server. Could the slam.proto header say so? SDK users read these comments as the reference docs. Without it they will expect the normal telemetry subscription to deliver the messages.

3. Can a submap be split across SlamMap parts?

The joining rule says repeated fields are concatenated in part order. If one submap's points can span two parts, the client gets two submaps entries with the same submap_id and has to merge them. Can that happen? Either answer belongs in the comment.

4. ResetPubFrequencyReq/Rep and ResetLogFrequencyReq/Rep

These are in the diff but missing from the description, and they have nothing to do with SLAM. Please add a line about them so they make it into the changelog.

5. Small things

  • SlamSensor.id says "when no sensor of that model is connected". Should that be "no other sensor of that model"?
  • ResetSlamRep has available as field 2. The other four replies have it as field 1, so the attribute order differs in the docs. Cheap to align now.
  • mount_tilt, depth, magnetic_declination and origin_depth are the first optional fields in the protocol. No change needed here; the Python docs for them need a template fix on our side.

Docs rendering (no change needed here)

SlamPose, Quaternion and SlamMapUpdateTel lose the blank line after the summary, and 13 of the new summaries are cut mid-sentence in the class tables. The generator mishandles a long first line. Many existing messages have the same problem, so I'll fix it in the blueye.protocol templates and leave the comments here as they are. The SDK also needs a reference page for slam.proto. I'll add that when the protocol is bumped.

From Sindre's review on the Python side:
- SlamSensor.type is sensor_type (proto-plus reserves type); same number.
- slam.proto says where the SLAM publishes (5561/9989) and how its
  requests reach it.
- A submap is never split across map parts.
- "no other sensor of that model"; ResetSlamRep has available first, as
  the other replies do.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
@follesoe

Copy link
Copy Markdown
Member Author

Thanks Sindre, very useful. Changes are in 32827e4, with be-slam d9e2866f (p2_drone#1216) and the viewer eb76d477 (p2-django#732, on the 3.4.0-sonar3d-slam.g32827e44 prerelease).

  1. type: renamed to sensor_type. It keeps the same field number, so existing recordings are unaffected.
  2. Where the SLAM publishes: the slam.proto header now says the SLAM has its own publisher on TCP 5561 (WebSocket 9989). Its requests go to the drone's request server like any other, which forwards them to the SLAM on 5562 (WebSocket 9990).
  3. Submaps across parts: no. be-slam never splits a submap, and the SlamMap comment now says so: each submap comes whole, in one part.
  4. Frequency messages: added a line about ResetPubFrequencyReq/Rep and ResetLogFrequencyReq/Rep to the description.
  5. Small things:
    • "no other sensor of that model": fixed.
    • ResetSlamRep now has available = 1 and map_id = 2, like the other replies. It's renumbered while it's unreleased, and be-slam and the viewer are updated.

Thanks for taking the docs rendering and the slam.proto reference page on your side.

A new map gets its id when it starts, before its first scan.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
@follesoe

Copy link
Copy Markdown
Member Author

Thanks Juan. Fixed in 7eed67c: SlamMap.map_id, GetSlamMapRep.map_id and SlamStatusTel.map_id now say 0 when the SLAM has no map. The sonar3d-slam trigger comes out before merging.

sensor_id is 3 and the points 4, as the fields read; the simulated
sensor type's reservation goes, as nothing was released with it.
Recordings convert with sbez_submaps.py.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🔵 Needs a closer look

SLAM wire-contract compatibility requires maintainer validation because the four linked producer and client repositories were inaccessible, preventing checks of endpoint routing, coordinate frames, and map synchronization.

No open issues 🎉

1 resolved since last review

Review effort: Balanced

follesoe and others added 2 commits November 11, 2026 12:43
SlamStatusTel says the newest update's sequence (a client that has less
missed one), what the estimate rests on besides its sensors (SlamAiding:
orientation source, depth, DVL velocity) and whether the back end was
dropped. SlamPoseEstimate names the keyframe it was placed against, so a
client can move kept poses with a loop closure. The SLAM's replies say
why they did not do what was asked (SlamRequestError). GetSlamMapRep:
the whole map may be one made earlier, with the updates after it.
All additions: recordings stay valid.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🔵 Needs a closer look

Human assessment is needed to evaluate ambiguous sensor-selection behavior when an empty sensor ID matches multiple sensors.

No open issues 🎉

Review effort: Balanced

follesoe and others added 2 commits November 11, 2026 18:14
SetSlamSensorSettingsReq without sensor_id goes to the SLAM's sensor of
the settings' kind when it has exactly one; with several, the request
must name one, else NO_SUCH_SENSOR. Comments only.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🔵 Needs a closer look

Human assessment is needed because critical map catch-up and snapshot-retry edge cases can double-apply pose corrections or silently lose keyframes.

No open issues 🎉

Review effort: Balanced

…ange check in CI

A SlamMap sent in parts has every field that is not repeated in its first
part (a later part may repeat map_id, sequence and part_count); a client
takes those from part 0 and concatenates the repeated fields in part order.
proto3 cannot tell a field set to its default from an unset one, so "the
part that sets it" could not hold for a field added later.

CI checks the definitions against master with buf breaking (buf.yaml): a
renumbered, retyped or removed field fails; additions pass.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟡 Changes recommended

Allowing later parts to omit snapshot identity fields can cause clients to combine different snapshots and apply subsequent updates to the wrong baseline.

1 open issue

Review effort: Balanced

Comment thread protobuf_definitions/slam.proto Resolved
A client groups parts by them and drops a set whose parts disagree, so
parts of two snapshots are never put together as one map. be-slam already
sets them in every part.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
@follesoe
follesoe marked this pull request as ready for review November 13, 2026 08:38
@follesoe
follesoe requested a review from jp-pino November 13, 2026 08:38
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

enhancement New feature or request new message New message definition

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants