diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index 6829319d6374..7339c0b3da63 100644 --- a/bindings/pydrake/systems/BUILD.bazel +++ b/bindings/pydrake/systems/BUILD.bazel @@ -173,6 +173,7 @@ drake_pybind_library( "//bindings/pydrake:documentation_pybind", "//bindings/pydrake/common:cpp_template_pybind", "//bindings/pydrake/common:default_scalars_pybind", + "//bindings/pydrake/common:deprecation_pybind", "//bindings/pydrake/common:eigen_pybind", "//bindings/pydrake/common:ref_cycle_pybind", "//bindings/pydrake/common:serialize_pybind", diff --git a/bindings/pydrake/systems/sensors_py_rgbd.cc b/bindings/pydrake/systems/sensors_py_rgbd.cc index 62f3708ea9bf..58902bca8ead 100644 --- a/bindings/pydrake/systems/sensors_py_rgbd.cc +++ b/bindings/pydrake/systems/sensors_py_rgbd.cc @@ -1,3 +1,4 @@ +#include "drake/bindings/pydrake/common/deprecation_pybind.h" #include "drake/bindings/pydrake/common/ref_cycle_pybind.h" #include "drake/bindings/pydrake/documentation_pybind.h" #include "drake/bindings/pydrake/systems/sensors_py.h" @@ -170,7 +171,9 @@ void DefineSensorsRgbd(py::module m) { { using Class = RgbdSensorAsync; constexpr auto& cls_doc = doc.RgbdSensorAsync; - py::class_>(m, "RgbdSensorAsync", cls_doc.doc) + py::class_> rgbd_sensor_async( + m, "RgbdSensorAsync", cls_doc.doc); + rgbd_sensor_async .def(py::init*, geometry::FrameId, const math::RigidTransformd&, double, double, double, std::optional, @@ -179,14 +182,46 @@ void DefineSensorsRgbd(py::module m) { py::arg("fps"), py::arg("capture_offset"), py::arg("output_delay"), py::arg("color_camera"), py::arg("depth_camera") = std::nullopt, py::arg("render_label_image") = false, cls_doc.ctor.doc) - .def("parent_id", &Class::parent_id, cls_doc.parent_id.doc) - .def("X_PB", &Class::X_PB, cls_doc.X_PB.doc) .def("fps", &Class::fps, cls_doc.fps.doc) .def("capture_offset", &Class::capture_offset, cls_doc.capture_offset.doc) .def("output_delay", &Class::output_delay, cls_doc.output_delay.doc) - .def("color_camera", &Class::color_camera, cls_doc.color_camera.doc) - .def("depth_camera", &Class::depth_camera, cls_doc.depth_camera.doc) + .def("default_parent_frame_id", &Class::default_parent_frame_id, + cls_doc.default_parent_frame_id.doc) + .def("set_default_parent_frame_id", &Class::set_default_parent_frame_id, + py::arg("id"), cls_doc.set_default_parent_frame_id.doc) + .def("GetParentFrameId", &Class::GetParentFrameId, py::arg("context"), + cls_doc.GetParentFrameId.doc) + .def("SetParentFrameId", &Class::SetParentFrameId, py::arg("context"), + py::arg("id"), cls_doc.SetParentFrameId.doc) + .def("default_X_PB", &Class::default_X_PB, py_rvp::reference_internal, + cls_doc.default_X_PB.doc) + .def("set_default_X_PB", &Class::set_default_X_PB, + py::arg("sensor_pose"), cls_doc.set_default_X_PB.doc) + .def( + "GetX_PB", &Class::GetX_PB, py::arg("context"), cls_doc.GetX_PB.doc) + .def("SetX_PB", &Class::SetX_PB, py::arg("context"), + py::arg("sensor_pose"), cls_doc.SetX_PB.doc) + .def("default_color_render_camera", &Class::default_color_render_camera, + py_rvp::reference_internal, cls_doc.default_color_render_camera.doc) + .def("set_default_color_render_camera", + &Class::set_default_color_render_camera, py::arg("color_camera"), + cls_doc.set_default_color_render_camera.doc) + .def("GetColorRenderCamera", &Class::GetColorRenderCamera, + py::arg("context"), cls_doc.GetColorRenderCamera.doc) + .def("SetColorRenderCamera", &Class::SetColorRenderCamera, + py::arg("context"), py::arg("color_camera"), + cls_doc.SetColorRenderCamera.doc) + .def("default_depth_render_camera", &Class::default_depth_render_camera, + py_rvp::reference_internal, cls_doc.default_depth_render_camera.doc) + .def("set_default_depth_render_camera", + &Class::set_default_depth_render_camera, py::arg("depth_camera"), + cls_doc.set_default_depth_render_camera.doc) + .def("GetDepthRenderCamera", &Class::GetDepthRenderCamera, + py::arg("context"), cls_doc.GetDepthRenderCamera.doc) + .def("SetDepthRenderCamera", &Class::SetDepthRenderCamera, + py::arg("context"), py::arg("depth_camera"), + cls_doc.SetDepthRenderCamera.doc) .def("color_image_output_port", &Class::color_image_output_port, py_rvp::reference_internal, cls_doc.color_image_output_port.doc) .def("depth_image_32F_output_port", &Class::depth_image_32F_output_port, @@ -200,6 +235,23 @@ void DefineSensorsRgbd(py::module m) { cls_doc.body_pose_in_world_output_port.doc) .def("image_time_output_port", &Class::image_time_output_port, py_rvp::reference_internal, cls_doc.image_time_output_port.doc); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + rgbd_sensor_async // BR + .def("parent_id", + WrapDeprecated(cls_doc.parent_id.doc_deprecated, &Class::parent_id), + cls_doc.parent_id.doc_deprecated) + .def("X_PB", WrapDeprecated(cls_doc.X_PB.doc_deprecated, &Class::X_PB), + cls_doc.X_PB.doc_deprecated) + .def("color_camera", + WrapDeprecated( + cls_doc.color_camera.doc_deprecated, &Class::color_camera), + cls_doc.color_camera.doc_deprecated) + .def("depth_camera", + WrapDeprecated( + cls_doc.depth_camera.doc_deprecated, &Class::depth_camera), + cls_doc.depth_camera.doc_deprecated); +#pragma GCC diagnostic pop } } diff --git a/bindings/pydrake/systems/test/sensors_test.py b/bindings/pydrake/systems/test/sensors_test.py index 7ba7b144f6bb..e87b7486c5e8 100644 --- a/bindings/pydrake/systems/test/sensors_test.py +++ b/bindings/pydrake/systems/test/sensors_test.py @@ -8,6 +8,7 @@ import numpy as np from pydrake.common import FindResourceOrThrow from pydrake.common.test_utilities import numpy_compare +from pydrake.common.test_utilities.deprecation import catch_drake_warnings from pydrake.common.test_utilities.pickle_compare import assert_pickle from pydrake.common.value import AbstractValue, Value from pydrake.geometry import ( @@ -465,25 +466,31 @@ def check_info(camera_info): for constructor in [construct, construct_single]: sensor = constructor(parent_id, X_WB) + + # Check default accessors. check_info(sensor.default_color_render_camera() .core().intrinsics()) check_info(sensor.default_depth_render_camera() .core().intrinsics()) self.assertIsInstance(sensor.default_X_PB(), RigidTransform) self.assertEqual(sensor.default_parent_frame_id(), parent_id) - sensor.set_default_parent_frame_id(parent_id) + sensor.set_default_parent_frame_id(id=parent_id) + sensor.set_default_X_PB(sensor_pose=RigidTransform()) + color_camera = sensor.default_color_render_camera() + sensor.set_default_color_render_camera(color_camera=color_camera) + depth_camera = sensor.default_depth_render_camera() + sensor.set_default_depth_render_camera(depth_camera=depth_camera) check_ports(sensor) + # Check parameter API. context = sensor.CreateDefaultContext() self.assertIsInstance(sensor.GetColorRenderCamera(context=context), ColorRenderCamera) - color_camera = sensor.default_color_render_camera() sensor.SetColorRenderCamera(context=context, color_camera=color_camera) self.assertIsInstance(sensor.GetDepthRenderCamera(context=context), DepthRenderCamera) - depth_camera = sensor.default_depth_render_camera() sensor.SetDepthRenderCamera(context=context, depth_camera=depth_camera) self.assertIsInstance(sensor.GetX_PB(context=context), @@ -520,8 +527,9 @@ def test_rgbd_sensor_async(self): camera_core = self._make_render_camera_core() color_camera = ColorRenderCamera(camera_core) depth_camera = DepthRenderCamera(camera_core, DepthRange(0.1, 5.5)) + parent_id = FrameId.get_new_id() dut = mut.RgbdSensorAsync(scene_graph=scene_graph, - parent_id=FrameId.get_new_id(), + parent_id=parent_id, X_PB=RigidTransform(), fps=1.0, capture_offset=0.1, @@ -529,13 +537,49 @@ def test_rgbd_sensor_async(self): color_camera=color_camera, depth_camera=depth_camera, render_label_image=True) - dut.parent_id() - dut.X_PB() - dut.fps() - dut.capture_offset() - dut.output_delay() - dut.color_camera() - dut.depth_camera() + + # Test deprecated methods. + with catch_drake_warnings(expected_count=1): + dut.parent_id() + with catch_drake_warnings(expected_count=1): + dut.X_PB() + with catch_drake_warnings(expected_count=1): + dut.color_camera() + with catch_drake_warnings(expected_count=1): + dut.depth_camera() + + # Check const configuration accessors. + self.assertIsInstance(dut.fps(), float) + self.assertIsInstance(dut.capture_offset(), float) + self.assertIsInstance(dut.output_delay(), float) + + # Check default accessors. + self.assertEqual(dut.default_parent_frame_id(), parent_id) + self.assertIsInstance(dut.default_X_PB(), RigidTransform) + self.assertIsInstance(dut.default_color_render_camera(), + ColorRenderCamera) + self.assertIsInstance(dut.default_depth_render_camera(), + DepthRenderCamera) + dut.set_default_parent_frame_id(id=parent_id) + dut.set_default_X_PB(sensor_pose=RigidTransform()) + color_camera = dut.default_color_render_camera() + dut.set_default_color_render_camera(color_camera=color_camera) + depth_camera = dut.default_depth_render_camera() + dut.set_default_depth_render_camera(depth_camera=depth_camera) + + # Check parameter API. + context = dut.CreateDefaultContext() + self.assertEqual(dut.GetParentFrameId(context=context), parent_id) + dut.SetParentFrameId(context=context, id=parent_id) + self.assertIsInstance(dut.GetX_PB(context=context), RigidTransform) + dut.SetX_PB(context=context, sensor_pose=RigidTransform()) + self.assertIsInstance(dut.GetColorRenderCamera(context=context), + ColorRenderCamera) + dut.SetColorRenderCamera(context=context, color_camera=color_camera) + self.assertIsInstance(dut.GetDepthRenderCamera(context=context), + DepthRenderCamera) + dut.SetDepthRenderCamera(context=context, depth_camera=depth_camera) + dut.color_image_output_port() dut.depth_image_32F_output_port() dut.depth_image_16U_output_port() diff --git a/systems/sensors/BUILD.bazel b/systems/sensors/BUILD.bazel index 127c34dd58bb..4f5a37c9e9b7 100644 --- a/systems/sensors/BUILD.bazel +++ b/systems/sensors/BUILD.bazel @@ -560,6 +560,7 @@ drake_cc_googletest( name = "rgbd_sensor_async_test", deps = [ ":rgbd_sensor_async", + "//common/test_utilities:eigen_matrix_compare", "//common/test_utilities:expect_throws_message", "//geometry/test_utilities:dummy_render_engine", "//multibody/plant", diff --git a/systems/sensors/rgbd_sensor_async.cc b/systems/sensors/rgbd_sensor_async.cc index dd3e4a2610f3..599057e8c2d2 100644 --- a/systems/sensors/rgbd_sensor_async.cc +++ b/systems/sensors/rgbd_sensor_async.cc @@ -50,6 +50,7 @@ using geometry::render::ColorRenderCamera; using geometry::render::DepthRange; using geometry::render::DepthRenderCamera; using geometry::render::RenderCameraCore; +using internal::RgbdSensorAsyncParameters; using math::RigidTransformd; namespace { @@ -170,15 +171,18 @@ class Worker { DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Worker); Worker(std::shared_ptr sensor, bool color, bool depth, - bool label) + bool label, uint64_t parameters_version) : sensor_{std::move(sensor)}, color_{color}, depth_{depth}, - label_{label} { + label_{label}, + parameters_version_{parameters_version} { DRAKE_DEMAND(sensor_ != nullptr); sensor_context_ = sensor_->CreateDefaultContext(); } + uint64_t parameters_version() const { return parameters_version_; } + /* Begins rendering the given geometry as an async task. */ void Start(double context_time, const QueryObject& query); @@ -193,6 +197,7 @@ class Worker { const bool color_; const bool depth_; const bool label_; + const uint64_t parameters_version_; std::unique_ptr> sensor_context_; std::future future_; }; @@ -219,21 +224,21 @@ RgbdSensorAsync::RgbdSensorAsync(const SceneGraph* scene_graph, std::optional depth_camera, bool render_label_image) : scene_graph_{scene_graph}, - parent_id_{parent_id}, - X_PB_{X_PB}, fps_{fps}, capture_offset_{capture_offset}, output_delay_{output_delay}, - color_camera_{std::move(color_camera)}, - depth_camera_{std::move(depth_camera)}, + defaults_{.parent_frame_id = parent_id, + .X_PB = X_PB, + .color_camera = std::move(color_camera), + .depth_camera = std::move(depth_camera)}, render_label_image_{render_label_image} { DRAKE_THROW_UNLESS(scene_graph != nullptr); DRAKE_THROW_UNLESS(std::isfinite(fps) && (fps > 0)); DRAKE_THROW_UNLESS(std::isfinite(capture_offset) && (capture_offset >= 0)); DRAKE_THROW_UNLESS(std::isfinite(output_delay) && (output_delay > 0)); DRAKE_THROW_UNLESS(output_delay < (1 / fps)); - DRAKE_THROW_UNLESS(color_camera_.has_value() || depth_camera_.has_value()); - DRAKE_THROW_UNLESS(!render_label_image || color_camera_.has_value()); + DRAKE_THROW_UNLESS(HasColorCamera() || HasDepthCamera()); + DRAKE_THROW_UNLESS(!render_label_image || HasColorCamera()); // TODO(jwnimmer-tri) Check that the render engine named by either of the two // cameras is not the (known non-threadsafe) VTK engine. @@ -251,10 +256,10 @@ RgbdSensorAsync::RgbdSensorAsync(const SceneGraph* scene_graph, // Output. These port names are intended to match RgbdSensor's port names. const std::set state = {abstract_state_ticket(state_index)}; - if (color_camera_.has_value()) { + if (HasColorCamera()) { DeclareAbstractOutputPort("color_image", &Self::CalcColor, state); } - if (depth_camera_.has_value()) { + if (HasDepthCamera()) { DeclareAbstractOutputPort("depth_image_32f", &Self::CalcDepth32F, state); DeclareAbstractOutputPort("depth_image_16u", &Self::CalcDepth16U, state); } @@ -263,6 +268,100 @@ RgbdSensorAsync::RgbdSensorAsync(const SceneGraph* scene_graph, } DeclareAbstractOutputPort("body_pose_in_world", &Self::CalcX_WB, state); DeclareVectorOutputPort("image_time", 1, &Self::CalcImageTime, state); + + parameter_index_ = AbstractParameterIndex{this->DeclareAbstractParameter( + Value(defaults_))}; +} + +geometry::FrameId RgbdSensorAsync::default_parent_frame_id() const { + return defaults_.parent_frame_id; +} + +void RgbdSensorAsync::set_default_parent_frame_id(geometry::FrameId id) { + defaults_.parent_frame_id = id; +} + +geometry::FrameId RgbdSensorAsync::GetParentFrameId( + const Context& context) const { + this->ValidateContext(context); + return GetParameters(context).parent_frame_id; +} + +void RgbdSensorAsync::SetParentFrameId(Context* context, + geometry::FrameId id) const { + this->ValidateContext(context); + GetMutableParameters(context)->parent_frame_id = id; +} + +const math::RigidTransformd& RgbdSensorAsync::default_X_PB() const { + return defaults_.X_PB; +} + +void RgbdSensorAsync::set_default_X_PB( + const math::RigidTransformd& sensor_pose) { + defaults_.X_PB = sensor_pose; +} + +const math::RigidTransformd& RgbdSensorAsync::GetX_PB( + const Context& context) const { + this->ValidateContext(context); + return GetParameters(context).X_PB; +} + +void RgbdSensorAsync::SetX_PB(Context* context, + const math::RigidTransformd& sensor_pose) const { + this->ValidateContext(context); + GetMutableParameters(context)->X_PB = sensor_pose; +} + +const std::optional& +RgbdSensorAsync::default_color_render_camera() const { + return defaults_.color_camera; +} + +void RgbdSensorAsync::set_default_color_render_camera( + const geometry::render::ColorRenderCamera& color_camera) { + DRAKE_THROW_UNLESS(HasColorCamera()); + defaults_.color_camera = std::move(color_camera); +} + +const std::optional& +RgbdSensorAsync::GetColorRenderCamera(const Context& context) const { + this->ValidateContext(context); + return GetParameters(context).color_camera; +} + +void RgbdSensorAsync::SetColorRenderCamera( + Context* context, + const geometry::render::ColorRenderCamera& color_camera) const { + DRAKE_THROW_UNLESS(HasColorCamera()); + this->ValidateContext(context); + GetMutableParameters(context)->color_camera = color_camera; +} + +const std::optional& +RgbdSensorAsync::default_depth_render_camera() const { + return defaults_.depth_camera; +} + +void RgbdSensorAsync::set_default_depth_render_camera( + const geometry::render::DepthRenderCamera& depth_camera) { + DRAKE_THROW_UNLESS(HasDepthCamera()); + defaults_.depth_camera = std::move(depth_camera); +} + +const std::optional& +RgbdSensorAsync::GetDepthRenderCamera(const Context& context) const { + this->ValidateContext(context); + return GetParameters(context).depth_camera; +} + +void RgbdSensorAsync::SetDepthRenderCamera( + Context* context, + const geometry::render::DepthRenderCamera& depth_camera) const { + DRAKE_THROW_UNLESS(HasDepthCamera()); + this->ValidateContext(context); + GetMutableParameters(context)->depth_camera = depth_camera; } const OutputPort& RgbdSensorAsync::color_image_output_port() const { @@ -307,23 +406,31 @@ RgbdSensorAsync::TickTockState& RgbdSensorAsync::get_mutable_state( return state->template get_mutable_abstract_state(0); } +bool RgbdSensorAsync::HasColorCamera() const { + return defaults_.color_camera.has_value(); +} + +bool RgbdSensorAsync::HasDepthCamera() const { + return defaults_.depth_camera.has_value(); +} + EventStatus RgbdSensorAsync::Initialize(const Context& context, State* state) const { // Grab the downcast reference from our argument. - unused(context); TickTockState& next_state = get_mutable_state(state); + const RgbdSensorAsyncParameters& params = GetParameters(context); // If we are only going to render one of color or depth, invent dummy // properties for the other one to simplify the SnapshotSensor code. - std::optional color = color_camera_; - std::optional depth = depth_camera_; - if (!color.has_value()) { - DRAKE_DEMAND(depth.has_value()); + std::optional color = params.color_camera; + std::optional depth = params.depth_camera; + if (!HasColorCamera()) { + DRAKE_DEMAND(HasDepthCamera()); const RenderCameraCore& core = depth->core(); color.emplace(core); } - if (!depth.has_value()) { - DRAKE_DEMAND(color.has_value()); + if (!HasDepthCamera()) { + DRAKE_DEMAND(HasColorCamera()); const RenderCameraCore& core = color->core(); const ClippingRange& clip = core.clipping(); // N.B. Avoid using clip.far() here; it can trip the "16 bit mm depth" @@ -336,10 +443,11 @@ EventStatus RgbdSensorAsync::Initialize(const Context& context, // job during initialization is to reset the nested system and any prior // output. auto sensor = std::make_shared( - scene_graph_, parent_id_, X_PB_, std::move(*color), std::move(*depth)); - next_state.worker = - std::make_shared(std::move(sensor), color_camera_.has_value(), - depth_camera_.has_value(), render_label_image_); + scene_graph_, params.parent_frame_id, params.X_PB, std::move(*color), + std::move(*depth)); + next_state.worker = std::make_shared( + std::move(sensor), HasColorCamera(), HasDepthCamera(), + render_label_image_, params.version); next_state.output = {}; return EventStatus::Succeeded(); } @@ -357,8 +465,11 @@ void RgbdSensorAsync::CalcTick(const Context& context, // launch a worker task, without any changes to our output. next_state.output = prior_state.output; - // Latch-initialize a worker we didn't have one yet. - if (prior_state.worker == nullptr) { + // Latch-initialize a worker if we didn't have one yet, or if the parameters + // are out of date. + if (prior_state.worker == nullptr || + (prior_state.worker->parameters_version() != + GetParameters(context).version)) { Initialize(context, state); } else { next_state.worker = prior_state.worker; @@ -391,6 +502,18 @@ void RgbdSensorAsync::CalcTock(const Context& context, } namespace { +/* If the size of `image` already matches `camera`, then does nothing. +Otherwise, resizes the `image` to match `camera`. */ +template +void Resize(const RenderCameraCore& camera, SomeImage* image) { + const int width = camera.intrinsics().width(); + const int height = camera.intrinsics().height(); + if (image->width() == width && image->height() == height) { + return; + } + image->resize(width, height); +} + /* If the rendered image is non-null, copy it to output. Otherwise, set the the output to be empty (i.e., zero-sized width and height). */ template @@ -410,25 +533,33 @@ void CopyImage(const ImageIn* rendered, ImageOut* output) { void RgbdSensorAsync::CalcColor(const Context& context, ImageRgba8U* output) const { - DRAKE_DEMAND(color_camera_.has_value()); + const std::optional& camera = + GetColorRenderCamera(context); + Resize(camera->core(), output); CopyImage(get_state(context).output.color.get(), output); } void RgbdSensorAsync::CalcLabel(const Context& context, ImageLabel16I* output) const { - DRAKE_DEMAND(color_camera_.has_value()); + const std::optional& camera = + GetColorRenderCamera(context); + Resize(camera->core(), output); CopyImage(get_state(context).output.label.get(), output); } void RgbdSensorAsync::CalcDepth32F(const Context& context, ImageDepth32F* output) const { - DRAKE_DEMAND(depth_camera_.has_value()); + const std::optional& camera = + GetDepthRenderCamera(context); + Resize(camera->core(), output); CopyImage(get_state(context).output.depth.get(), output); } void RgbdSensorAsync::CalcDepth16U(const Context& context, ImageDepth16U* output) const { - DRAKE_DEMAND(depth_camera_.has_value()); + const std::optional& camera = + GetDepthRenderCamera(context); + Resize(camera->core(), output); CopyImage(get_state(context).output.depth.get(), output); } @@ -442,6 +573,28 @@ void RgbdSensorAsync::CalcImageTime(const Context& context, output->SetFromVector(Vector1d{get_state(context).output.time}); } +void RgbdSensorAsync::SetDefaultParameters( + const Context& context, Parameters* parameters) const { + LeafSystem::SetDefaultParameters(context, parameters); + parameters->get_mutable_abstract_parameter(parameter_index_) + .set_value(defaults_); +} + +const RgbdSensorAsyncParameters& RgbdSensorAsync::GetParameters( + const Context& context) const { + return context.get_abstract_parameter(parameter_index_) + .get_value(); +} + +RgbdSensorAsyncParameters* RgbdSensorAsync::GetMutableParameters( + Context* context) const { + auto* params = + &(context->get_mutable_abstract_parameter(parameter_index_) + .template get_mutable_value()); + ++params->version; + return params; +} + namespace { void Worker::Start(double context_time, const QueryObject& query) { diff --git a/systems/sensors/rgbd_sensor_async.h b/systems/sensors/rgbd_sensor_async.h index 27f5517d14d0..ec0b5def57ce 100644 --- a/systems/sensors/rgbd_sensor_async.h +++ b/systems/sensors/rgbd_sensor_async.h @@ -3,6 +3,7 @@ #include #include "drake/common/drake_copyable.h" +#include "drake/common/drake_deprecated.h" #include "drake/geometry/geometry_ids.h" #include "drake/geometry/query_object.h" #include "drake/geometry/render/render_camera.h" @@ -15,6 +16,24 @@ namespace drake { namespace systems { namespace sensors { +namespace internal { + +// TOOD(jwnimmer-tri) Maybe we should make this public at some point? Is it +// helpful for users? +struct RgbdSensorAsyncParameters { + // A modification version number for tracking of changes. + uint64_t version{}; + // The default identifier for the parent frame `P`. + geometry::FrameId parent_frame_id; + // The position of the camera's B frame relative to its parent frame P. + math::RigidTransformd X_PB; + // The camera specification for color & label. + std::optional color_camera; + // The camera specification for depth. + std::optional depth_camera; +}; + +} // namespace internal /** A sensor similar to RgbdSensorDiscrete but the rendering occurs on a background thread to offer improved performance. @@ -79,6 +98,14 @@ associated with rendering. See also RgbdSensorDiscrete for a simpler (unthreaded) discrete sensor model, or RgbdSensor for a continuous model. +@note There are some limits on what configuration can be changed after +construction. It is not supported to change which cameras are present, since +that would imply a change in what output ports are provided. However, it is +possible to change the details of the camera configurations if they were +provided at construction. It is not supported to change the timing variables +(`fps`, `capture_offset`, `output_delay`), since that would require a change in +the event definitions. + @warning As the moment, this implementation cannnot respond to changes to geometry shapes, textures, etc. after a simulation has started. The only thing it responds to are changes to geometry poses. If you change anything beyond @@ -124,35 +151,99 @@ class RgbdSensorAsync final : public LeafSystem { std::optional depth_camera = {}, bool render_label_image = false); - // TODO(#21872): update the APIs for parent id, X_PB, and the cameras to - // align with RgbdSensor. + DRAKE_DEPRECATED("2025-05-01", "Use default_parent_frame_id() instead.") + geometry::FrameId parent_id() const { return default_parent_frame_id(); } - /** Returns the `parent_id` passed to the constructor. */ - geometry::FrameId parent_id() const { return parent_id_; } + DRAKE_DEPRECATED("2025-05-01", "Use default_X_PB() instead.") + const math::RigidTransformd& X_PB() const { return default_X_PB(); } - /** Returns the `X_PB` passed to the constructor. */ - const math::RigidTransformd& X_PB() const { return X_PB_; } + DRAKE_DEPRECATED("2025-05-01", "Use default_color_render_camera() instead.") + const std::optional& color_camera() + const { + return default_color_render_camera(); + } + + DRAKE_DEPRECATED("2025-05-01", "Use default_depth_render_camera() instead.") + const std::optional& depth_camera() + const { + return default_depth_render_camera(); + } - /** Returns the `fps` passed to the constructor. */ + /** Returns the frame rate provided at construction. */ double fps() const { return fps_; } - /** Returns the `capture_offset` passed to the constructor. */ + /** Returns the capture offset provided at construction. */ double capture_offset() const { return capture_offset_; } - /** Returns the `output_delay` passed to the constructor. */ + /** Returns the output delay provided at construction. */ double output_delay() const { return output_delay_; } - /** Returns the `color_camera` passed to the constructor. */ - const std::optional& color_camera() - const { - return color_camera_; - } + /** Returns the default id of the frame to which the body is affixed. */ + geometry::FrameId default_parent_frame_id() const; - /** Returns the `depth_camera` passed to the constructor. */ - const std::optional& depth_camera() - const { - return depth_camera_; - } + /** Sets the default id of the frame to which the body is affixed. */ + void set_default_parent_frame_id(geometry::FrameId id); + + /** Returns the context dependent id of the frame to which the body is + affixed. */ + geometry::FrameId GetParentFrameId(const Context& context) const; + + /** Sets the id of the frame to which the body is affixed, as stored as + parameters in `context`. */ + void SetParentFrameId(Context* context, geometry::FrameId id) const; + + /** Returns the default `X_PB`. */ + const math::RigidTransformd& default_X_PB() const; + + /** Sets the default `X_PB`. */ + void set_default_X_PB(const math::RigidTransformd& sensor_pose); + + /** Returns the context dependent `X_PB`. */ + const math::RigidTransformd& GetX_PB(const Context& context) const; + + /** Sets the `X_PB`, as stored as parameters in `context`. */ + void SetX_PB(Context* context, + const math::RigidTransformd& sensor_pose) const; + + /** Returns the default render camera for color renderings. */ + const std::optional& + default_color_render_camera() const; + + /** Sets the default render camera for color/label renderings. + @throws std::exception if no color camera was provided at construction. */ + void set_default_color_render_camera( + const geometry::render::ColorRenderCamera& color_camera); + + /** Returns the context dependent render camera for color/label renderings. */ + const std::optional& + GetColorRenderCamera(const Context& context) const; + + /** Sets the render camera for color/label renderings, as stored as + parameters in `context`. + @throws std::exception if no color camera was provided at construction. */ + void SetColorRenderCamera( + Context* context, + const geometry::render::ColorRenderCamera& color_camera) const; + + /** Returns the default render camera for depth renderings. */ + const std::optional& + default_depth_render_camera() const; + + /** Sets the default render camera for depth renderings. + @throws std::exception if no depth camera was provided at construction. */ + void set_default_depth_render_camera( + const geometry::render::DepthRenderCamera& depth_camera); + + /** Returns the context dependent render camera for depth renderings. */ + const std::optional& + GetDepthRenderCamera(const Context& context) const; + + /** Sets the render camera for depth renderings, as stored as parameters in + `context`. + @throws std::exception if no depth camera was provided at construction. */ + void SetDepthRenderCamera( + Context* context, + const geometry::render::DepthRenderCamera& depth_camera) const; // TODO(jwnimmer-tri) Add an output port for the timestamp associated with // the other output ports (images, etc.) to make it easier for the user to @@ -190,6 +281,9 @@ class RgbdSensorAsync final : public LeafSystem { const TickTockState& get_state(const Context&) const; TickTockState& get_mutable_state(State*) const; + bool HasColorCamera() const; + bool HasDepthCamera() const; + EventStatus Initialize(const Context&, State*) const; void CalcTick(const Context&, State*) const; void CalcTock(const Context&, State*) const; @@ -200,14 +294,26 @@ class RgbdSensorAsync final : public LeafSystem { void CalcX_WB(const Context&, math::RigidTransformd*) const; void CalcImageTime(const Context&, BasicVector*) const; + // Writes the current default values to the context's parameters. + void SetDefaultParameters(const Context& context, + Parameters* parameters) const override; + const internal::RgbdSensorAsyncParameters& GetParameters( + const Context& context) const; + internal::RgbdSensorAsyncParameters* GetMutableParameters( + Context* context) const; + const geometry::SceneGraph* const scene_graph_; - const geometry::FrameId parent_id_; - const math::RigidTransformd X_PB_; + + // How often to sample the `geometry_query` input and render images. const double fps_; + // The time of the first sample of `geometry_query` input. const double capture_offset_; + // How long after input sampling that the outputs will be updated. const double output_delay_; - const std::optional color_camera_; - const std::optional depth_camera_; + + internal::RgbdSensorAsyncParameters defaults_; + AbstractParameterIndex parameter_index_; + const bool render_label_image_; }; diff --git a/systems/sensors/test/rgbd_sensor_async_test.cc b/systems/sensors/test/rgbd_sensor_async_test.cc index 0f8a7f6a2588..e16f017db6e4 100644 --- a/systems/sensors/test/rgbd_sensor_async_test.cc +++ b/systems/sensors/test/rgbd_sensor_async_test.cc @@ -3,6 +3,7 @@ #include #include +#include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/common/test_utilities/expect_throws_message.h" #include "drake/geometry/test_utilities/dummy_render_engine.h" #include "drake/multibody/plant/multibody_plant.h" @@ -18,15 +19,19 @@ namespace systems { namespace sensors { namespace { +using Eigen::Vector3d; using geometry::FrameId; +using geometry::GeometryFrame; using geometry::GeometryId; using geometry::GeometryInstance; using geometry::PerceptionProperties; using geometry::SceneGraph; +using geometry::SourceId; using geometry::Sphere; using geometry::internal::DummyRenderEngine; using geometry::render::ColorRenderCamera; using geometry::render::DepthRenderCamera; +using geometry::render::RenderCameraCore; using geometry::render::RenderLabel; using math::RigidTransform; using multibody::AddMultibodyPlantSceneGraph; @@ -117,10 +122,18 @@ class RgbdSensorAsyncTest : public ::testing::Test { EXPECT_THAT(image_time(0), testing::IsNan()); } - /* Checks that all four image output ports of the given system are producing a - "clear" image of the correct size, and the output_time is expected_time. */ - void ExpectClearImages(const System& system, - const Context& context, double expected_time) { + /* Checks that all four image output ports of the given system are producing + a "clear" image of the correct size, and the output_time is + expected_time. Note that the expected image size can change based on the + camera configuration in the context. The camera arguments to this function + express the expected camera configurations. */ + void ExpectClearImages( + const System& system, const Context& context, + double expected_time, + std::optional color_camera = + std::nullopt, + std::optional depth_camera = + std::nullopt) { SCOPED_TRACE(fmt::format("... at context.time = {}", context.get_time())); const auto& color_image = @@ -134,13 +147,19 @@ class RgbdSensorAsyncTest : public ::testing::Test { const Eigen::VectorXd& image_time = system.GetOutputPort("image_time").Eval(context); - const auto& color_intrinsics = color_camera_.core().intrinsics(); + if (!color_camera) { + color_camera = color_camera_; + } + const auto& color_intrinsics = color_camera->core().intrinsics(); ASSERT_EQ(color_image.width(), color_intrinsics.width()); ASSERT_EQ(color_image.height(), color_intrinsics.height()); ASSERT_EQ(label_image.width(), color_intrinsics.width()); ASSERT_EQ(label_image.height(), color_intrinsics.height()); - const auto& depth_intrinsics = depth_camera_.core().intrinsics(); + if (!depth_camera) { + depth_camera = depth_camera_; + } + const auto& depth_intrinsics = depth_camera->core().intrinsics(); ASSERT_EQ(depth32_image.width(), depth_intrinsics.width()); ASSERT_EQ(depth32_image.height(), depth_intrinsics.height()); ASSERT_EQ(depth16_image.width(), depth_intrinsics.width()); @@ -179,13 +198,13 @@ TEST_F(RgbdSensorAsyncTest, ConstructorAndSimpleAccessors) { output_delay, color_camera_, depth_camera_, render_label_image); - EXPECT_EQ(dut.parent_id(), parent_id); - EXPECT_EQ(dut.X_PB().GetAsMatrix34(), X_PB.GetAsMatrix34()); + EXPECT_EQ(dut.default_parent_frame_id(), parent_id); + EXPECT_EQ(dut.default_X_PB().GetAsMatrix34(), X_PB.GetAsMatrix34()); EXPECT_EQ(dut.fps(), fps); EXPECT_EQ(dut.capture_offset(), capture_offset); EXPECT_EQ(dut.output_delay(), output_delay); - EXPECT_TRUE(dut.color_camera().has_value()); - EXPECT_TRUE(dut.depth_camera().has_value()); + EXPECT_TRUE(dut.default_color_render_camera().has_value()); + EXPECT_TRUE(dut.default_depth_render_camera().has_value()); EXPECT_EQ(dut.get_input_port().get_name(), "geometry_query"); EXPECT_EQ(dut.color_image_output_port().get_name(), "color_image"); @@ -199,7 +218,147 @@ TEST_F(RgbdSensorAsyncTest, ConstructorAndSimpleAccessors) { simulator.Initialize(); } -// Confirm that we fail-fast when geometry changes post-initialze. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +TEST_F(RgbdSensorAsyncTest, DeprecatedMethods) { + SceneGraph scene_graph; + const FrameId parent_id = SceneGraph::world_frame_id(); + const RigidTransform X_PB(Eigen::Vector3d(1, 2, 3)); + const double fps = 4; + const double capture_offset = 0.001; + const double output_delay = 0.200; + const bool render_label_image = true; + const RgbdSensorAsync dut(&scene_graph, parent_id, X_PB, fps, capture_offset, + output_delay, color_camera_, depth_camera_, + render_label_image); + + EXPECT_EQ(dut.parent_id(), parent_id); + EXPECT_EQ(dut.X_PB().GetAsMatrix34(), X_PB.GetAsMatrix34()); + EXPECT_TRUE(dut.color_camera().has_value()); + EXPECT_TRUE(dut.depth_camera().has_value()); +} +#pragma GCC diagnostic pop + +TEST_F(RgbdSensorAsyncTest, Parameters) { + SceneGraph scene_graph; + const FrameId parent_id = SceneGraph::world_frame_id(); + const RigidTransform X_PB(Eigen::Vector3d(1, 2, 3)); + /* Only construction params that are systems::Parameters are tested here. */ + RgbdSensorAsync dut(&scene_graph, parent_id, X_PB, /* fps = */ 4, + /* capture_offset = */ 0.001, + /* output_delay = */ 0.2, color_camera_, depth_camera_, + /* render_label_image = */ true); + + // Prepare some replacement value items. + const GeometryFrame frame("some_frame"); + SourceId source_id = scene_graph.RegisterSource("source"); + scene_graph.RegisterFrame(source_id, frame); + // Replacement cameras have arbitrary tiny width and height. + ColorRenderCamera tiny_color_camera( + {kRendererName, + {16, 12, M_PI / 4}, + {0.1, 10.0}, + RigidTransform{Vector3d{1, 0, 0}}}, + false); + DepthRenderCamera tiny_depth_camera( + {kRendererName, + {8, 6, M_PI / 6}, + {0.1, 10.0}, + RigidTransform{Vector3d{-1, 0, 0}}}, + {0.1, 10}); + + // Define some local sugar to extract camera intrinsics width. + auto width = [](const auto& camera) { + return camera->core().intrinsics().width(); + }; + + // Check the default values. + EXPECT_EQ(dut.default_parent_frame_id(), parent_id); + EXPECT_TRUE( + CompareMatrices(dut.default_X_PB().GetAsMatrix4(), X_PB.GetAsMatrix4())); + EXPECT_EQ(width(dut.default_color_render_camera()), width(&color_camera_)); + EXPECT_EQ(width(dut.default_depth_render_camera()), width(&depth_camera_)); + + // Create a context with the constructed default values. + auto context = dut.CreateDefaultContext(); + + // Defaults match the parameters in the context created above. + EXPECT_EQ(dut.default_parent_frame_id(), dut.GetParentFrameId(*context)); + EXPECT_TRUE(CompareMatrices(dut.default_X_PB().GetAsMatrix4(), + dut.GetX_PB(*context).GetAsMatrix4())); + EXPECT_EQ(width(dut.default_color_render_camera()), + width(dut.GetColorRenderCamera(*context))); + EXPECT_EQ(width(dut.default_depth_render_camera()), + width(dut.GetDepthRenderCamera(*context))); + + // Change the default values. + dut.set_default_parent_frame_id(frame.id()); + dut.set_default_X_PB(RigidTransform::Identity()); + dut.set_default_color_render_camera(tiny_color_camera); + dut.set_default_depth_render_camera(tiny_depth_camera); + + // Check the changed default values. + EXPECT_EQ(dut.default_parent_frame_id(), frame.id()); + EXPECT_TRUE( + CompareMatrices(dut.default_X_PB().GetAsMatrix4(), + RigidTransform::Identity().GetAsMatrix4())); + EXPECT_EQ(width(dut.default_color_render_camera()), + width(&tiny_color_camera)); + EXPECT_EQ(width(dut.default_depth_render_camera()), + width(&tiny_depth_camera)); + + // Defaults now differ from the parameters in the context created above. + EXPECT_NE(dut.default_parent_frame_id(), dut.GetParentFrameId(*context)); + EXPECT_FALSE(CompareMatrices(dut.default_X_PB().GetAsMatrix4(), + dut.GetX_PB(*context).GetAsMatrix4())); + EXPECT_NE(width(dut.default_color_render_camera()), + width(dut.GetColorRenderCamera(*context))); + EXPECT_NE(width(dut.default_depth_render_camera()), + width(dut.GetDepthRenderCamera(*context))); + + // Create a new context with the changed default values. + auto other_context = dut.CreateDefaultContext(); + + // The new context matches the changed defaults. + EXPECT_EQ(dut.default_parent_frame_id(), + dut.GetParentFrameId(*other_context)); + EXPECT_TRUE(CompareMatrices(dut.default_X_PB().GetAsMatrix4(), + dut.GetX_PB(*other_context).GetAsMatrix4())); + EXPECT_EQ(width(dut.default_color_render_camera()), + width(dut.GetColorRenderCamera(*other_context))); + EXPECT_EQ(width(dut.default_depth_render_camera()), + width(dut.GetDepthRenderCamera(*other_context))); + + // Push the parameter values from the original context into the new context, + // to test parameter mutating methods. + dut.SetParentFrameId(other_context.get(), dut.GetParentFrameId(*context)); + dut.SetX_PB(other_context.get(), dut.GetX_PB(*context)); + dut.SetColorRenderCamera(other_context.get(), + *dut.GetColorRenderCamera(*context)); + dut.SetDepthRenderCamera(other_context.get(), + *dut.GetDepthRenderCamera(*context)); + + // Now, the new context matches the original values. + EXPECT_EQ(dut.GetParentFrameId(*other_context), parent_id); + EXPECT_TRUE(CompareMatrices(dut.GetX_PB(*other_context).GetAsMatrix4(), + X_PB.GetAsMatrix4())); + EXPECT_EQ(width(dut.GetColorRenderCamera(*other_context)), + width(&color_camera_)); + EXPECT_EQ(width(dut.GetDepthRenderCamera(*other_context)), + width(&depth_camera_)); + + // Now, the new context differs from the changed defaults. + EXPECT_NE(dut.default_parent_frame_id(), + dut.GetParentFrameId(*other_context)); + EXPECT_FALSE(CompareMatrices(dut.default_X_PB().GetAsMatrix4(), + dut.GetX_PB(*other_context).GetAsMatrix4())); + EXPECT_NE(width(dut.default_color_render_camera()), + width(dut.GetColorRenderCamera(*other_context))); + EXPECT_NE(width(dut.default_depth_render_camera()), + width(dut.GetDepthRenderCamera(*other_context))); +} + +// Confirm that we fail-fast when geometry changes post-initialize. TEST_F(RgbdSensorAsyncTest, GeometryVersionFailFast) { // Prepare a full simulation. DiagramBuilder builder; @@ -251,10 +410,10 @@ TEST_F(RgbdSensorAsyncTest, ConstructorColorOnly) { output_delay, color_camera_, std::nullopt, render_label_image); - EXPECT_TRUE(dut.color_camera().has_value()); + EXPECT_TRUE(dut.default_color_render_camera().has_value()); EXPECT_NO_THROW(dut.color_image_output_port()); - EXPECT_FALSE(dut.depth_camera().has_value()); + EXPECT_FALSE(dut.default_depth_render_camera().has_value()); EXPECT_THROW(dut.depth_image_32F_output_port(), std::exception); EXPECT_THROW(dut.depth_image_16U_output_port(), std::exception); EXPECT_THROW(dut.label_image_output_port(), std::exception); @@ -275,11 +434,11 @@ TEST_F(RgbdSensorAsyncTest, ConstructorDepthOnly) { output_delay, std::nullopt, depth_camera_, render_label_image); - EXPECT_TRUE(dut.depth_camera().has_value()); + EXPECT_TRUE(dut.default_depth_render_camera().has_value()); EXPECT_NO_THROW(dut.depth_image_32F_output_port()); EXPECT_NO_THROW(dut.depth_image_16U_output_port()); - EXPECT_FALSE(dut.color_camera().has_value()); + EXPECT_FALSE(dut.default_color_render_camera().has_value()); EXPECT_THROW(dut.color_image_output_port(), std::exception); EXPECT_THROW(dut.label_image_output_port(), std::exception); @@ -412,6 +571,87 @@ TEST_F(RgbdSensorAsyncTest, RenderBackgroundColor) { .succeeded()); } +// Check that processed image sizes match the configured camera parameters in +// the context. The default images returned during initialization are still 0 +// size. +TEST_F(RgbdSensorAsyncTest, ChangeImageSize) { + // Add the plant, scene_graph, and renderer. + DiagramBuilder builder; + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0); + scene_graph.AddRenderer(kRendererName, + std::make_unique()); + + // Add the RgbdSensorAsync and export all of its output ports. + const FrameId parent_id = SceneGraph::world_frame_id(); + const RigidTransform X_PB(Eigen::Vector3d(1, 2, 3)); + const double fps = 4; + const double capture_offset = 0.001; + const double output_delay = 0.200; + const bool render_label_image = true; + const auto* dut = builder.AddSystem( + &scene_graph, parent_id, X_PB, fps, capture_offset, output_delay, + color_camera_, depth_camera_, render_label_image); + builder.Connect(scene_graph.get_query_output_port(), dut->get_input_port()); + for (OutputPortIndex i{0}; i < dut->num_output_ports(); ++i) { + const auto& output_port = dut->get_output_port(i); + builder.ExportOutput(output_port, output_port.get_name()); + } + + // Prepare the simulation. + plant.Finalize(); + Simulator simulator(builder.Build()); + + // Change the image size parameter in the context. + const int new_width = 404; + const int new_height = 202; + const ColorRenderCamera new_color_camera{ + RenderCameraCore(color_camera_.core().renderer_name(), + CameraInfo{new_width, new_height, + color_camera_.core().intrinsics().fov_y()}, + color_camera_.core().clipping(), + color_camera_.core().sensor_pose_in_camera_body())}; + const DepthRenderCamera new_depth_camera{ + RenderCameraCore(depth_camera_.core().renderer_name(), + CameraInfo{new_width, new_height, + depth_camera_.core().intrinsics().fov_y()}, + depth_camera_.core().clipping(), + depth_camera_.core().sensor_pose_in_camera_body()), + depth_camera_.depth_range()}; + auto dut_mutable_context = [&]() { + return &dut->GetMyMutableContextFromRoot(&simulator.get_mutable_context()); + }; + dut->SetColorRenderCamera(dut_mutable_context(), new_color_camera); + dut->SetDepthRenderCamera(dut_mutable_context(), new_depth_camera); + + // Confirm that rendering uses the new image size. The timing sequence of + // simulation is cribbed from the RenderBackgroundColor test. + + // Prior to initialization, the output image is empty. + simulator.get_mutable_context().SetTime(0.100); + ExpectDefaultImages(simulator.get_system(), simulator.get_context()); + + // The output image is still empty as we pass through various initialization + // and update events. + for (double time : {0.101, 0.202, 0.252, 0.450}) { + simulator.AdvanceTo(time); + ExpectDefaultImages(simulator.get_system(), simulator.get_context()); + } + + // After the second tock event, we see the engine's "clear" color, rendered + // at the new sizes. + simulator.AdvanceTo(0.452); + ExpectClearImages(simulator.get_system(), simulator.get_context(), 0.251, + new_color_camera, new_depth_camera); + + // Revert to original camera size; see output size change back. + dut->SetColorRenderCamera(dut_mutable_context(), color_camera_); + dut->SetDepthRenderCamera(dut_mutable_context(), depth_camera_); + + simulator.AdvanceTo(0.702); + ExpectClearImages(simulator.get_system(), simulator.get_context(), 0.501, + color_camera_, depth_camera_); +} + } // namespace } // namespace sensors } // namespace systems