Parameterize RgbdSensorAsync

[rpoyner-tri]

This change is 

[rpoyner-tri]

Still needs:

• python bindings reform
• test coverage
• documentation

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @rpoyner-tri)

[rpoyner-tri]

-(status: do not merge) -(status: do not review)
+@jwnimmer-tri for feature review.

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, labeled "do not merge" (waiting on @rpoyner-tri)

[jwnimmer-tri]

Reviewed 5 of 8 files at r1, all commit messages.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers

---

systems/sensors/rgbd_sensor_async.h line 113 at r1 (raw file):

As with geometry changes, if you change configuration parameters with the Set*(context, ...) methods, you must manually call Simulator::Initialize() for
the updates to take effect ...

This is not at all true (although with #12649 and related I don't blame you for being confused). Users can add more shapes to scene graph's context without calling Initialize (nor having the simulator implicitly call Initialize).

What our sensor here needs to do is notice (in its event handlers -- CalcTick and CalcTock) that the parameters have changed, and if so then implement the necessary adjustments.

For noticing, a "serial number" in the parameter struct would work (incremented anytime the user sets something), or we could do the full operator== deep equality check.

For handling when it does change, it would probably look a little bit like the prior_state.worker == nullptr cases already in Tick and Tock.

If the user changes parameters in the middle of a Tick, it's okay for the next Tock to produce either an empty image, or maybe just letting the (old-params) image come out is actually better?

---

systems/sensors/rgbd_sensor_async.h line 157 at r1 (raw file):

      bool render_label_image = false);

  DRAKE_DEPRECATED("2025-04-01", "Use default_parent_frame_id() instead.")

nit 2025-05-01 throughout

---

systems/sensors/rgbd_sensor_async.h line 191 at r1 (raw file):

  /** Returns the context dependent id of the frame to which the body is
   affixed. */

nit The class overview doxygen and constructor doxygen do not have the extra 1-space reverse indent, but all of these new functions do have it. At the very least, we should be consistent within a file.

(My own taste is that the reverse indent is awful, so we should prefer to have 0-space -- no extra indentation -- in new code. The RgbdSensor only has the undesirable 1-space indent due to historical accident.)

---

systems/sensors/rgbd_sensor_async.h line 251 at r1 (raw file):

  default_color_render_camera() const;

  /** Sets the default render camera for color/label renderings. */

Missing local documentation on exceptional condition -- the user is not allowed to change the yes/no sense of color images. It is mentioned in the class overview inside some other text, but I think needs to be reinforced here, too.

In that light, I'm not sure why we have optional<> here in the first place -- resetting from nullopt to nullopt is silly (no-op), and passing nullopt when already set to non-null is not allowed, so we might as well ditch optional here, I imagine? Or maybe we're paving the way to allow resetting back to nullopt later on?

Ditto for depth.

---

systems/sensors/rgbd_sensor_async.cc line 327 at r1 (raw file):

void RgbdSensorAsync::set_default_fps(double fps) {
  defaults_.fps = fps;

The constructor checks invariants on fps vs the other settings, and throws if violated. Here we must also work to maintain the same invariants.

Ditto for the per-context flavor.

Ditto for capture_offset and output_delay setters of both flavors.

---

systems/sensors/rgbd_sensor_async.cc line 583 at r1 (raw file):

namespace {
/* If the size of `image` already matches `camera`, or is 0 in both dimensions,

nit This comment does not seem correct? Maybe it was copy-pasted from an old version of the other file.

Code quote:

 or is 0 in both dimensions,

---

systems/sensors/rgbd_sensor_async.cc line 614 at r1 (raw file):

void RgbdSensorAsync::CalcColor(const Context<double>& context,
                                ImageRgba8U* output) const {
  DRAKE_DEMAND(HasColorCamera());

BTW This demand is a little bit silly now. This is the callback function for the port evaluator, so cross-checking that the port actually exists isn't really ever going to fail.

---

systems/sensors/rgbd_sensor_async.cc line 616 at r1 (raw file):

  DRAKE_DEMAND(HasColorCamera());
  CopyImage(get_state(context).output.color.get(), output);
  if (std::isfinite(get_state(context).output.time)) {

On first read, this is an extremely odd spot for this to happen. Surely to be useful at all it needs to happen before the CopyImage? Maybe if I play with the code and tests I'd figure it out, but in any case maybe we need a comment to help along future readers?

[jwnimmer-tri]

Reviewable status: 8 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers

---

systems/sensors/rgbd_sensor_async.cc line 327 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The constructor checks invariants on fps vs the other settings, and throws if violated. Here we must also work to maintain the same invariants.

Ditto for the per-context flavor.

Ditto for capture_offset and output_delay setters of both flavors.

Nevermind, this will be moot anyway. We can't actually offer this method in the first place.

---

systems/sensors/rgbd_sensor_async.cc line 249 at r1 (raw file):

  auto state_index = DeclareAbstractState(Value<TickTockState>{});
  DeclareInitializationUnrestrictedUpdateEvent(&Self::Initialize);
  DeclarePeriodicUnrestrictedUpdateEvent(1 / fps, capture_offset,

Actually with our event schedule here, it doesn't make any sense to move the timing parameters (fps, offset, delay) into the Context at all, does it? We can't actually implement any changes to them...

[rpoyner-tri]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers

---

systems/sensors/rgbd_sensor_async.h line 113 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

As with geometry changes, if you change configuration parameters with the Set*(context, ...) methods, you must manually call Simulator::Initialize() for
the updates to take effect ...

This is not at all true (although with #12649 and related I don't blame you for being confused). Users can add more shapes to scene graph's context without calling Initialize (nor having the simulator implicitly call Initialize).

What our sensor here needs to do is notice (in its event handlers -- CalcTick and CalcTock) that the parameters have changed, and if so then implement the necessary adjustments.

For noticing, a "serial number" in the parameter struct would work (incremented anytime the user sets something), or we could do the full operator== deep equality check.

For handling when it does change, it would probably look a little bit like the prior_state.worker == nullptr cases already in Tick and Tock.

If the user changes parameters in the middle of a Tick, it's okay for the next Tock to produce either an empty image, or maybe just letting the (old-params) image come out is actually better?

still working

---

systems/sensors/rgbd_sensor_async.cc line 249 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Actually with our event schedule here, it doesn't make any sense to move the timing parameters (fps, offset, delay) into the Context at all, does it? We can't actually implement any changes to them...

Done.

[rpoyner-tri]

minor stuff is cleaned up. still working on proper tick/tock semantics.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers

[jwnimmer-tri]

Reviewed 2 of 5 files at r2, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers

---

systems/sensors/rgbd_sensor_async.cc line 616 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

On first read, this is an extremely odd spot for this to happen. Surely to be useful at all it needs to happen before the CopyImage? Maybe if I play with the code and tests I'd figure it out, but in any case maybe we need a comment to help along future readers?

Oops. I mean to flag the Resize call as the suspicious line (not the "if"). Don't we need to call Resize before we call CopyImage?

---

systems/sensors/rgbd_sensor_async.h line 305 at r2 (raw file):

  double capture_offset_;
  // How long after input sampling that the outputs will be updated.
  double output_delay_;

nit These three have lost their const (as per on master).

[rpoyner-tri]

still working; build/test failures should be fixed.

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers

[jwnimmer-tri]

Reviewed 1 of 5 files at r2, 1 of 3 files at r3, all commit messages.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers

---

bindings/pydrake/systems/sensors_py_rgbd.cc line 202 at r3 (raw file):

            py::arg("sensor_pose"), cls_doc.set_default_X_PB.doc)
        .def("GetX_PB", &Class::GetX_PB, py::arg("context"),
            py_rvp::reference_internal, cls_doc.GetX_PB.doc)

BTW Some of these rvp are wrong. (In this case, the reference would be coming from the context, not self.)

Anyway, previously we always copied the const ReturnValue& instead of aliasing into C++ memory. Since these objects are cheap to copy, that still seems safest? Or if you want to keep the rvp you'll need to comb through carefully.

[rpoyner-tri]

maybe done? PTAL

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers

---

systems/sensors/rgbd_sensor_async.h line 113 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

still working

Done.

---

systems/sensors/rgbd_sensor_async.cc line 616 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Oops. I mean to flag the Resize call as the suspicious line (not the "if"). Don't we need to call Resize before we call CopyImage?

seems to work either way. Let's do it your way.

[jwnimmer-tri]

Reviewed 1 of 3 files at r3, 3 of 3 files at r4, 1 of 1 files at r5, all commit messages.
Reviewable status: 5 unresolved discussions, needs at least two assigned reviewers

---

systems/sensors/rgbd_sensor_async.cc line 537 at r4 (raw file):

void RgbdSensorAsync::CalcColor(const Context<double>& context,
                                ImageRgba8U* output) const {
  if (std::isfinite(get_state(context).output.time)) {

If I back out all of these if guards, no tests fail. That makes sense to me because I can't figure out any reason we would need to check that the context contains a finite time? The parameters in the context should always have a valid camera size, I think? Or if the if guard is serving some purpose, we'll need tests (and maybe comments).

Ah, maybe its to handle if someone called AllocateContext() and then tried to compute using it? I could buy a DRAKE_THROW_UNLESS(context-is-initialized) sanity check for that, but we shouldn't still output an image, should we?

---

bindings/pydrake/systems/test/sensors_test.py line 551 at r4 (raw file):

            dut.depth_camera()

        # check const configuration accessors.

nit capitalize

---

systems/sensors/test/rgbd_sensor_async_test.cc line 234 at r5 (raw file):

  EXPECT_EQ(dut.parent_id(), parent_id);
  EXPECT_EQ(dut.X_PB().GetAsMatrix34(), X_PB.GetAsMatrix34());
  EXPECT_EQ(dut.fps(), fps);

nit These next 3 are not deprecated.

---

systems/sensors/test/rgbd_sensor_async_test.cc line 255 at r5 (raw file):

  // Prepare some replacement value items.
  SourceId source_id;

Isn't the default-constructed value of an Identifier<> illegal to use, per its docs? Oh. We reassign it two lines later. Probably the initialization should not be split from declaration?

---

systems/sensors/test/rgbd_sensor_async_test.cc line 578 at r5 (raw file):

      *dut, &simulator.get_mutable_context());
  const auto& old_color_camera = dut->GetColorRenderCamera(context);
  const auto& old_depth_camera = dut->GetDepthRenderCamera(context);

Here the context is from a "get mutable context" on the simulator. After we do any mutation to the simulator context (set time, advance time) it becomes illegal to keep using these aliases into it for either context or old_..._camera. So down on line 617 our use during further operations is not valid. Aliases into the context are only momentarily valid.

Ideally we would not have any locals that alias into the context in a long-lived scope (e.g., bury them in a nested scope) but I'd be satisfied with just being careful not to re-use them (maybe with a comment).

Or actually, if const Color...Camera old_color_camera = ... were a full copy, then that's probably simplest?

Then to stop using a long-lived sub-context alias, we should probably just call dut->GetMyFromRoot(simulator.get_context()) or dut->GetMyMutableContextFromRoot(simulator.get_mutable_context()) at each call site instance. That's how we tell users to do it, so we should set a good example. No need to have any local variable aliases.

[rpoyner-tri]

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers

---

systems/sensors/test/rgbd_sensor_async_test.cc line 578 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Here the context is from a "get mutable context" on the simulator. After we do any mutation to the simulator context (set time, advance time) it becomes illegal to keep using these aliases into it for either context or old_..._camera. So down on line 617 our use during further operations is not valid. Aliases into the context are only momentarily valid.

Ideally we would not have any locals that alias into the context in a long-lived scope (e.g., bury them in a nested scope) but I'd be satisfied with just being careful not to re-use them (maybe with a comment).

Or actually, if const Color...Camera old_color_camera = ... were a full copy, then that's probably simplest?

Then to stop using a long-lived sub-context alias, we should probably just call dut->GetMyFromRoot(simulator.get_context()) or dut->GetMyMutableContextFromRoot(simulator.get_mutable_context()) at each call site instance. That's how we tell users to do it, so we should set a good example. No need to have any local variable aliases.

Done.

[rpoyner-tri]

+@SeanCurtis-TRI for platform review; Thursday schedule.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

[jwnimmer-tri]

Reviewed 3 of 3 files at r6, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

---

systems/sensors/test/rgbd_sensor_async_test.cc line 615 at r6 (raw file):

  // Revert to original camera size; see output size change back.
  dut->SetColorRenderCamera(&context, *old_color_camera);

nit This re-use of the context deep alias is still invalid.

[rpoyner-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

---

systems/sensors/test/rgbd_sensor_async_test.cc line 615 at r6 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit This re-use of the context deep alias is still invalid.

Explain how it is an alias.

[jwnimmer-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

---

systems/sensors/test/rgbd_sensor_async_test.cc line 615 at r6 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Explain how it is an alias.

There is no guarantee that after calling simulator.AdvanceTo the return value of simulator.get_mutable_context() will be at the same location in memory. Any simulator mutation operation invalidates all prior context pointers.

[jwnimmer-tri]

Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform)

---

systems/sensors/test/rgbd_sensor_async_test.cc line 615 at r6 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

There is no guarantee that after calling simulator.AdvanceTo the return value of simulator.get_mutable_context() will be at the same location in memory. Any simulator mutation operation invalidates all prior context pointers.

The rule of thumb is that anytime we call any get_mutable_foo related to a context or slice of a context (subsystem, state, params, etc), we only get to hold onto that writable pointer for the slimmest of lifetimes (something like the very next line or two). Memory locations can change, cache tickets need (re)invalidation, etc. Keeping around deep aliases into contexts leads to difficult to diagnose correctness problems. Obviously today this code works correctly as of this git sha, but the point is that holding on so long is extremely brittle, so either this test could bitrot later for unrelated reasons, or users could mimic this dangerous pattern in their own code. Calling GetMyMutableContextFromRoot or GetMutableSubsystemContext again is more verbose but obviously correct without any temporal reasoning about memory safety, and what our users should be doing (see, e.g., the rendering_multibody_plant tutorials).

[jwnimmer-tri]

Reviewed all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

---

systems/sensors/test/rgbd_sensor_async_test.cc line 615 at r6 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The rule of thumb is that anytime we call any get_mutable_foo related to a context or slice of a context (subsystem, state, params, etc), we only get to hold onto that writable pointer for the slimmest of lifetimes (something like the very next line or two). Memory locations can change, cache tickets need (re)invalidation, etc. Keeping around deep aliases into contexts leads to difficult to diagnose correctness problems. Obviously today this code works correctly as of this git sha, but the point is that holding on so long is extremely brittle, so either this test could bitrot later for unrelated reasons, or users could mimic this dangerous pattern in their own code. Calling GetMyMutableContextFromRoot or GetMutableSubsystemContext again is more verbose but obviously correct without any temporal reasoning about memory safety, and what our users should be doing (see, e.g., the rendering_multibody_plant tutorials).

The problem was the &context here (not the color_camera_), though I like the color_camera_ change on its own merits.

[rpoyner-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

---

systems/sensors/test/rgbd_sensor_async_test.cc line 615 at r6 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The problem was the &context here (not the color_camera_), though I like the color_camera_ change on its own merits.

I maintain that auto without & copies. But I made the broader change because it is more clearly correct.

[jwnimmer-tri]

Reviewed 1 of 1 files at r7.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

---

systems/sensors/test/rgbd_sensor_async_test.cc line 615 at r6 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

I maintain that auto without & copies. But I made the broader change because it is more clearly correct.

Yes. The second argument was fine in the prior review with the auto copy, and also in this one. My post here was about the first argument.

This is an alias:

  auto& context = simulator.get_system().GetMutableSubsystemContext(
      *dut, &simulator.get_mutable_context());

[rpoyner-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

---

systems/sensors/test/rgbd_sensor_async_test.cc line 615 at r6 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Yes. The second argument was fine in the prior review with the auto copy, and also in this one. My post here was about the first argument.

This is an alias:

  auto& context = simulator.get_system().GetMutableSubsystemContext(
      *dut, &simulator.get_mutable_context());

look ma, no hands.

[jwnimmer-tri]

Reviewed 1 of 1 files at r8, all commit messages.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform)

[rpoyner-tri]

thanks, missing digest:

@drake-jenkins-bot linux-jammy-gcc-bazel-experimental-debug please.