Allow adding render engine after registering geometry (RobotLocomotio…

…n#11820)

* Allows geometries to be added before renderers

This removes the old requirement that all renderers needed to be
added before any geometry. Now, geometry that has been registered
and has had the perception property assigned will automatically
be added to any subsequent render engine that gets added.

* Eliminate the two-way coupling between RenderEngine and InternalGeometry

InternalGeometry used to have a list of all render engines it was
registered with. This required updating *two* things every time a geometry
was added/removed from a render engine.

This modifies RenderEngine to report if an id has been registered with it.
Now, rather than querying the internal geometry if it is a render engine
(and hope that it is correct), we simply ask the engine directly.

geometry/geometry_state.cc

@@ -775,15 +775,10 @@ void GeometryState<T>::AssignRole(SourceId source_id, GeometryId geometry_id,
   geometry.SetRole(std::move(properties));
 
   for (auto& pair : render_engines_) {
-    const std::string& renderer_name = pair.first;
     auto& engine = pair.second;
-    const bool accepted =
-        engine->RegisterVisual(geometry_id, geometry.shape(),
-                               *geometry.perception_properties(),
-                               RigidTransformd(geometry.X_FG()),
-                               geometry.is_dynamic());
-    // If accepted, inform the geometry that it exists in the renderer.
-    if (accepted) geometry.set_renderer(renderer_name);
+    engine->RegisterVisual(
+        geometry_id, geometry.shape(), *geometry.perception_properties(),
+        RigidTransformd(geometry.X_FG()), geometry.is_dynamic());
   }
 }
 
@@ -909,17 +904,23 @@ void GeometryState<T>::ExcludeCollisionsBetween(const GeometrySet& setA,
 template <typename T>
 void GeometryState<T>::AddRenderer(
     std::string name, std::unique_ptr<render::RenderEngine> renderer) {
-  if (geometries_.size() > 0) {
-    throw std::logic_error(
-        fmt::format("AddRenderer(): Error adding renderer '{}'; geometries "
-                    "have already been registered",
-                    name));
-  }
   if (render_engines_.count(name) > 0) {
     throw std::logic_error(fmt::format(
         "AddRenderer(): A renderer with the name '{}' already exists", name));
   }
-  render_engines_[move(name)] = move(renderer);
+  render::RenderEngine* render_engine = renderer.get();
+  render_engines_[name] = move(renderer);
+  for (auto& id_geo_pair : geometries_) {
+    InternalGeometry& geometry = id_geo_pair.second;
+    if (geometry.has_perception_role()) {
+      const GeometryId id = id_geo_pair.first;
+      const PerceptionProperties* properties = geometry.perception_properties();
+      DRAKE_DEMAND(properties != nullptr);
+      render_engine->RegisterVisual(id, geometry.shape(), *properties,
+                                    RigidTransformd(geometry.X_FG()),
+                                    geometry.is_dynamic());
+    }
+  }
 }
 
 template <typename T>
@@ -1270,16 +1271,10 @@ bool GeometryState<T>::RemoveRoleUnchecked(GeometryId geometry_id, Role role) {
 template <typename T>
 bool GeometryState<T>::RemoveFromRendererUnchecked(
     const std::string& renderer_name, GeometryId id) {
-  internal::InternalGeometry* geometry = GetMutableGeometry(id);
-
-  if (geometry->in_renderer(renderer_name)) {
-    // Speculatively remove the id from the renderer. If it hasn't been
-    // registered, we'll simply return false.
-    render::RenderEngine* engine = render_engines_[renderer_name].get_mutable();
-    geometry->ClearRenderer(renderer_name);
-    // The internal geometry instance has reported its belief that it is in
-    // this named renderer. Therefore, attempting to remove its id from the
-    // renderer should report success.
+  render::RenderEngine* engine = render_engines_[renderer_name].get_mutable();
+  if (engine->has_geometry(id)) {
+    // The engine has reported the belief that it has geometry `id`. Therefore,
+    // removal should report true.
     DRAKE_DEMAND(engine->RemoveGeometry(id) == true);
     return true;
   }

geometry/internal_geometry.cc

@@ -37,13 +37,6 @@ bool InternalGeometry::has_role(Role role) const {
   DRAKE_UNREACHABLE();
 }
 
-void InternalGeometry::set_renderer(std::string renderer_name) {
-  auto result = renderers_.emplace(move(renderer_name));
-  // As an internal class, setting a duplicate renderer is a programming error
-  // in SceneGraph's internals.
-  DRAKE_ASSERT(result.second);
-}
-
 }  // namespace internal
 }  // namespace geometry
 }  // namespace drake

geometry/internal_geometry.h

@@ -226,28 +226,6 @@ class InternalGeometry {
     return nullptr;
   }
 
-  /** Informs this geometry that it is registered to the given named render
-   engine.
-   @pre This geometry doesn't already have this renderer name.
-   @pre The `renderer_name` is a name for a valid renderer.  */
-  void set_renderer(std::string renderer_name);
-
-  /** Reports true if this geometry knows that it is registered with a render
-   engine with the given name.  */
-  bool in_renderer(const std::string& renderer_name) const {
-    return renderers_.count(renderer_name) > 0;
-  }
-
-  /** Clears this geometry's knowledge that it has been registered with the
-   named render engine. If the geometry doesn't know about the given name,
-   nothing happens.
-   @note This leaves the properties intact; it makes no effort to divine which
-   properties caused this geometry to be accepted by that renderer or its
-   uniqueness (both would be required for removing those properties).  */
-  void ClearRenderer(const std::string& renderer_name) {
-    renderers_.erase(renderer_name);
-  }
-
   /** Removes the proximity role assigned to this geometry -- if there was
    no proximity role previously, this has no effect.  */
   void RemoveProximityRole() {
@@ -264,7 +242,6 @@ class InternalGeometry {
    no perception role previously, this has no effect.  */
   void RemovePerceptionRole() {
     perception_props_ = nullopt;
-    renderers_.clear();
   }
 
   //@}
@@ -307,9 +284,6 @@ class InternalGeometry {
   optional<ProximityProperties> proximity_props_{};
   optional<IllustrationProperties> illustration_props_{};
   optional<PerceptionProperties> perception_props_{};
-
-  // The renderers in which this geometry is registered.
-  std::unordered_set<std::string> renderers_;
 };
 
 }  // namespace internal

geometry/render/render_engine.cc

@@ -35,10 +35,13 @@ bool RenderEngine::RemoveGeometry(GeometryId id) {
   } else {
     DRAKE_DEMAND(update_ids_.count(id) == 0 || anchored_ids_.count(id) == 0);
   }
-
   return removed;
 }
 
+bool RenderEngine::has_geometry(GeometryId id) const {
+  return update_ids_.count(id) > 0 || anchored_ids_.count(id) > 0;
+}
+
 RenderLabel RenderEngine::GetRenderLabelOrThrow(
     const PerceptionProperties& properties) const {
   RenderLabel label =

geometry/render/render_engine.h

@@ -141,6 +141,10 @@ class RenderEngine : public ShapeReifier {
             registered with this engine).  */
   bool RemoveGeometry(GeometryId id);
 
+  /** Reports true if a geometry with the given `id` has been registered with
+   `this` engine.  */
+  bool has_geometry(GeometryId id) const;
+
   /** Updates the poses of all geometries marked as "needing update" (see
  RegisterVisual()).
 

geometry/scene_graph.h

@@ -474,14 +474,28 @@ class SceneGraph final : public systems::LeafSystem<T> {
   //@{
 
   /** 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
+   render engine. 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).
+
+   There is no restriction on when a renderer is added relative to geometry
+   registration and role assignment. Given a representative sequence of
+   registration and perception role assignment, the addition of the renderer
+   can be introduced anywhere in the sequence and the end result would be
+   the same.
+
+   ```
+   GeometryId id1 = scene_graph.RegisterGeometry(source_id, ...);
+   scene_graph.AssignRole(source_id, id1, PerceptionProperties());
+   GeometryId id2 = scene_graph.RegisterGeometry(source_id, ...);
+   scene_graph.AssignRole(source_id, id2, PerceptionProperties());
+   GeometryId id3 = scene_graph.RegisterGeometry(source_id, ...);
+   scene_graph.AssignRole(source_id, id3, PerceptionProperties());
+   ```
+
    @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.  */
+   @throws std::logic_error if the name is not unique.  */
   void AddRenderer(std::string name,
                    std::unique_ptr<render::RenderEngine> renderer);
 

geometry/test/geometry_state_test.cc

@@ -907,7 +907,6 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) {
       EXPECT_EQ(geometry.child_geometry_ids().size(), 0);
       EXPECT_FALSE(geometry.parent_id());
       EXPECT_EQ(geometry.name(), geometry_names_[i]);
-      EXPECT_FALSE(geometry.in_renderer(kDummyRenderName));
       EXPECT_EQ(geometry.child_geometry_ids().size(), 0);
 
       // Note: There are no geometries parented to other geometries. The results
@@ -2851,8 +2850,9 @@ TEST_F(GeometryStateTest, RemoveUnassignedRole) {
 TEST_F(GeometryStateTest, RemoveGeometryFromRenderer) {
   // Add an additional renderer *before* populating the world (as required).
   const string other_renderer_name = "alt_renderer";
-  geometry_state_.AddRenderer(other_renderer_name,
-                              make_unique<DummyRenderEngine>());
+  auto temp_engine = make_unique<DummyRenderEngine>();
+  const DummyRenderEngine& other_engine = *temp_engine;
+  geometry_state_.AddRenderer(other_renderer_name, move(temp_engine));
 
   SetUpSingleSourceTree(Assign::kPerception);
 
@@ -2864,15 +2864,15 @@ TEST_F(GeometryStateTest, RemoveGeometryFromRenderer) {
   // addition,
   //   a) report itself in the "other" render engine, xor
   //   b) be present in the `removed_from_renderer` set.
-  auto confirm_renderers = [=](set<GeometryId> removed_from_renderer) {
+  auto confirm_renderers = [=, &other_engine](
+                               set<GeometryId> removed_from_renderer) {
     set<GeometryId> ids(geometries_.begin(), geometries_.end());
     ids.insert(anchored_geometry_);
     for (GeometryId id : ids) {
       // All should report in the dummy renderer.
-      EXPECT_TRUE(gs_tester_.GetGeometry(id)->in_renderer(kDummyRenderName));
+      EXPECT_TRUE(render_engine_->has_geometry(id));
       // Should report in the renderer if it is *not* in the removed set.
-      EXPECT_EQ(gs_tester_.GetGeometry(id)
-                    ->in_renderer(other_renderer_name),
+      EXPECT_EQ(other_engine.has_geometry(id),
                 removed_from_renderer.count(id) == 0);
     }
   };
@@ -2927,8 +2927,9 @@ TEST_F(GeometryStateTest, RemoveFrameFromRenderer) {
   // TODO(SeanCurtis-TRI): Consider refactoring this set-up code between _this_
   // test and the RemoveGeometryFromRenderer test.
   const string other_renderer_name = "alt_renderer";
-  geometry_state_.AddRenderer(other_renderer_name,
-                              make_unique<DummyRenderEngine>());
+  auto temp_engine = make_unique<DummyRenderEngine>();
+  const DummyRenderEngine& other_engine = *temp_engine;
+  geometry_state_.AddRenderer(other_renderer_name, move(temp_engine));
 
   SetUpSingleSourceTree(Assign::kPerception);
 
@@ -2940,16 +2941,15 @@ TEST_F(GeometryStateTest, RemoveFrameFromRenderer) {
   // addition,
   //   a) report itself in the "other" render engine, xor
   //   b) be present in the `removed_from_renderer` set.
-  auto confirm_renderers = [=](set<GeometryId> removed_from_renderer) {
+  auto confirm_renderers = [=, &other_engine](
+                               set<GeometryId> removed_from_renderer) {
     set<GeometryId> ids(geometries_.begin(), geometries_.end());
     ids.insert(anchored_geometry_);
     for (GeometryId id : ids) {
       // All should report in the dummy renderer.
-      EXPECT_TRUE(gs_tester_.GetGeometry(id)
-                      ->in_renderer(kDummyRenderName));
+      EXPECT_TRUE(render_engine_->has_geometry(id));
       // Should report in the renderer if it is *not* in the removed set.
-      EXPECT_EQ(gs_tester_.GetGeometry(id)
-                    ->in_renderer(other_renderer_name),
+      EXPECT_EQ(other_engine.has_geometry(id),
                 removed_from_renderer.count(id) == 0);
     }
   };
@@ -3025,6 +3025,34 @@ TEST_F(GeometryStateTest, RemoveFrameFromRenderer) {
       "Referenced frame .+ but the frame doesn't belong to the source.");
 }
 
+TEST_F(GeometryStateTest, AddRendererAfterGeometry) {
+  SetUpSingleSourceTree(Assign::kPerception);
+  // Add one geometry that has no perception properties.
+  const GeometryId id_no_perception = geometry_state_.RegisterGeometry(
+      source_id_, frames_[0],
+      make_unique<GeometryInstance>(Isometry3d::Identity(),
+                                    make_unique<Sphere>(0.5), "shape"));
+  EXPECT_EQ(render_engine_->num_registered(),
+            single_tree_total_geometry_count());
+
+  auto new_renderer = make_unique<DummyRenderEngine>();
+  DummyRenderEngine* other_renderer = new_renderer.get();
+  // The new renderer has no geometry assigned.
+  EXPECT_EQ(other_renderer->num_registered(), 0);
+  const string other_name = "other";
+  geometry_state_.AddRenderer(other_name, move(new_renderer));
+  // The new renderer only has the geometries with perception properties
+  // assigned.
+  EXPECT_EQ(other_renderer->num_registered(),
+            single_tree_total_geometry_count());
+
+  EXPECT_FALSE(other_renderer->has_geometry(id_no_perception));
+
+  for (const GeometryId id : geometries_) {
+    EXPECT_TRUE(other_renderer->has_geometry(id));
+  }
+}
+
 // Successful invocations of AddRenderer are implicit in SetupSingleSource().
 // This merely tests the error conditions.
 TEST_F(GeometryStateTest, AddRendererError) {
@@ -3038,15 +3066,6 @@ TEST_F(GeometryStateTest, AddRendererError) {
       std::logic_error,
       fmt::format("AddRenderer..: A renderer with the name '{}' already exists",
                   kName));
-
-  // Geometry has been registered.
-  SetUpSingleSourceTree();
-  DRAKE_EXPECT_THROWS_MESSAGE(
-      geometry_state_.AddRenderer(kName, make_unique<DummyRenderEngine>()),
-      std::logic_error,
-      fmt::format("AddRenderer..: Error adding renderer '{}'; geometries have "
-                  "already been registered",
-                  kName));
 }
 
 TEST_F(GeometryStateTest, GetRenderEngine) {
@@ -3072,8 +3091,8 @@ TEST_F(GeometryStateTest, RendererPoseUpdate) {
   // Reality check -- all geometries report as part of both engines.
   for (int i = 0; i < single_tree_dynamic_geometry_count(); ++i) {
     const InternalGeometry* geometry = gs_tester_.GetGeometry(geometries_[i]);
-    EXPECT_TRUE(geometry->in_renderer(kDummyRenderName));
-    EXPECT_TRUE(geometry->in_renderer(second_engine_name));
+    EXPECT_TRUE(render_engine_->has_geometry(geometry->id()));
+    EXPECT_TRUE(second_engine->has_geometry(geometry->id()));
   }
 
   EXPECT_EQ(second_engine->updated_ids().size(), 0u);