Add ability to convert GeometrySet into unordered_set<GeometryId> (Ro…

…botLocomotion#14871)

The ability to convert also includes the ability to filter based on role.
This includes the public-facing inspector API as well as the GeometryState
implementation.

bindings/pydrake/geometry_py.cc

@@ -403,6 +403,8 @@ void DoScalarDependentDefinitions(py::module m, T) {
             cls_doc.num_geometries.doc)
         .def("GetAllGeometryIds", &Class::GetAllGeometryIds,
             cls_doc.GetAllGeometryIds.doc)
+        .def("GetGeometryIds", &Class::GetGeometryIds, py::arg("geometry_set"),
+            py::arg("role") = std::nullopt, cls_doc.GetGeometryIds.doc)
         .def("NumGeometriesWithRole", &Class::NumGeometriesWithRole,
             py::arg("role"), cls_doc.NumGeometriesWithRole.doc)
         .def("NumDynamicGeometries", &Class::NumDynamicGeometries,
@@ -1164,6 +1166,9 @@ void DoScalarIndependentDefinitions(py::module m) {
     constexpr auto& cls_doc = doc.GeometrySet;
     py::class_<Class>(m, "GeometrySet", cls_doc.doc)
         .def(py::init(), doc.GeometrySet.ctor.doc);
+    // TODO(SeanCurtis-TRI) This is *useless* in python. I can only construct
+    //  an empty GeometrySet. Bind constructors and adders so that these APIs
+    //  can actually be used.
   }
 
   // GeometryVersion

bindings/pydrake/test/geometry_test.py

@@ -87,6 +87,10 @@ def test_scene_graph_api(self, T):
         self.assertIsInstance(inspector.world_frame_id(), mut.FrameId)
         self.assertEqual(inspector.num_geometries(), 3)
         self.assertEqual(len(inspector.GetAllGeometryIds()), 3)
+        self.assertEqual(len(inspector.GetGeometryIds(mut.GeometrySet())), 0)
+        self.assertEqual(len(inspector.GetGeometryIds(mut.GeometrySet(),
+                                                      mut.Role.kProximity)),
+                         0)
         self.assertEqual(
             inspector.NumGeometriesWithRole(role=mut.Role.kUnassigned), 3)
         self.assertEqual(inspector.NumDynamicGeometries(), 2)

geometry/geometry_state.cc

@@ -132,6 +132,16 @@ GeometryState<T>::GeometryState()
   source_root_frame_map_[self_source_] = {world};
 }
 
+template <typename T>
+std::unordered_set<GeometryId> GeometryState<T>::GetGeometryIds(
+      const GeometrySet& geometry_set, const std::optional<Role>& role) const {
+  std::unordered_set<GeometryId> ids;
+  // We don't need to distinguish between anchored and dynamic ids; we'll simply
+  // accumulate them all into the same set -- so we pass ids twice.
+  CollectIds(geometry_set, &ids, &ids, role);
+  return ids;
+}
+
 template <typename T>
 int GeometryState<T>::NumGeometriesWithRole(Role role) const {
   int count = 0;
@@ -930,7 +940,7 @@ void GeometryState<T>::ExcludeCollisionsWithin(const GeometrySet& set) {
 
   std::unordered_set<GeometryId> dynamic;
   std::unordered_set<GeometryId> anchored;
-  CollectIds(set, &dynamic, &anchored);
+  CollectIds(set, &dynamic, &anchored, Role::kProximity);
 
   geometry_version_.modify_proximity();
 
@@ -942,10 +952,10 @@ void GeometryState<T>::ExcludeCollisionsBetween(const GeometrySet& setA,
                                                 const GeometrySet& setB) {
   std::unordered_set<GeometryId> dynamic1;
   std::unordered_set<GeometryId> anchored1;
-  CollectIds(setA, &dynamic1, &anchored1);
+  CollectIds(setA, &dynamic1, &anchored1, Role::kProximity);
   std::unordered_set<GeometryId> dynamic2;
   std::unordered_set<GeometryId> anchored2;
-  CollectIds(setB, &dynamic2, &anchored2);
+  CollectIds(setB, &dynamic2, &anchored2, Role::kProximity);
 
   geometry_version_.modify_proximity();
 
@@ -1039,18 +1049,17 @@ std::unique_ptr<GeometryState<AutoDiffXd>> GeometryState<T>::ToAutoDiffXd()
 }
 
 template <typename T>
-void GeometryState<T>::CollectIds(
-    const GeometrySet& geometry_set, std::unordered_set<GeometryId>* dynamic,
-    std::unordered_set<GeometryId>* anchored) {
-  // TODO(SeanCurtis-TRI): Consider expanding this to include Role if it proves
-  // that collecting ids for *other* role-related tasks prove necessary.
+void GeometryState<T>::CollectIds(const GeometrySet& geometry_set,
+                                  std::unordered_set<GeometryId>* dynamic,
+                                  std::unordered_set<GeometryId>* anchored,
+                                  const std::optional<Role>& role) const {
   std::unordered_set<GeometryId>* target;
   for (auto frame_id : geometry_set.frames()) {
     const auto& frame = GetValueOrThrow(frame_id, frames_);
     target = frame.is_world() ? anchored : dynamic;
     for (auto geometry_id : frame.child_geometries()) {
-      InternalGeometry& geometry = geometries_[geometry_id];
-      if (geometry.has_proximity_role()) {
+      const InternalGeometry& geometry = geometries_.at(geometry_id);
+      if (!role.has_value() || geometry.has_role(*role)) {
         target->insert(geometry_id);
       }
     }
@@ -1064,7 +1073,7 @@ void GeometryState<T>::CollectIds(
           "SceneGraph: " +
           to_string(geometry_id));
     }
-    if (geometry->has_proximity_role()) {
+    if (!role.has_value() || geometry->has_role(*role)) {
       if (geometry->is_dynamic()) {
         dynamic->insert(geometry_id);
       } else {

geometry/geometry_state.h

@@ -119,6 +119,10 @@ class GeometryState {
     return ids;
   }
 
+  /** Implementation of SceneGraphInspector::GetGeometryIds().  */
+  std::unordered_set<GeometryId> GetGeometryIds(
+      const GeometrySet& geometry_set, const std::optional<Role>& role) const;
+
   /** Implementation of SceneGraphInspector::NumGeometriesWithRole().  */
   int NumGeometriesWithRole(Role role) const;
 
@@ -592,6 +596,9 @@ class GeometryState {
   // defined in terms of geometry ids *and* frame ids). If GeometrySet only
   // has GeometryIds, it is essentially a copy. Ids that can't be identified
   // will cause an exception to be thrown.
+  // The ids can be optionally filtered based on role. If `role` is nullopt,
+  // no filtering takes place. Otherwise, just those geometries with the given
+  // role will be returned.
   // TODO(SeanCurtis-TRI): Because all geometries only have a single id
   // type, we have two sets of the same id type. The compiler cannot know
   // that only anchored geometries go into the anchored set and only dynamic go
@@ -605,7 +612,8 @@ class GeometryState {
   // id type in both sets.
   void CollectIds(const GeometrySet& geometry_set,
                   std::unordered_set<GeometryId>* dynamic,
-                  std::unordered_set<GeometryId>* anchored);
+                  std::unordered_set<GeometryId>* anchored,
+                  const std::optional<Role>& role) const;
 
   // Sets the kinematic poses for the frames indicated by the given ids.
   // @param poses The frame id and pose values.

geometry/scene_graph_inspector.h

@@ -121,6 +121,28 @@ class SceneGraphInspector {
     return state_->GetAllGeometryIds();
   }
 
+  /** Returns the geometry ids that are *implied* by the given GeometrySet and
+   `role`. Remember that a GeometrySet can reference a FrameId in place of the
+   ids of the individual geometries affixed to it. If a `role` is provided, only
+   geometries with that role assigned will be returned, otherwise all geometries
+   will be returned.
+
+   @note Specifying `role` *can* have the effect of filtering geometries *from*
+   the given geometry_set` -- if a GeometryId is an explicit member of the
+   geometry set but does not have the requested role, it will not be contained
+   in the output.
+
+   @param geometry_set    The encoding of the set of geometries.
+   @param role            The requested role; if omitted, all geometries
+                          registered to the frame are returned.
+   @returns The requested unique geometry ids.  */
+  std::unordered_set<GeometryId> GetGeometryIds(
+      const GeometrySet& geometry_set,
+      const std::optional<Role>& role = std::nullopt) const {
+    DRAKE_DEMAND(state_ != nullptr);
+    return state_->GetGeometryIds(geometry_set, role);
+  }
+
   /** Reports the _total_ number of geometries in the scene graph with the
    indicated role.  */
   int NumGeometriesWithRole(Role role) const {

geometry/test/geometry_state_test.cc

@@ -1052,6 +1052,91 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) {
   EXPECT_EQ(gs_tester_.get_frame_parent_poses().size(), kFrameCount + 1);
 }
 
+// Confirms that a GeometrySet can be converted into a set of geometry ids.
+TEST_F(GeometryStateTest, GetGeometryIds) {
+  // Configure a scene where *every* geometry has a proximity role, but other
+  // geometries selectively have illustration and/or perception roles.
+  SetUpSingleSourceTree(Assign::kProximity);
+  ASSERT_LE(6, single_tree_dynamic_geometry_count());
+
+  // Frame 0 has proximity only. Frame 1 has proximity and perception.
+  PerceptionProperties perception;
+  perception.AddProperty("phong", "diffuse", Rgba{0.8, 0.8, 0.8, 1.0});
+  perception.AddProperty("label", "id", RenderLabel::kDontCare);
+  geometry_state_.AssignRole(source_id_, geometries_[2], perception);
+  geometry_state_.AssignRole(source_id_, geometries_[3], perception);
+
+  // Frame 2 has *one* geometry with illustration and proximity and one frame
+  // with all three.
+  IllustrationProperties illustration;
+  illustration.AddProperty("phong", "diffuse", Rgba{0.8, 0.8, 0.8, 1.0});
+  geometry_state_.AssignRole(source_id_, geometries_[4], illustration);
+  geometry_state_.AssignRole(source_id_, geometries_[5], illustration);
+  geometry_state_.AssignRole(source_id_, geometries_[5], perception);
+
+  // Collect up all geometries (in various categories) for convenience.
+  unordered_set<GeometryId> all_geometries(geometries_.begin(),
+                                           geometries_.end());
+  all_geometries.insert(anchored_geometry_);
+  const unordered_set<GeometryId> perception_geometries{
+      geometries_[2], geometries_[3], geometries_[5]};
+  const unordered_set<GeometryId> illustration_geometries{geometries_[4],
+                                                          geometries_[5]};
+
+  {
+    // Case: All geometries referenced solely by their parent frames.
+    unordered_set<FrameId> all_frames(frames_.begin(), frames_.end());
+    all_frames.insert(InternalFrame::world_frame_id());
+    EXPECT_EQ(
+        geometry_state_.GetGeometryIds(GeometrySet(all_frames), std::nullopt),
+        all_geometries);
+    EXPECT_EQ(geometry_state_.GetGeometryIds(GeometrySet(all_frames),
+                                             Role::kProximity),
+              all_geometries);
+    EXPECT_EQ(geometry_state_.GetGeometryIds(GeometrySet(all_frames),
+                                             Role::kPerception),
+              perception_geometries);
+    EXPECT_EQ(geometry_state_.GetGeometryIds(GeometrySet(all_frames),
+                                             Role::kIllustration),
+              illustration_geometries);
+  }
+
+  {
+    // Case: All geometries referenced explicitly; this implicitly confirms
+    // "culling" of otherwise explicitly declared GeometryIds.
+    EXPECT_EQ(geometry_state_.GetGeometryIds(GeometrySet(all_geometries),
+                                             std::nullopt),
+              all_geometries);
+    EXPECT_EQ(geometry_state_.GetGeometryIds(GeometrySet(all_geometries),
+                                             Role::kProximity),
+              all_geometries);
+    EXPECT_EQ(geometry_state_.GetGeometryIds(GeometrySet(all_geometries),
+                                             Role::kPerception),
+              perception_geometries);
+    EXPECT_EQ(geometry_state_.GetGeometryIds(GeometrySet(all_geometries),
+                                             Role::kIllustration),
+              illustration_geometries);
+  }
+
+  {
+    // Case: Redundant specification; a geometry id is included explicitly as
+    // well as its parent frame. If it *should* be included, it will appear
+    // once. If it should *not* appear, it will not be included at all.
+    EXPECT_EQ(geometry_state_.GetFrameId(geometries_[2]), frames_[1]);
+    EXPECT_EQ(geometry_state_.GetFrameId(geometries_[3]), frames_[1]);
+    const GeometrySet redundant_set({geometries_[2]}, {frames_[1]});
+    EXPECT_EQ(geometry_state_.GetGeometryIds(redundant_set, std::nullopt),
+              unordered_set<GeometryId>({geometries_[2], geometries_[3]}));
+    EXPECT_EQ(geometry_state_.GetGeometryIds(redundant_set, Role::kProximity),
+              unordered_set<GeometryId>({geometries_[2], geometries_[3]}));
+    EXPECT_EQ(geometry_state_.GetGeometryIds(redundant_set, Role::kPerception),
+              unordered_set<GeometryId>({geometries_[2], geometries_[3]}));
+    EXPECT_EQ(
+        geometry_state_.GetGeometryIds(redundant_set, Role::kIllustration),
+        unordered_set<GeometryId>());
+  }
+}
+
 // Tests the GetNum*Geometry*Methods.
 TEST_F(GeometryStateTest, GetNumGeometryTests) {
   SetUpSingleSourceTree(Assign::kProximity);

geometry/test/scene_graph_inspector_test.cc

@@ -47,6 +47,7 @@ GTEST_TEST(SceneGraphInspector, ExerciseEverything) {
   inspector.all_frame_ids();
   inspector.num_geometries();
   inspector.GetAllGeometryIds();
+  inspector.GetGeometryIds(GeometrySet{});
   inspector.NumGeometriesWithRole(Role::kUnassigned);
   inspector.NumDynamicGeometries();
   inspector.NumAnchoredGeometries();