scene_graph_inspector.h

#pragma once

#include <memory>
#include <optional>
#include <set>
#include <string>
#include <unordered_set>
#include <utility>
#include <variant>
#include <vector>

#include "drake/common/drake_deprecated.h"
#include "drake/geometry/geometry_instance.h"
#include "drake/geometry/geometry_roles.h"
#include "drake/geometry/geometry_set.h"
#include "drake/geometry/geometry_version.h"
#include "drake/geometry/internal_frame.h"
#include "drake/geometry/proximity/triangle_surface_mesh.h"
#include "drake/geometry/proximity/volume_mesh.h"
#include "drake/geometry/shape_specification.h"

namespace drake {
namespace geometry {

#ifndef DRAKE_DOXYGEN_CXX
template <typename T>
class GeometryState;
template <typename T>
class QueryObject;
template <typename T>
class SceneGraph;
#endif

/** The %SceneGraphInspector serves as a mechanism to query the topological
 structure of a SceneGraph instance. The topological structure consists of all
 of the SceneGraph data that does _not_ depend on input pose data. Including,
 but not limited to:

 - names of frames and geometries
 - hierarchies (parents of geometries, parents of frames, etc.)
 - geometry parameters (e.g., contact, rendering, visualization)
 - fixed poses of geometries relative to frames

 In contrast, the following pieces of data *do* depend on input pose data and
 _cannot_ be performed with the %SceneGraphInspector (see the QueryObject
 instead):

 - world pose of frames or geometry
 - collision queries
 - proximity queries

 A %SceneGraphInspector cannot be instantiated explicitly. Nor can it be copied
 or moved. A _reference_ to a %SceneGraphInspector instance can be acquired from

 - a SceneGraph instance (to inspect the state of the system's _model_), or
 - a QueryObject instance (to inspect the state of the scene graph data stored
   in the context).

 The reference should not be persisted (and, as previously indicated, cannot
 be copied). %SceneGraphInspector instances are cheap; they can be created,
 queried, and thrown out. If there is any doubt about the valid lifespan of
 a %SceneGraphInspector, throw out the old instance and request a new instance.

 @tparam T The scalar of the associated SceneGraph instance. The template
 parameter is provided for the sake of compatibility, although no queries (or
 their results) depend on the scalar.  */
template <typename T>
class SceneGraphInspector {
 public:
  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SceneGraphInspector)

  // NOTE: An inspector should never be released into the wild without having
  // set the state variable. Every query should start by demanding that state_
  // is defined.

  //----------------------------------------------------------------------------

  /** @name              Scene-graph wide data  */
  //@{

  /** Reports the number of registered sources -- whether they have registered
   frames/geometries or not. This will always be at least 1; the SceneGraph
   itself counts as a source.  */
  int num_sources() const;

  /** Reports the _total_ number of frames registered in the scene graph
   (including the world frame).  */
  int num_frames() const;

  /** Returns all of the frame ids in the scene graph. The order is not
   guaranteed; but it will be consistent across invocations as long as there are
   no changes to the topology. The ids includes the world frame's id.  */
  typename std::vector<FrameId> GetAllFrameIds() const;

  /** Reports the id for the world frame.  */
  FrameId world_frame_id() const {
    DRAKE_DEMAND(state_ != nullptr);
    return internal::InternalFrame::world_frame_id();
  }

  /** Reports the _total_ number of geometries in the scene graph.  */
  int num_geometries() const;

  /** Returns the set of all ids for registered geometries. The order is _not_
   guaranteed to have any particular meaning. But the order is
   guaranteed to remain fixed until a topological change is made (e.g., removal
   or addition of geometry/frames).  */
  const std::vector<GeometryId> GetAllGeometryIds() const;

  /** 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;

  /** Reports the _total_ number of geometries in the scene graph with the
   indicated role.  */
  int NumGeometriesWithRole(Role role) const;

  /** Reports the total number of _dynamic_ geometries in the scene graph. This
   include all deformable geometries.  */
  int NumDynamicGeometries() const;

  /** Reports the total number of _anchored_ non-deformable geometries. This
   should provide the same answer as calling NumGeometriesForFrame() with the
   world frame id.  */
  int NumAnchoredGeometries() const;

  /** Returns all pairs of geometries that are candidates for collision (in no
   particular order). See CollisionFilterDeclaration and
   CollisionFilterManager::Apply() for information on why a particular pair may
   _not_ be a candidate.

   For candidate pair (A, B), the candidate is always guaranteed to be reported
   in a fixed order (i.e., always (A, B) and _never_ (B, A)). This is the same
   ordering as would be returned by, e.g.,
   QueryObject::ComputePointPairPenetration().  */
  std::set<std::pair<GeometryId, GeometryId>> GetCollisionCandidates() const;

  /** Returns the geometry version that can be used to detect changes
   to the geometry data associated with geometry roles. The reference returned
   should not be persisted. If it needs to be persisted, it should be copied. */
  const GeometryVersion& geometry_version() const;
  //@}

  /** @name                Sources and source-related data  */
  //@{

  /** Reports `true` if the given `source_id` maps to a registered source.  */
  bool SourceIsRegistered(SourceId source_id) const;

  /** Reports the name for the source with the given `source_id`.
   @throws std::exception if `source_id` does not map to a registered source. */
  const std::string& GetName(SourceId source_id) const;

  /** Reports the number of frames registered to the source with the given
   `source_id`.
   @throws std::exception if `source_id` does not map to a registered source.
   */
  int NumFramesForSource(SourceId source_id) const;

  /** Reports the ids of all of the frames registered to the source with the
   given source `source_id`.
   @throws std::exception if `source_id` does not map to a registered source.
   */
  const std::unordered_set<FrameId>& FramesForSource(SourceId source_id) const;

  //@}

  /** @name              Frames and their properties  */
  //@{

  /** Reports if the frame with given `frame_id` was registered to the source
   with the given `source_id`.
   @param frame_id      The query frame id.
   @param source_id     The query source id.
   @returns True if `frame_id` was registered on `source_id`.
   @throws std::exception  If `frame_id` does not map to a registered frame
                           or `source_id` does not map to a registered source.
   */
  bool BelongsToSource(FrameId frame_id, SourceId source_id) const;

  /** Reports the _name_ of the geometry source that registered the frame with
   the given `frame_id`.
   @throws std::exception  If `frame_id` does not map to a registered frame.
   */
  const std::string& GetOwningSourceName(FrameId frame_id) const;

  /** Reports the name of the frame with the given `frame_id`.
   @throws std::exception if `frame_id` does not map to a registered frame.
   */
  const std::string& GetName(FrameId frame_id) const;

  /** Reports the FrameId of the parent of `frame_id`.
   @throws std::exception if `frame_id` does not map to a registered frame.
   */
  FrameId GetParentFrame(FrameId frame_id) const;

  /** Reports the frame group for the frame with the given `frame_id`.
   @throws std::exception if `frame_id` does not map to a registered frame.
   @internal This value is equivalent to the old "model instance id".  */
  int GetFrameGroup(FrameId frame_id) const;

  /** Reports the number of geometries affixed to the frame with the given
   `frame_id`. This count does _not_ include geometries attached to frames that
   are descendants of this frame.
   @throws std::exception if `frame_id` does not map to a registered frame.
   */
  int NumGeometriesForFrame(FrameId frame_id) const;

  /** Reports the total number of geometries with the given `role` directly
   registered to the frame with the given `frame_id`. This count does _not_
   include geometries attached to frames that are descendants of this frame.
   @throws std::exception if `frame_id` does not map to a registered frame.
   */
  int NumGeometriesForFrameWithRole(FrameId frame_id, Role role) const;

  /** Returns geometry ids that have been registered directly to the frame
   indicated by `frame_id`. If a `role` is provided, only geometries with that
   role assigned will be returned, otherwise all geometries will be returned.
   @param frame_id    The id of the frame in question.
   @param role  The requested role; if omitted, all geometries registered to the
                frame are returned.
   @returns The requested unique geometry ids in a consistent order.
   @throws std::exception if `id` does not map to a registered frame.  */
  std::vector<GeometryId> GetGeometries(
      FrameId frame_id, const std::optional<Role>& role = std::nullopt) const;

  /** Reports the id of the geometry with the given `name` and `role`, attached
   to the frame with the given frame `frame_id`.
   @param frame_id  The frame_id of the frame whose geometry is being
                    queried.
   @param role      The assigned role of the desired geometry.
   @param name      The name of the geometry to query for. The name will be
                    canonicalized prior to lookup (see
                    @ref canonicalized_geometry_names "GeometryInstance" for
                    details).
   @return The id of the queried geometry.
   @throws std::exception if no such geometry exists, multiple geometries have
                          that name, or if the `frame_id` does not map to a
                          registered frame.  */
  GeometryId GetGeometryIdByName(FrameId frame_id, Role role,
                                 const std::string& name) const;

  //@}

  /** @name           Geometries and their properties  */
  //@{

  /** Reports if the given geometry id was registered to the source with the
   given source id.
   @param geometry_id   The query geometry id.
   @param source_id     The query source id.
   @returns True if `geometry_id` was registered on `source_id`.
   @throws std::exception  If `geometry_id` does not map to a registered
                           geometry or `source_id` does not map to a
                           registered source.  */
  bool BelongsToSource(GeometryId geometry_id, SourceId source_id) const;

  /** Reports the _name_ of the geometry source that registered the geometry
   with the given `geometry_id`.
   @throws std::exception  If `geometry_id` does not map to a registered
   geometry. */
  const std::string& GetOwningSourceName(GeometryId geometry_id) const;

  /** Reports the id of the frame to which the given geometry with the given
   `geometry_id` is registered.
   @throws std::exception if `geometry_id` does not map to a registered
   geometry.  */
  FrameId GetFrameId(GeometryId geometry_id) const;

  /** Reports the stored, canonical name of the geometry with the given
   `geometry_id` (see  @ref canonicalized_geometry_names "GeometryInstance" for
   details).
   @throws std::exception if `geometry_id` does not map to a registered
   geometry.  */
  const std::string& GetName(GeometryId geometry_id) const;

  /** Returns the shape specified for the geometry with the given `geometry_id`.
   In order to extract the details of the shape, it should be passed through an
   implementation of a ShapeReifier.  */
  const Shape& GetShape(GeometryId geometry_id) const;

  /** Reports the pose of the geometry G with the given `geometry_id` in its
   registered _topological parent_ P, `X_PG`. That topological parent may be a
   frame F or another geometry. If the geometry was registered directly to F,
   then `X_PG = X_FG`.
   @sa GetPoseInFrame()
   @note For deformable geometries, this returns the pose of the reference mesh.
   @throws std::exception if `geometry_id` does not map to a registered
   geometry. */
  DRAKE_DEPRECATED("2023-04-01",
                   "Geometries are no longer posed with respect to other "
                   "geometries -- only frames; use GetPoseInFrame().")
  const math::RigidTransform<double>& GetPoseInParent(
      GeometryId geometry_id) const;

  /** Reports the pose of the geometry G with the given `geometry_id` in its
   registered frame F (regardless of whether its _topological parent_ is another
   geometry P or not). If the geometry was registered directly to the frame F,
   then `X_PG = X_FG`.
   <!-- 2023-04-01 When deprecation is complete, remove references to other
    geometries. -->
   @sa GetPoseInParent()
   @note For deformable geometries, this returns the pose of the reference mesh.
   @throws std::exception if `geometry_id` does not map to a registered
   geometry. */
  const math::RigidTransform<double>& GetPoseInFrame(
      GeometryId geometry_id) const;

  /** Returns the *mesh* used to represent this geometry in hydroelastic contact
   calculations, if it exists. Most primitives (sphere, cylinder, etc.) are
   actually represented by discrete approximations (i.e., the mesh). If there is
   no mesh, the returned variant will hold neither the
   TriangleSurfaceMesh<double> nor the VolumeMesh<double> alternatives. If
   either alternative is present, the pointer is guaranteed to be non-null.

   Just because hydroelastic properties have been assigned to a geometry does
   *not* mean there is necessarily a mesh associated with it. Some shape types
   (e.g., half space) have non-mesh representations.

   The result can be tested as follows:

   @code
     auto maybe_mesh = inspector.maybe_get_hydroelastic_mesh(id);

     // These two methods are equivalent, although testing index is more
     // brittle.
     const bool no_mesh1 = maybe_mesh.index() == 0;
     const bool no_mesh2 = std::holds_alternative<std::monostate>(maybe_mesh);
   @endcode

   @param geometry_id  The id of the geometry to query.
   @returns The associated mesh, if it exists. */
  std::variant<std::monostate, const TriangleSurfaceMesh<double>*,
               const VolumeMesh<double>*>
  maybe_get_hydroelastic_mesh(GeometryId geometry_id) const;

  /** Return a pointer to the const properties indicated by `role` of the
   geometry with the given `geometry_id`.
   @param geometry_id    The identifier for the queried geometry.
   @param role           The role whose properties are acquired.
   @return A pointer to the properties (or `nullptr` if there are no such
           properties).
   @throws std::exception if `geometry_id` does not map to a registered
           geometry.  */
  const GeometryProperties* GetProperties(GeometryId geometry_id,
                                          Role role) const;

  /** Returns a pointer to the const proximity properties of the geometry with
   the given `geometry_id`.
   @param geometry_id   The identifier for the queried geometry.
   @return A pointer to the properties (or `nullptr` if there are no such
           properties).
   @throws std::exception if `geometry_id` does not map to a registered
           geometry.  */
  const ProximityProperties* GetProximityProperties(
      GeometryId geometry_id) const;

  /** Returns a pointer to the const illustration properties of the geometry
   with the given `geometry_id`.
   @param geometry_id   The identifier for the queried geometry.
   @return A pointer to the properties (or `nullptr` if there are no such
           properties).
   @throws std::exception if `geometry_id` does not map to a registered
           geometry.  */
  const IllustrationProperties* GetIllustrationProperties(
      GeometryId geometry_id) const;

  /** Returns a pointer to the const perception properties of the geometry
   with the given `geometry_id`.
   @param geometry_id   The identifier for the queried geometry.
   @return A pointer to the properties (or `nullptr` if there are no such
           properties).
   @throws std::exception if `geometry_id` does not map to a registered
           geometry.  */
  const PerceptionProperties* GetPerceptionProperties(
      GeometryId geometry_id) const;

  /** Returns the reference mesh of the geometry with the given `geometry_id`,
   measured and expressed in the geometry's frame, G.
   @param geometry_id   The identifier for the queried geometry.
   @return A pointer to the reference mesh of the geometry if the geometry is
           deformable, or `nullptr` if the geometry is rigid.
   @note GetPoseInFrame() provides the transform necessary to measure and
         express the reference mesh's vertex positions in the geometry's
         registered frame F.
   @throws std::exception if `geometry_id` does not map to a registered
           geometry.  */
  const VolumeMesh<double>* GetReferenceMesh(GeometryId geometry_id) const;

  /** Returns true if the geometry with the given `geometry_id` is deformable.
   @param geometry_id   The identifier for the queried geometry.
   @throws std::exception if `geometry_id` does not map to a registered
           geometry.  */
  bool IsDeformableGeometry(GeometryId geometry_id) const;

  /** Returns all geometry ids that correspond to deformable geometries. */
  std::vector<GeometryId> GetAllDeformableGeometryIds() const;

  /** Reports true if the two geometries with given ids `geometry_id1` and
   `geometry_id2`, define a collision pair that has been filtered out.
   @throws std::exception if either id does not map to a registered geometry
                          or if any of the geometries do not have a proximity
                          role.  */
  bool CollisionFiltered(GeometryId geometry_id1,
                         GeometryId geometry_id2) const;

  /** Introspects the geometry indicated by the given `geometry_id`. The
   geometry will be passed into the provided `reifier`. This is the mechanism by
   which external code can discover and respond to the different types of
   geometries stored in SceneGraph. See ShapeToString as an example.
   @throws std::exception if the `geometry_id` does not refer to a valid
   geometry.  */
  void Reify(GeometryId geometry_id, ShapeReifier* reifier) const;

  /** Obtains a new GeometryInstance that copies the geometry indicated by the
   given `geometry_id`.
   @return A new GeometryInstance that is ready to be added as a new geometry.
           All roles/properties will be copied, the shape will be cloned based
           off of the original, but the returned id() will completely unique.
   @throws std::exception if the `geometry_id` does not refer to a valid
   geometry.  */
  std::unique_ptr<GeometryInstance>
  CloneGeometryInstance(GeometryId geometry_id) const;
  //@}

 private:
  // Only SceneGraph and QueryObject instances can construct
  // SceneGraphInspector instances.
  friend class SceneGraph<T>;
  friend class QueryObject<T>;

  // Testing utility.
  friend class SceneGraphInspectorTester;

  // Simple default constructor to be used by SceneGraph and QueryObject.
  SceneGraphInspector() = default;

  // Sets the scene graph data to inspect -- the inspector does _not_ own the
  // data and expects that the lifespan of the provided data is longer than
  // the inspector.
  void set(const GeometryState<T>* state) { state_ = state; }

  const GeometryState<T>* state_{nullptr};
};

}  // namespace geometry
}  // namespace drake