diff --git a/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc b/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc index ff0f28bebf5b..5855b57b3e7a 100644 --- a/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc +++ b/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc @@ -25,7 +25,9 @@ using geometry::GeometryInstance; using geometry::IllustrationProperties; using geometry::MakePhongIllustrationProperties; using geometry::Mesh; +using geometry::PerceptionProperties; using geometry::ProximityProperties; +using geometry::render::RenderLabel; using geometry::SceneGraph; using geometry::Shape; using geometry::Sphere; @@ -111,6 +113,17 @@ RigidBodyPlantBridge::rigid_body_plant_state_input_port() const { return this->get_input_port(plant_state_port_); } +template +int RigidBodyPlantBridge::BodyForLabel(RenderLabel label) const { + if (label <= RenderLabel::kMaxUnreserved) { + return label_to_index_.at(label); + } else if (label == RenderLabel::kDontCare) { + return 0; // world index. + } else { + return -1; + } +} + template void RigidBodyPlantBridge::RegisterTree(SceneGraph* scene_graph) { // TODO(SeanCurtis-TRI): This treats all bodies in the tree as dynamic. Some @@ -135,11 +148,20 @@ void RigidBodyPlantBridge::RegisterTree(SceneGraph* scene_graph) { // Default to the world body configuration. FrameId body_id = scene_graph->world_frame_id(); + RenderLabel label; if (body.get_body_index() != tree_->world().get_body_index()) { // All other bodies register a frame and (possibly) get a unique label. body_id = scene_graph->RegisterFrame( source_id_, GeometryFrame(body.get_name(), body.get_model_instance_id())); + + if (body.get_visual_elements().size() > 0) { + // We'll have the render label map to the body index. + // NOTE: This is only valid if the RBT is the only source of geometry. + // But given that the RBT is on the way out, why not? + label = RenderLabel(static_cast(body.get_body_index())); + label_to_index_[label] = body.get_body_index(); + } } body_ids_.push_back(body_id); @@ -160,6 +182,12 @@ void RigidBodyPlantBridge::RegisterTree(SceneGraph* scene_graph) { const Vector4& diffuse = visual_element.getMaterial(); scene_graph->AssignRole(source_id_, id, MakePhongIllustrationProperties(diffuse)); + + // Perception properties -- diffuse color and per-body label. + PerceptionProperties perception; + perception.AddProperty("phong", "diffuse", diffuse); + perception.AddProperty("label", "id", label); + scene_graph->AssignRole(source_id_, id, perception); } } int collision_count = 0; diff --git a/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.h b/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.h index ef4f8fdb8fda..a1be86723477 100644 --- a/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.h +++ b/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.h @@ -5,6 +5,7 @@ #include "drake/common/drake_copyable.h" #include "drake/geometry/frame_kinematics_vector.h" #include "drake/geometry/geometry_ids.h" +#include "drake/geometry/render/render_label.h" #include "drake/geometry/scene_graph.h" #include "drake/multibody/rigid_body_tree.h" #include "drake/systems/framework/context.h" @@ -120,6 +121,18 @@ class RigidBodyPlantBridge : public systems::LeafSystem { const systems::InputPort& rigid_body_plant_state_input_port() const; + /** Given a render label, reports the index of the RigidBody to which this + label was assigned. The RenderLabel::kDontCare label maps to the index of the + world body. The other reserved labels return a negative value. + + @throws std::logic_error If the label doesn't otherwise specify a body. */ + int BodyForLabel(geometry::render::RenderLabel label) const; + + /** Reports the frame id for the given body. */ + geometry::FrameId FrameIdFromBody(const RigidBody& body) const { + return body_ids_[body.get_body_index()]; + } + private: // Registers `this` system's tree's bodies and geometries to the given // geometry system. @@ -139,6 +152,10 @@ class RigidBodyPlantBridge : public systems::LeafSystem { int geometry_pose_port_{-1}; int plant_state_port_{-1}; + // Maps from a generated render label to the index of the rigid body assigned + // that label. + std::unordered_map label_to_index_; + // Registered frames. In this incarnation, body i's frame_id is stored in // element i. This is because *all* frames are currently being registered // (regardless of weldedness or whether it has geometry). diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index 16c11274f7a1..5e5d50c12f9f 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -348,6 +348,7 @@ drake_cc_googletest( ":geometry_visualization", ":scene_graph", "//common/test_utilities:expect_throws_message", + "//geometry/test_utilities:dummy_render_engine", ], ) diff --git a/geometry/geometry_state.cc b/geometry/geometry_state.cc index f2b01a01d24f..9b7c54f4c092 100644 --- a/geometry/geometry_state.cc +++ b/geometry/geometry_state.cc @@ -32,6 +32,9 @@ using std::make_unique; using std::move; using std::swap; using std::to_string; +using systems::sensors::ImageDepth32F; +using systems::sensors::ImageLabel16I; +using systems::sensors::ImageRgba8U; //----------------------------------------------------------------------------- @@ -921,6 +924,58 @@ void GeometryState::AddRenderer( render_engines_[move(name)] = move(renderer); } +template +std::vector GeometryState::RegisteredRendererNames() const { + std::vector names; + names.reserve(render_engines_.size()); + for (const auto& name_engine_pair : render_engines_) { + names.push_back(name_engine_pair.first); + } + return names; +} + +template +void GeometryState::RenderColorImage(const render::CameraProperties& camera, + FrameId parent_frame, + const RigidTransformd& X_PC, + bool show_window, + ImageRgba8U* color_image_out) const { + const RigidTransformd X_WC = GetDoubleWorldPose(parent_frame) * X_PC; + const render::RenderEngine& engine = + GetRenderEngineOrThrow(camera.renderer_name); + // TODO(SeanCurtis-TRI): Invoke UpdateViewpoint() as part of a calc cache + // entry. Challenge: how to do that with a parameter passed here? + const_cast(engine).UpdateViewpoint(X_WC); + engine.RenderColorImage(camera, show_window, color_image_out); +} + +template +void GeometryState::RenderDepthImage( + const render::DepthCameraProperties& camera, + FrameId parent_frame, const RigidTransformd& X_PC, + ImageDepth32F* depth_image_out) const { + const RigidTransformd X_WC = GetDoubleWorldPose(parent_frame) * X_PC; + const render::RenderEngine& engine = + GetRenderEngineOrThrow(camera.renderer_name); + // See note in RenderColorImage() about this const cast. + const_cast(engine).UpdateViewpoint(X_WC); + engine.RenderDepthImage(camera, depth_image_out); +} + +template +void GeometryState::RenderLabelImage(const render::CameraProperties& camera, + FrameId parent_frame, + const RigidTransformd& X_PC, + bool show_window, + ImageLabel16I* label_image_out) const { + const RigidTransformd X_WC = GetDoubleWorldPose(parent_frame) * X_PC; + const render::RenderEngine& engine = + GetRenderEngineOrThrow(camera.renderer_name); + // See note in RenderColorImage() about this const cast. + const_cast(engine).UpdateViewpoint(X_WC); + engine.RenderLabelImage(camera, show_window, label_image_out); +} + template std::unique_ptr> GeometryState::ToAutoDiffXd() const { @@ -1364,6 +1419,15 @@ const render::RenderEngine& GeometryState::GetRenderEngineOrThrow( fmt::format("No renderer exists with name: '{}'", renderer_name)); } +template +RigidTransformd GeometryState::GetDoubleWorldPose(FrameId frame_id) const { + if (frame_id == InternalFrame::world_frame_id()) { + return RigidTransformd::Identity(); + } + const internal::InternalFrame& frame = GetValueOrThrow(frame_id, frames_); + return RigidTransformd(internal::convert_to_double(X_WF_[frame.index()])); +} + } // namespace geometry } // namespace drake diff --git a/geometry/geometry_state.h b/geometry/geometry_state.h index bd5fd4fe660a..f333f62188c6 100644 --- a/geometry/geometry_state.h +++ b/geometry/geometry_state.h @@ -48,6 +48,9 @@ using FrameIdSet = std::unordered_set; //@} +// TODO(SeanCurtis-TRI): Move GeometryState into `internal` namespace (and then +// I can kill the `@note` in the class documentation). + /** The context-dependent state of SceneGraph. This serves as an AbstractValue in the context. SceneGraph's time-dependent state includes more than just @@ -55,6 +58,8 @@ using FrameIdSet = std::unordered_set; SceneGraph's context-dependent state includes values (the poses) and structure (the topology of the world). + @note This is intended as an internal class only. + @tparam T The scalar type. Must be a valid Eigen scalar. Instantiated templates for the following kinds of T's are provided: @@ -357,15 +362,34 @@ class GeometryState { "SceneGraph::RemoveRole()". */ int RemoveRole(SourceId source_id, GeometryId geometry_id, Role role); - /** Implementation of - @ref SceneGraph::RemoveFromRenderer(const std::string&, SourceId, FrameId) - "SceneGraph::RemoveFromRenderer()". */ + // TODO(SeanCurtis-TRI): These two "RemoveFromRenderer()" methods are not + // currently exposed in the SceneGraph API. They've been left here so that + // they (and their unit tests) don't have to be recreated. I need to + // determine definitively if these methods *should* exist (in which case I + // put them in the SceneGraph API or not, in which case I can remove them + // entirely). + + /** For every geometry directly registered to the frame with the given + `frame_id`, if it has been added to the renderer with the given + `renderer_name` it is removed from that renderer. + @return The number of geometries affected by the removal. + @throws std::logic_error if 1) `source_id` does not map to a registered + source, 2) `frame_id` does not map to a registered + frame, 3) `frame_id` does not belong to + `source_id` (unless `frame_id` is the world frame + id), or 4) the context has already been + allocated. */ int RemoveFromRenderer(const std::string& renderer_name, SourceId source_id, FrameId frame_id); - /** Implementation of - @ref SceneGraph::RemoveFromRenderer(const std::string&, SourceId, GeometryId) - "SceneGraph::RemoveFromRenderer()". */ + /** Removes the geometry with the given `geometry_id` from the renderer with + the given `renderer_name`, _if_ it has previously been added. + @return The number of geometries affected by the removal (0 or 1). + @throws std::logic_error if 1) `source_id` does not map to a registered + source, 2) `geometry_id` does not map to a + registered geometry, 3) `geometry_id` does not + belong to `source_id`, or 4) the context has already + been allocated. */ int RemoveFromRenderer(const std::string& renderer_name, SourceId source_id, GeometryId geometry_id); @@ -445,10 +469,50 @@ class GeometryState { //@} + //--------------------------------------------------------------------------- + /** @name Render Queries */ + //@{ + /** Implementation support for SceneGraph::AddRenderer(). */ void AddRenderer(std::string name, std::unique_ptr renderer); + /** Implementation support for SceneGraph::HasRenderer(). */ + bool HasRenderer(const std::string& name) const { + return render_engines_.count(name) > 0; + } + + /** Implementation support for SceneGraph::RendererCount(). */ + int RendererCount() const { return static_cast(render_engines_.size()); } + + /** Implementation support for SceneGraph::RegisteredRendererNames(). */ + std::vector RegisteredRendererNames() const; + + /** Implementation support for QueryObject::RenderColorImage(). + @pre All poses have already been updated. */ + void RenderColorImage(const render::CameraProperties& camera, + FrameId parent_frame, + const math::RigidTransformd& X_PC, + bool show_window, + systems::sensors::ImageRgba8U* color_image_out) const; + + /** Implementation support for QueryObject::RenderDepthImage(). + @pre All poses have already been updated. */ + void RenderDepthImage(const render::DepthCameraProperties& camera, + FrameId parent_frame, + const math::RigidTransformd& X_PC, + systems::sensors::ImageDepth32F* depth_image_out) const; + + /** Implementation support for QueryObject::RenderLabelImage(). + @pre All poses have already been updated. */ + void RenderLabelImage(const render::CameraProperties& camera, + FrameId parent_frame, + const math::RigidTransformd& X_PC, + bool show_window, + systems::sensors::ImageLabel16I* label_image_out) const; + + //@} + /** @name Scalar conversion */ //@{ @@ -658,6 +722,10 @@ class GeometryState { const render::RenderEngine& GetRenderEngineOrThrow( const std::string& renderer_name) const; + // Utility function to facilitate getting a double-valued pose for a frame, + // regardless of T's actual type. + math::RigidTransformd GetDoubleWorldPose(FrameId frame_id) const; + // NOTE: If adding a member it is important that it be _explicitly_ copied // in the converting copy constructor and likewise tested in the unit test // for that constructor. diff --git a/geometry/query_object.cc b/geometry/query_object.cc index 0b45e3c54e44..1c70c57e5c39 100644 --- a/geometry/query_object.cc +++ b/geometry/query_object.cc @@ -8,6 +8,12 @@ namespace drake { namespace geometry { using math::RigidTransform; +using math::RigidTransformd; +using render::CameraProperties; +using render::DepthCameraProperties; +using systems::sensors::ImageDepth32F; +using systems::sensors::ImageLabel16I; +using systems::sensors::ImageRgba8U; template QueryObject::QueryObject(const QueryObject& query_object) { @@ -66,25 +72,23 @@ RigidTransform QueryObject::X_WG(GeometryId id) const { } template -std::vector> -QueryObject::ComputeContactSurfaces() const { +std::vector> +QueryObject::ComputePointPairPenetration() const { ThrowIfNotCallable(); - // TODO(DamrongGuoy): Modify this when the cache system is in place. FullPoseUpdate(); const GeometryState& state = geometry_state(); - return state.ComputeContactSurfaces(); + return state.ComputePointPairPenetration(); } template -std::vector> -QueryObject::ComputePointPairPenetration() const { +std::vector> +QueryObject::ComputeContactSurfaces() const { ThrowIfNotCallable(); - // TODO(SeanCurtis-TRI): Modify this when the cache system is in place. FullPoseUpdate(); const GeometryState& state = geometry_state(); - return state.ComputePointPairPenetration(); + return state.ComputeContactSurfaces(); } template @@ -93,7 +97,6 @@ QueryObject::ComputeSignedDistancePairwiseClosestPoints( const double max_distance) const { ThrowIfNotCallable(); - // TODO(SeanCurtis-TRI): Modify this when the cache system is in place. FullPoseUpdate(); const GeometryState& state = geometry_state(); return state.ComputeSignedDistancePairwiseClosestPoints(max_distance); @@ -111,6 +114,46 @@ QueryObject::ComputeSignedDistanceToPoint( return state.ComputeSignedDistanceToPoint(p_WQ, threshold); } +template +void QueryObject::RenderColorImage(const CameraProperties& camera, + FrameId parent_frame, + const RigidTransformd& X_PC, + bool show_window, + ImageRgba8U* color_image_out) const { + ThrowIfNotCallable(); + + FullPoseUpdate(); + const GeometryState& state = geometry_state(); + return state.RenderColorImage(camera, parent_frame, X_PC, show_window, + color_image_out); +} + +template +void QueryObject::RenderDepthImage(const DepthCameraProperties& camera, + FrameId parent_frame, + const RigidTransformd& X_PC, + ImageDepth32F* depth_image_out) const { + ThrowIfNotCallable(); + + FullPoseUpdate(); + const GeometryState& state = geometry_state(); + return state.RenderDepthImage(camera, parent_frame, X_PC, depth_image_out); +} + +template +void QueryObject::RenderLabelImage(const CameraProperties& camera, + FrameId parent_frame, + const RigidTransformd& X_PC, + bool show_window, + ImageLabel16I* label_image_out) const { + ThrowIfNotCallable(); + + FullPoseUpdate(); + const GeometryState& state = geometry_state(); + return state.RenderLabelImage(camera, parent_frame, X_PC, show_window, + label_image_out); +} + template const GeometryState& QueryObject::geometry_state() const { // Some extra insurance in case some query *hadn't* called this. diff --git a/geometry/query_object.h b/geometry/query_object.h index d5d24254eb59..6b017b7240da 100644 --- a/geometry/query_object.h +++ b/geometry/query_object.h @@ -10,6 +10,7 @@ #include "drake/geometry/query_results/signed_distance_pair.h" #include "drake/geometry/query_results/signed_distance_to_point.h" #include "drake/geometry/scene_graph_inspector.h" +#include "drake/math/rigid_transform.h" #include "drake/systems/framework/context.h" namespace drake { @@ -347,6 +348,68 @@ class QueryObject { = std::numeric_limits::infinity()) const; //@} + + //--------------------------------------------------------------------------- + /** @name Render Queries + + The methods support queries along the lines of "What do I see?" They support + simulation of sensors. External entities define a sensor camera -- its + extrinsic and intrinsic properties and %QueryObject renders into the + provided image. + + + */ + //@{ + + /** Renders an RGB image for the given `camera` posed with respect to the + indicated parent frame P. + + @param camera The intrinsic properties of the camera. + @param parent_frame The id for the camera's parent frame. + @param X_PC The pose of the camera body in the world frame. + @param show_window If true, the render window will be displayed. + @param[out] color_image_out The rendered color image. */ + void RenderColorImage(const render::CameraProperties& camera, + FrameId parent_frame, + const math::RigidTransformd& X_PC, + bool show_window, + systems::sensors::ImageRgba8U* color_image_out) const; + + /** Renders a depth image for the given `camera` posed with respect to the + indicated parent frame P. + + In contrast to the other rendering methods, rendering depth images doesn't + provide the option to display the window; generally, basic depth images are + not readily communicative to humans. + + @param camera The intrinsic properties of the camera. + @param parent_frame The id for the camera's parent frame. + @param X_PC The pose of the camera body in the world frame. + @param[out] depth_image_out The rendered depth image. */ + void RenderDepthImage(const render::DepthCameraProperties& camera, + FrameId parent_frame, + const math::RigidTransformd& X_PC, + systems::sensors::ImageDepth32F* depth_image_out) const; + + /** Renders a label image for the given `camera` posed with respect to the + indicated parent frame P. + + @param camera The intrinsic properties of the camera. + @param parent_frame The id for the camera's parent frame. + @param X_PC The pose of the camera body in the world frame. + @param show_window If true, the render window will be displayed. + @param[out] label_image_out The rendered label image. */ + void RenderLabelImage(const render::CameraProperties& camera, + FrameId parent_frame, + const math::RigidTransformd& X_PC, + bool show_window, + systems::sensors::ImageLabel16I* label_image_out) const; + + //@} + private: // SceneGraph is the only class that may call set(). friend class SceneGraph; @@ -372,6 +435,7 @@ class QueryObject { // Update all poses. This method does no work if this is a "baked" query // object (see class docs for discussion). void FullPoseUpdate() const { + // TODO(SeanCurtis-TRI): Modify this when the cache system is in place. if (scene_graph_) scene_graph_->FullPoseUpdate(*context_); } diff --git a/geometry/render/BUILD.bazel b/geometry/render/BUILD.bazel index 5f7a23d65c38..87cf8704437b 100644 --- a/geometry/render/BUILD.bazel +++ b/geometry/render/BUILD.bazel @@ -17,7 +17,6 @@ drake_cc_package_library( ":render_engine_vtk", ":render_label", ":render_label_class", - ":render_label_manager", ], ) @@ -97,17 +96,6 @@ drake_cc_library( ], ) -drake_cc_library( - name = "render_label_manager", - srcs = ["render_label_manager.cc"], - hdrs = ["render_label_manager.h"], - deps = [ - ":render_label", - ":render_label_class", - "@fmt", - ], -) - # === test/ === drake_cc_googletest( @@ -139,7 +127,6 @@ drake_cc_googletest( ], deps = [ ":render_engine_vtk", - ":render_label_manager", "//common:find_resource", "//common/test_utilities:eigen_matrix_compare", "//common/test_utilities:expect_throws_message", @@ -156,13 +143,4 @@ drake_cc_googletest( ], ) -drake_cc_googletest( - name = "render_label_manager_test", - deps = [ - ":render_label_manager", - "//common:value", - "//common/test_utilities", - ], -) - add_lint_tests() diff --git a/geometry/render/render_label.h b/geometry/render/render_label.h index e6b5bd9f9430..9bc56dd5de41 100644 --- a/geometry/render/render_label.h +++ b/geometry/render/render_label.h @@ -54,36 +54,14 @@ namespace render {

Usage

- For a label image to be _meaningful_, every pixel value should admit an - unambiguous interpretation. To do that, %RenderLabels need to be coordinated - to avoid accidental overloading. An application can achieve this in one of two - ways: the application can rely on SceneGraph to allocate and coordinate - %RenderLabel values across multiple geometry sources or the application can - manage its own %RenderLabel values. Mixing the two strategies is highly - inadvisable; the responsibility for guaranteeing unique %RenderLabel values - does not survive sharing well. - -

Allocation of %RenderLabel values from SceneGraph

- - An application defines a semantic class with a name (e.g., "car", "robot", - "table", etc.) and associated source id -- if two sources were both to define - their own semantic class named "robot", they would be considered distinct - classes by virtue of their different source ids. - - The application associates a %RenderLabel with its semantic class by - invoking SceneGraph::GetRenderLabel(). SceneGraph maintains a mapping between - %RenderLabel and all registered semantic classes. They can be queried via - SceneGraphInspector::GetSemanticClassNameFromLabel(). - -

Manual %RenderLabel allocation

- An application can simply instantiate %RenderLabel with an arbitrary value. This allows the application to define a particular mapping from render label - class to a preferred %RenderLabel value. The application bears _full_ - responsibility in making sure that a single value is not inadvertently - associated with multiple render classes. Finally, a %RenderLabel cannot be - explicitly constructed with a reserved value -- those can only be accessed - through the static methods provided. + class to a preferred %RenderLabel value. For a label image to be _meaningful_, + every pixel value should admit an unambiguous interpretation. The application + bears _full_ responsibility in making sure that a single value is not + inadvertently associated with multiple render classes. Finally, a %RenderLabel + cannot be explicitly constructed with a reserved value -- those can only be + accessed through the static methods provided. @note The %RenderLabel class is based on a 16-bit integer. This makes the label image more compact but means there are only, approximately, 32,000 unique diff --git a/geometry/render/render_label_manager.cc b/geometry/render/render_label_manager.cc deleted file mode 100644 index 03706d20a120..000000000000 --- a/geometry/render/render_label_manager.cc +++ /dev/null @@ -1,54 +0,0 @@ -#include "drake/geometry/render/render_label_manager.h" - -#include - -namespace drake { -namespace geometry { -namespace render { -namespace internal { - -using std::move; -using std::pair; -using std::string; -using std::unordered_multimap; -using std::vector; - -RenderLabelManager::RenderLabelManager(SourceId id) - : maximum_value_(RenderLabel::kMaxUnreserved) { - // The reserved labels all belong to the provided source id. - name_label_map_.insert( - {"empty", RenderLabelClass("empty", id, RenderLabel::kEmpty)}); - name_label_map_.insert( - {"do_not_render", - RenderLabelClass("do_not_render", id, RenderLabel::kDoNotRender)}); - name_label_map_.insert( - {"dont_care", RenderLabelClass("dont_care", id, RenderLabel::kDontCare)}); - name_label_map_.insert( - {"unspecified", - RenderLabelClass("unspecified", id, RenderLabel::kUnspecified)}); -} - -RenderLabel RenderLabelManager::GetRenderLabel(SourceId source_id, - string class_name) { - // Check to see if this name has been used by the source id already. - auto range = name_label_map_.equal_range(class_name); - for (auto it = range.first; it != range.second; ++it) { - if (it->second.source_id == source_id) return it->second.label; - } - - // It hasn't been used; create a label and record usage. - if (next_value_ <= maximum_value_) { - RenderLabel label(next_value_++); - name_label_map_.insert( - {class_name, RenderLabelClass(class_name, source_id, label)}); - return label; - } else { - throw std::logic_error(fmt::format( - "All {} render labels have been allocated", maximum_value_)); - } -} - -} // namespace internal -} // namespace render -} // namespace geometry -} // namespace drake diff --git a/geometry/render/render_label_manager.h b/geometry/render/render_label_manager.h deleted file mode 100644 index 276e3b72ecc4..000000000000 --- a/geometry/render/render_label_manager.h +++ /dev/null @@ -1,75 +0,0 @@ -#pragma once - -// Exclude internal class from doxygen's view. -#if !defined(DRAKE_DOXYGEN_CXX) - -#include -#include -#include -#include - -#include "drake/geometry/geometry_ids.h" -#include "drake/geometry/render/render_label.h" -#include "drake/geometry/render/render_label_class.h" - -namespace drake { -namespace geometry { -namespace render { -namespace internal { - -/** The class responsible for allocating render labels and tracking those that - have been allocated by SceneGraph. - - A new RenderLabel is allocated for every unique semantic class (name and source - id pair). If GetRenderLabel() is called multiple times with the same name and - source id, the same RenderLabel value will be returned each time. - - The %RenderLabelManager is copyable to facilitate SceneGraph cloning. Each copy - contains the same record of allocated labels up to the point of copying, but - subsequent allocations will be independent. - - @see RenderLabel */ -class RenderLabelManager { - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RenderLabelManager) - - /** Constructs the manager attributing ownership of the reserved semantic - classes to the source with the given `id` (expected to be the id of the - owning SceneGraph). - */ - explicit RenderLabelManager(SourceId id); - - /** Implementation of SceneGraph::GetRenderLabel(). */ - RenderLabel GetRenderLabel(SourceId source_id, std::string class_name); - - /** Returns the record of all allocated render labels and their semantic - classes. It is represented as a map from the _semantic name_ to the semantic - class's data. For semantic names that have been used by more than one source, - the semantic name will map to multiple semantic classes, distinguished by - their source id and RenderLabel value. */ - const std::unordered_multimap& - render_label_classes() const { return name_label_map_; } - - private: - friend class RenderLabelManagerTester; - - // A map from the name of the label to each of the semantic classes associated - // with that name (they *must* all have different source ids). - std::unordered_multimap name_label_map_; - - using ValueType = RenderLabel::ValueType; - - // The next value to provide when allocating a RenderLabel. - ValueType next_value_{0}; - - // The maximum allowable render label value. This is mostly a convenience - // lever to test logic. - ValueType maximum_value_{RenderLabel::kMaxUnreserved}; -}; - -} // namespace internal -} // namespace render -} // namespace geometry -} // namespace drake - -#endif // DRAKE_DOXYGEN_CXX diff --git a/geometry/render/test/render_engine_vtk_test.cc b/geometry/render/test/render_engine_vtk_test.cc index be256015d6fa..4a82eca83165 100644 --- a/geometry/render/test/render_engine_vtk_test.cc +++ b/geometry/render/test/render_engine_vtk_test.cc @@ -13,7 +13,6 @@ #include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/common/test_utilities/expect_throws_message.h" #include "drake/geometry/render/camera_properties.h" -#include "drake/geometry/render/render_label_manager.h" #include "drake/geometry/shape_specification.h" #include "drake/geometry/test_utilities/dummy_render_engine.h" #include "drake/math/rigid_transform.h" diff --git a/geometry/render/test/render_label_manager_test.cc b/geometry/render/test/render_label_manager_test.cc deleted file mode 100644 index 82a04060e034..000000000000 --- a/geometry/render/test/render_label_manager_test.cc +++ /dev/null @@ -1,184 +0,0 @@ -#include "drake/geometry/render/render_label_manager.h" - -#include -#include - -#include - -#include "drake/common/test_utilities/expect_throws_message.h" -#include "drake/common/value.h" - -namespace drake { -namespace geometry { -namespace render { -namespace internal { - -// Simple class with friend access to RenderLabelManager for testing the -// internal state in tests. -class RenderLabelManagerTester { - public: - RenderLabelManagerTester() = default; - - // Allocates a label manager with the source id but limits the allocatable - // number of labels to the given `max_value`. - RenderLabelManager make_manager(SourceId source_id, - RenderLabel::ValueType max_value) { - RenderLabelManager manager(source_id); - manager.maximum_value_ = max_value; - return manager; - } - - // Reports the manager's maximum valid value. - RenderLabel::ValueType maximum_value( - const RenderLabelManager& manager) const { - return manager.maximum_value_; - } - - // Consistency check; confirms that every render label associated with a name - // *stores* that name. - ::testing::AssertionResult HasConsistentNames( - const RenderLabelManager& manager) const { - for (auto it = manager.name_label_map_.begin(); - it != manager.name_label_map_.end(); ++it) { - const std::string& label_name = it->first; - const RenderLabelClass& label_class = it->second; - if (label_name != label_class.name) { - return ::testing::AssertionFailure() - << "Render label class has name '" << label_class.name - << "' stored in the collection named '" << label_name << "'"; - } - } - return ::testing::AssertionSuccess(); - } - - // Confirms that the given render label class is in the manager. - ::testing::AssertionResult HasRenderLabelClass( - const RenderLabelManager& manager, - const RenderLabelClass label_class) const { - if (manager.name_label_map_.count(label_class.name) == 0) { - return ::testing::AssertionFailure() - << "No labels allocated with the name '" << label_class.name - << "'"; - } - auto range = manager.name_label_map_.equal_range(label_class.name); - for (auto it = range.first; it != range.second; ++it) { - const RenderLabelClass& test_class = it->second; - if (test_class.source_id == label_class.source_id) { - if (test_class.label == label_class.label) { - // We don't need to check names; we'll already have tested for name - // consistency. - return ::testing::AssertionSuccess(); - } else { - return ::testing::AssertionFailure() - << "Found render label class with the name '" - << label_class.name << "' and matching source id " - << label_class.source_id - << ", but the label value doesn't match. Expected " - << label_class.label << ", found " << test_class.label; - } - } - } - return ::testing::AssertionFailure() - << "Unable to find a render label class with name '" - << label_class.name << "' and source id " << label_class.source_id; - } - - // Reports the number of labels registered in `manager`. - int LabelCount(const RenderLabelManager& manager) const { - return static_cast(manager.name_label_map_.size()); - } - - // Confirms that the reserved labels are in the manager (and recorded - // correctly). In the case of multiple points of failure, only the first - // detected is reported. - ::testing::AssertionResult HasReservedLabels( - const RenderLabelManager& manager, SourceId source_id) const { - ::testing::AssertionResult result = HasRenderLabelClass( - manager, RenderLabelClass("empty", source_id, RenderLabel::kEmpty)); - if (result) { - result = HasRenderLabelClass( - manager, RenderLabelClass("do_not_render", source_id, - RenderLabel::kDoNotRender)); - if (result) { - result = HasRenderLabelClass( - manager, RenderLabelClass("unspecified", source_id, - RenderLabel::kUnspecified)); - if (result) { - result = HasRenderLabelClass( - manager, - RenderLabelClass("dont_care", source_id, RenderLabel::kDontCare)); - } - } - } - return result; - } -}; - -namespace { - -using ValueType = RenderLabel::ValueType; - -// Simple test confirming that the construction is consistent and coherent. -GTEST_TEST(RenderLabelManagerTest, Constructor) { - RenderLabelManagerTester tester; - SourceId source_id = SourceId::get_new_id(); - RenderLabelManager manager(source_id); - EXPECT_EQ(4, tester.LabelCount(manager)); - EXPECT_TRUE(tester.HasConsistentNames(manager)); - EXPECT_TRUE(tester.HasReservedLabels(manager, source_id)); - EXPECT_EQ(RenderLabel::kMaxUnreserved, tester.maximum_value(manager)); - - // Copy constructor. - RenderLabelManager copy(manager); - EXPECT_EQ(4, tester.LabelCount(copy)); - EXPECT_TRUE(tester.HasConsistentNames(copy)); - EXPECT_TRUE(tester.HasReservedLabels(copy, source_id)); - EXPECT_EQ(RenderLabel::kMaxUnreserved, tester.maximum_value(copy)); -} - -GTEST_TEST(RenderLabelManagerTest, GetRenderLabel) { - RenderLabelManagerTester tester; - SourceId source_id = SourceId::get_new_id(); - RenderLabelManager manager(source_id); - - // Case: Request with new source (which means implicitly new name) --> - // produces new label. - SourceId new_source = SourceId::get_new_id(); - RenderLabel label1; - EXPECT_NO_THROW({ label1 = manager.GetRenderLabel(new_source, "case1"); }); - EXPECT_TRUE(tester.HasConsistentNames(manager)); - - // Case: Request with previously used source and new name --> produces new - // label. - RenderLabel label2; - EXPECT_NO_THROW({ label2 = manager.GetRenderLabel(new_source, "case2"); }); - EXPECT_TRUE(tester.HasConsistentNames(manager)); - EXPECT_NE(label1, label2); - - // Case: Request with previously used source and name --> produces previous - // label. - RenderLabel label3; - EXPECT_NO_THROW({ label3 = manager.GetRenderLabel(new_source, "case2"); }); - EXPECT_TRUE(tester.HasConsistentNames(manager)); - EXPECT_EQ(label2, label3); - - // Case: Exhaust labels. - // Confirm ability to set maximum value works to facilitate label exhaustion. - const ValueType max_value = 5; - RenderLabelManager small_manager = tester.make_manager(source_id, max_value); - EXPECT_EQ(max_value, tester.maximum_value(small_manager)); - - for (int i = 0; i <= max_value; ++i) { - small_manager.GetRenderLabel(new_source, std::to_string(i)); - EXPECT_TRUE(tester.HasConsistentNames(small_manager)); - } - DRAKE_EXPECT_THROWS_MESSAGE( - small_manager.GetRenderLabel(new_source, "too many"), std::logic_error, - "All \\d+ render labels have been allocated"); -} - -} // namespace -} // namespace internal -} // namespace render -} // namespace geometry -} // namespace drake diff --git a/geometry/scene_graph.cc b/geometry/scene_graph.cc index 30ec19496cd7..0e4f70ad3cd1 100644 --- a/geometry/scene_graph.cc +++ b/geometry/scene_graph.cc @@ -4,6 +4,8 @@ #include #include +#include + #include "drake/common/drake_assert.h" #include "drake/geometry/geometry_instance.h" #include "drake/geometry/geometry_state.h" @@ -13,13 +15,11 @@ namespace drake { namespace geometry { +using render::RenderLabel; using systems::Context; using systems::InputPort; -using systems::LeafContext; using systems::LeafSystem; using systems::rendering::PoseBundle; -using systems::SystemOutput; -using systems::SystemSymbolicInspector; using systems::SystemTypeTag; using std::make_unique; using std::vector; @@ -68,13 +68,13 @@ SceneGraph::SceneGraph() model_inspector_.set(initial_state_); geometry_state_index_ = this->DeclareAbstractState(std::move(state_value)); - bundle_port_index_ = this->DeclareAbstractOutputPort("lcm_visualization", - &SceneGraph::MakePoseBundle, - &SceneGraph::CalcPoseBundle).get_index(); + bundle_port_index_ = this->DeclareAbstractOutputPort( + "lcm_visualization", &SceneGraph::MakePoseBundle, + &SceneGraph::CalcPoseBundle) + .get_index(); query_port_index_ = - this->DeclareAbstractOutputPort("query", - &SceneGraph::CalcQueryObject) + this->DeclareAbstractOutputPort("query", &SceneGraph::CalcQueryObject) .get_index(); auto& pose_update_cache_entry = this->DeclareCacheEntry( @@ -103,7 +103,7 @@ SceneGraph::SceneGraph(const SceneGraph& other) : SceneGraph() { // time. // Therefore, for SourceIds i and j, the if i > j, then the port indices for // source i must all be greater than those for j. - std::vector source_ids; + vector source_ids; for (const auto pair : other.input_source_ids_) { source_ids.push_back(pair.first); } @@ -133,7 +133,7 @@ bool SceneGraph::SourceIsRegistered(SourceId id) const { } template -const systems::InputPort& SceneGraph::get_source_pose_port( +const InputPort& SceneGraph::get_source_pose_port( SourceId id) const { ThrowUnlessRegistered(id, "Can't acquire pose port for unknown source id: "); return this->get_input_port(input_source_ids_.at(id).pose_port); @@ -203,6 +203,27 @@ void SceneGraph::RemoveGeometry(Context* context, SourceId source_id, g_state.RemoveGeometry(source_id, geometry_id); } +template +void SceneGraph::AddRenderer( + std::string name, std::unique_ptr renderer) { + return initial_state_->AddRenderer(std::move(name), std::move(renderer)); +} + +template +bool SceneGraph::HasRenderer(const std::string& name) const { + return initial_state_->HasRenderer(name); +} + +template +int SceneGraph::RendererCount() const { + return initial_state_->RendererCount(); +} + +template +vector SceneGraph::RegisteredRendererNames() const { + return initial_state_->RegisteredRendererNames(); +} + template void SceneGraph::AssignRole(SourceId source_id, GeometryId geometry_id, @@ -210,6 +231,29 @@ void SceneGraph::AssignRole(SourceId source_id, initial_state_->AssignRole(source_id, geometry_id, std::move(properties)); } +template +void SceneGraph::AssignRole(Context* context, SourceId source_id, + GeometryId geometry_id, + ProximityProperties properties) const { + auto& g_state = mutable_geometry_state(context); + g_state.AssignRole(source_id, geometry_id, std::move(properties)); +} + +template +void SceneGraph::AssignRole(SourceId source_id, + GeometryId geometry_id, + PerceptionProperties properties) { + initial_state_->AssignRole(source_id, geometry_id, std::move(properties)); +} + +template +void SceneGraph::AssignRole(Context* context, SourceId source_id, + GeometryId geometry_id, + PerceptionProperties properties) const { + auto& g_state = mutable_geometry_state(context); + g_state.AssignRole(source_id, geometry_id, std::move(properties)); +} + template void SceneGraph::AssignRole(SourceId source_id, GeometryId geometry_id, @@ -217,6 +261,39 @@ void SceneGraph::AssignRole(SourceId source_id, initial_state_->AssignRole(source_id, geometry_id, std::move(properties)); } +template +void SceneGraph::AssignRole(Context* context, SourceId source_id, + GeometryId geometry_id, + IllustrationProperties properties) const { + auto& g_state = mutable_geometry_state(context); + g_state.AssignRole(source_id, geometry_id, std::move(properties)); +} + +template +int SceneGraph::RemoveRole(SourceId source_id, FrameId frame_id, Role role) { + return initial_state_->RemoveRole(source_id, frame_id, role); +} + +template +int SceneGraph::RemoveRole(Context* context, SourceId source_id, + FrameId frame_id, Role role) const { + auto& g_state = mutable_geometry_state(context); + return g_state.RemoveRole(source_id, frame_id, role); +} + +template +int SceneGraph::RemoveRole(SourceId source_id, GeometryId geometry_id, + Role role) { + return initial_state_->RemoveRole(source_id, geometry_id, role); +} + +template +int SceneGraph::RemoveRole(Context* context, SourceId source_id, + GeometryId geometry_id, Role role) const { + auto& g_state = mutable_geometry_state(context); + return g_state.RemoveRole(source_id, geometry_id, role); +} + template const SceneGraphInspector& SceneGraph::model_inspector() const { return model_inspector_; @@ -284,7 +361,7 @@ PoseBundle SceneGraph::MakePoseBundle() const { // *model*. // TODO(SeanCurtis-TRI): This happens *twice* now (once here and once in // CalcPoseBundle); might be worth refactoring/caching it. - std::vector dynamic_frames; + vector dynamic_frames; for (const auto& pair : g_state.frames_) { const FrameId frame_id = pair.first; if (frame_id == world_frame_id()) continue; @@ -322,7 +399,7 @@ void SceneGraph::CalcPoseBundle(const Context& context, // Collect only those frames that have illustration geometry -- based on the // *model*. - std::vector dynamic_frames; + vector dynamic_frames; for (const auto& pair : g_state.frames_) { const FrameId frame_id = pair.first; if (frame_id == world_frame_id()) continue; diff --git a/geometry/scene_graph.h b/geometry/scene_graph.h index 43bddf5722ae..b8a88419a751 100644 --- a/geometry/scene_graph.h +++ b/geometry/scene_graph.h @@ -36,13 +36,19 @@ class QueryObject; interface for registering the geometry, updating its position based on the current context, and performing geometric queries. + @system{SceneGraph, + @input_port{source_pose{0}} @input_port{...} @input_port{source_pose{N-1}}, + @output_port{lcm_visualization} @output_port{query} + } + Only registered "geometry sources" can introduce geometry into %SceneGraph. Geometry sources will typically be other leaf systems, but, in the case of _anchored_ (i.e., stationary) geometry, it could also be some other block of code (e.g., adding a common ground plane with which all systems' geometries interact). For dynamic geometry (geometry whose pose depends on a Context), the geometry source must also provide pose values for all of the geometries the - source owns, via a port connection on %SceneGraph. + source owns, via a port connection on %SceneGraph. For N geometry sources, + the %SceneGraph instance will have N pose input ports. The basic workflow for interacting with %SceneGraph is: @@ -464,6 +470,33 @@ class SceneGraph final : public systems::LeafSystem { //@} + /** @name Managing RenderEngine instances */ + //@{ + + /** Adds a new render engine to this %SceneGraph. The %SceneGraph owns the + render engine. All render engines must be assigned prior to any geometry + registration. The render engine's name should be referenced in the + @ref render::CameraProperties "CameraProperties" provided in the render + queries (see QueryObject::RenderColorImage() as an example). + @param name The unique name of the renderer. + @param renderer The `renderer` to add. + @throws std::logic_error if the name is not unique, or geometry has already + been registered. */ + void AddRenderer(std::string name, + std::unique_ptr renderer); + + /** Reports if this %SceneGraph has a renderer registered to the given name. + */ + bool HasRenderer(const std::string& name) const; + + /** Reports the number of renderers registered to this %SceneGraph. */ + int RendererCount() const; + + /** Reports the names of all registered renderers. */ + std::vector RegisteredRendererNames() const; + + //@} + /** @name Assigning roles to geometry Geometries must be assigned one or more *roles* before they have an effect @@ -478,23 +511,88 @@ class SceneGraph final : public systems::LeafSystem { - The geometry id is invalid. - The geometry id is not owned by the given source id. - The indicated role has already been assigned to the geometry. + - Another geometry with the same name, affixed to the same frame, already + has the role. This methods modify the underlying model and require a new Context to be allocated. */ - // TODO(SeanCurtis-TRI): Provide mechanism for modifying properties and/or - // removing roles. + // TODO(SeanCurtis-TRI): Provide mechanism for modifying properties. //@{ - /** Assigns the proximity role to the given geometry. */ + /** Assigns the proximity role to the geometry indicated by `geometry_id`. */ void AssignRole(SourceId source_id, GeometryId geometry_id, ProximityProperties properties); - /** Assigns the illustration role to the given geometry. */ + /** systems::Context-modifying variant of + @ref AssignRole(SourceId,GeometryId,ProximityProperties) "AssignRole()" for + proximity properties. Rather than modifying %SceneGraph's model, it modifies + the copy of the model stored in the provided context. */ + void AssignRole(systems::Context* context, SourceId source_id, + GeometryId geometry_id, ProximityProperties properties) const; + + /** Assigns the perception role to the geometry indicated by `geometry_id`. + */ + void AssignRole(SourceId source_id, GeometryId geometry_id, + PerceptionProperties properties); + + /** systems::Context-modifying variant of + @ref AssignRole(SourceId,GeometryId,PerceptionProperties) "AssignRole()" for + perception properties. Rather than modifying %SceneGraph's model, it modifies + the copy of the model stored in the provided context. */ + void AssignRole(systems::Context* context, SourceId source_id, + GeometryId geometry_id, + PerceptionProperties properties) const; + + /** Assigns the illustration role to the geometry indicated by `geometry_id`. + */ void AssignRole(SourceId source_id, GeometryId geometry_id, IllustrationProperties properties); + /** systems::Context-modifying variant of + @ref AssignRole(SourceId,GeometryId,IllustrationProperties) "AssignRole()" + for illustration properties. Rather than modifying %SceneGraph's model, it + modifies the copy of the model stored in the provided context. */ + void AssignRole(systems::Context* context, SourceId source_id, + GeometryId geometry_id, + IllustrationProperties properties) const; + + /** Removes the indicated `role` from any geometry directly registered to the + frame indicated by `frame_id` (if the geometry has the role). + @returns The number of geometries affected by the removed role. + @throws std::logic_error if 1) `source_id` does not map to a registered + source, 2) `frame_id` does not map to a registered + frame, 3) `frame_id` does not belong to + `source_id` (unless `frame_id` is the world frame + id), or 4) the context has already been + allocated. */ + int RemoveRole(SourceId source_id, FrameId frame_id, Role role); + + /** systems::Context-modifying variant of + @ref RemoveRole(SourceId,FrameId,Role) "RemoveRole()" for frames. + Rather than modifying %SceneGraph's model, it modifies the copy of the model + stored in the provided context. */ + int RemoveRole(systems::Context* context, SourceId source_id, + FrameId frame_id, Role role) const; + + /** Removes the indicated `role` from the geometry indicated by `geometry_id`. + @returns One if the geometry had the role removed and zero if the geometry + did not have the role assigned in the first place. + @throws std::logic_error if 1) `source_id` does not map to a registered + source, 2) `geometry_id` does not map to a + registered geometry, 3) `geometry_id` does + not belong to `source_id`, or 4) the context has + already been allocated. */ + int RemoveRole(SourceId source_id, GeometryId geometry_id, Role role); + + /** systems::Context-modifying variant of + @ref RemoveRole(SourceId,GeometryId,Role) "RemoveRole()" for individual + geometries. Rather than modifying %SceneGraph's model, it modifies the copy + of the model stored in the provided context. */ + int RemoveRole(systems::Context* context, SourceId source_id, + GeometryId geometry_id, Role role) const; + //@} /** Reports the identifier for the world frame. */ @@ -662,6 +760,7 @@ class SceneGraph final : public systems::LeafSystem { // allocating contexts for this system). The instance is owned by // model_abstract_states_. GeometryState* initial_state_{}; + SceneGraphInspector model_inspector_; // The index of the geometry state in the context's abstract state. diff --git a/geometry/test/geometry_state_test.cc b/geometry/test/geometry_state_test.cc index 265af57a2fac..ace252100568 100644 --- a/geometry/test/geometry_state_test.cc +++ b/geometry/test/geometry_state_test.cc @@ -3391,6 +3391,13 @@ TEST_F(GeometryStateNoRendererTest, PerceptionRoleWithoutRenderer) { 0); } +// TODO(SeanCurtis-TRI): The `Render*Image` interface is insufficiently tested. +// GeometryState is a thin wrapper on the render engine, but GeometryState is +// responsible for: +// 1. Confirming the parent frame is valid. +// 2. Updating the camera pose. +// 3. Calling the appropriate render engine. + } // namespace } // namespace geometry } // namespace drake diff --git a/geometry/test/query_object_test.cc b/geometry/test/query_object_test.cc index 87422a657df0..8f81e848b513 100644 --- a/geometry/test/query_object_test.cc +++ b/geometry/test/query_object_test.cc @@ -105,9 +105,14 @@ class QueryObjectTester { namespace { +using render::DepthCameraProperties; +using math::RigidTransformd; using std::make_unique; using std::unique_ptr; using systems::Context; +using systems::sensors::ImageDepth32F; +using systems::sensors::ImageLabel16I; +using systems::sensors::ImageRgba8U; class QueryObjectTest : public ::testing::Test { protected: @@ -164,19 +169,37 @@ TEST_F(QueryObjectTest, DefaultQueryThrows) { EXPECT_DEFAULT_ERROR(QOT::ThrowIfNotCallable(*default_object)); // Enumerate *all* queries to confirm they throw the proper exception. + + // Scalar-dependent state queries. + EXPECT_DEFAULT_ERROR(default_object->X_WF(FrameId::get_new_id())); + EXPECT_DEFAULT_ERROR(default_object->X_PF(FrameId::get_new_id())); + EXPECT_DEFAULT_ERROR(default_object->X_WG(GeometryId::get_new_id())); + + // Penetration queries. EXPECT_DEFAULT_ERROR(default_object->ComputePointPairPenetration()); + EXPECT_DEFAULT_ERROR(default_object->ComputeContactSurfaces()); + + // Signed distance queries. EXPECT_DEFAULT_ERROR( default_object->ComputeSignedDistancePairwiseClosestPoints()); EXPECT_DEFAULT_ERROR( default_object->ComputeSignedDistanceToPoint(Vector3::Zero())); - EXPECT_DEFAULT_ERROR( - default_object->ComputeContactSurfaces()); - EXPECT_DEFAULT_ERROR( - default_object->X_WF(FrameId::get_new_id())); - EXPECT_DEFAULT_ERROR( - default_object->X_PF(FrameId::get_new_id())); - EXPECT_DEFAULT_ERROR( - default_object->X_WG(GeometryId::get_new_id())); + + // Render queries. + DepthCameraProperties properties(2, 2, M_PI, "dummy_renderer", 0.1, 5.0); + RigidTransformd X_WC = RigidTransformd::Identity(); + ImageRgba8U color; + EXPECT_DEFAULT_ERROR(default_object->RenderColorImage( + properties, FrameId::get_new_id(), X_WC, false, &color)); + + ImageDepth32F depth; + EXPECT_DEFAULT_ERROR(default_object->RenderDepthImage( + properties, FrameId::get_new_id(), X_WC, &depth)); + + ImageLabel16I label; + EXPECT_DEFAULT_ERROR(default_object->RenderLabelImage( + properties, FrameId::get_new_id(), X_WC, false, &label)); + #undef EXPECT_DEFAULT_ERROR } diff --git a/geometry/test/scene_graph_test.cc b/geometry/test/scene_graph_test.cc index 33707676f003..dfeb498ab36a 100644 --- a/geometry/test/scene_graph_test.cc +++ b/geometry/test/scene_graph_test.cc @@ -11,7 +11,9 @@ #include "drake/geometry/geometry_set.h" #include "drake/geometry/geometry_visualization.h" #include "drake/geometry/query_object.h" +#include "drake/geometry/render/render_label.h" #include "drake/geometry/shape_specification.h" +#include "drake/geometry/test_utilities/dummy_render_engine.h" #include "drake/systems/framework/context.h" #include "drake/systems/framework/diagram_builder.h" #include "drake/systems/framework/leaf_system.h" @@ -27,6 +29,7 @@ namespace drake { namespace geometry { using Eigen::Isometry3d; +using internal::DummyRenderEngine; using systems::Context; using systems::rendering::PoseBundle; using systems::System; @@ -396,6 +399,49 @@ TEST_F(SceneGraphTest, ModelInspector) { anchored_id); } +// SceneGraph provides a thin wrapper on the GeometryState renderer +// configuration/introspection code. These tests are just smoke tests that the +// functions work. It relies on GeometryState to properly unit test the +// full behavior. +TEST_F(SceneGraphTest, RendererSmokeTest) { + const std::string kRendererName = "bob"; + + EXPECT_EQ(scene_graph_.RendererCount(), 0); + EXPECT_EQ(scene_graph_.RegisteredRendererNames().size(), 0u); + EXPECT_FALSE(scene_graph_.HasRenderer(kRendererName)); + + EXPECT_NO_THROW(scene_graph_.AddRenderer(kRendererName, + make_unique())); + + EXPECT_EQ(scene_graph_.RendererCount(), 1); + EXPECT_EQ(scene_graph_.RegisteredRendererNames()[0], kRendererName); + EXPECT_TRUE(scene_graph_.HasRenderer(kRendererName)); +} + +// SceneGraph provides a thin wrapper on the GeometryState role manipulation +// code. These tests are just smoke tests that the functions work. It relies on +// GeometryState to properly unit test the full behavior. +TEST_F(SceneGraphTest, RoleManagementSmokeTest) { + SourceId s_id = scene_graph_.RegisterSource("test"); + FrameId f_id = scene_graph_.RegisterFrame(s_id, GeometryFrame("frame")); + auto instance = make_unique( + Isometry3::Identity(), make_unique(1.0), "sphere"); + instance->set_illustration_properties(IllustrationProperties()); + instance->set_proximity_properties(ProximityProperties()); + instance->set_perception_properties(PerceptionProperties()); + + GeometryId g_id = scene_graph_.RegisterGeometry(s_id, f_id, move(instance)); + + const SceneGraphInspector& inspector = scene_graph_.model_inspector(); + EXPECT_NE(inspector.GetProximityProperties(g_id), nullptr); + EXPECT_NE(inspector.GetIllustrationProperties(g_id), nullptr); + EXPECT_NE(inspector.GetPerceptionProperties(g_id), nullptr); + + EXPECT_EQ(scene_graph_.RemoveRole(s_id, g_id, Role::kPerception), 1); + EXPECT_EQ(scene_graph_.RemoveRole(s_id, g_id, Role::kProximity), 1); + EXPECT_EQ(scene_graph_.RemoveRole(s_id, g_id, Role::kIllustration), 1); +} + // Dummy system to serve as geometry source. class GeometrySourceSystem : public systems::LeafSystem { public: @@ -715,6 +761,33 @@ GTEST_TEST(SceneGraphContextModifier, CollisionFilters) { // confirm that the model didn't change. } +// A limited test -- the majority of this functionality is encoded in and tested +// via GeometryState. This is just a regression test to make sure SceneGraph's +// invocation of that function doesn't become corrupt. +GTEST_TEST(SceneGraphRenderTest, AddRenderer) { + SceneGraph scene_graph; + + EXPECT_NO_THROW(scene_graph.AddRenderer("unique", + make_unique())); + + // Non-unique renderer name. + // NOTE: The error message is tested in geometry_state_test.cc. + EXPECT_THROW( + scene_graph.AddRenderer("unique", make_unique()), + std::logic_error); + + // Adding a renderer _after_ geometry registration. + SourceId s_id = scene_graph.RegisterSource("dummy"); + scene_graph.RegisterGeometry( + s_id, scene_graph.world_frame_id(), + make_unique(Isometry3::Identity(), + make_unique(1.0), "sphere")); + + EXPECT_THROW( + scene_graph.AddRenderer("different", make_unique()), + std::logic_error); +} + } // namespace } // namespace geometry } // namespace drake