[geometry] Improve SceneGraph error for unconnected input ports (Robo…

…tLocomotion#14773)

* Correct SceneGraph error for unconnected input ports

The old error message strictly reported source *id*, which is not
incredibly helpful for the end user. So, now, it includes both
source id and registered name. It further elaborates on the
condition when not connecting is erroneous.

It also removes the lambda function which served purely a
speculative role -- it was waiting for the day when velocities or
accelerations were also available. When we start consuming
velocities, we can restore the generality.

geometry/scene_graph.cc

@@ -475,13 +475,6 @@ void SceneGraph<T>::CalcPoseUpdate(const Context<T>& context,
   const GeometryState<T>& state = geometry_state(context);
   GeometryState<T>& mutable_state = const_cast<GeometryState<T>&>(state);
 
-  auto throw_error = [](SourceId source_id, const std::string& origin) {
-    throw std::logic_error("Source " + to_string(source_id) +
-                           " has registered frames "
-                           "but does not provide " +
-                           origin + " values on the input port.");
-  };
-
   // Process all sources *except*:
   //   - the internal source and
   //   - sources with no frames.
@@ -494,7 +487,10 @@ void SceneGraph<T>::CalcPoseUpdate(const Context<T>& context,
       if (itr != input_source_ids_.end()) {
         const auto& pose_port = this->get_input_port(itr->second.pose_port);
         if (!pose_port.HasValue(context)) {
-          throw_error(source_id, "pose");
+          throw std::logic_error(
+              fmt::format("Source '{}' (id: {}) has registered dynamic frames "
+                          "but is not connected to the appropriate input port.",
+                          state.GetName(source_id), source_id));
         }
         const auto& poses =
             pose_port.template Eval<FramePoseVector<T>>(context);

geometry/test/scene_graph_test.cc

@@ -3,6 +3,7 @@
 #include <memory>
 #include <utility>
 
+#include <fmt/format.h>
 #include <gtest/gtest.h>
 
 #include "drake/common/test_utilities/expect_no_throw.h"
@@ -499,7 +500,7 @@ class GeometrySourceSystem : public systems::LeafSystem<double> {
   explicit GeometrySourceSystem(SceneGraph<double>* scene_graph)
       : systems::LeafSystem<double>(), scene_graph_(scene_graph) {
     // Register with SceneGraph.
-    source_id_ = scene_graph->RegisterSource();
+    source_id_ = scene_graph->RegisterSource(kRegisteredSourceName);
     FrameId f_id =
         scene_graph->RegisterFrame(source_id_, GeometryFrame("frame"));
     frame_ids_.push_back(f_id);
@@ -524,6 +525,8 @@ class GeometrySourceSystem : public systems::LeafSystem<double> {
   // addition to all of the default poses.
   void add_extra_pose() { extra_poses_.push_back(RigidTransformd()); }
 
+  static constexpr char kRegisteredSourceName[] = "source_system";
+
  private:
   // Populate with the pose data.
   void CalcFramePoseOutput(const Context<double>& context,
@@ -585,8 +588,9 @@ GTEST_TEST(SceneGraphConnectionTest, FullPoseUpdateDisconnected) {
   DRAKE_EXPECT_THROWS_MESSAGE(
       SceneGraphTester::FullPoseUpdate(*scene_graph, sg_context),
       std::logic_error,
-      "Source \\d+ has registered frames but does not provide pose values on "
-      "the input port.");
+      fmt::format("Source '{}' \\(id: \\d+\\) has registered dynamic frames "
+                  "but is not connected .+",
+                  GeometrySourceSystem::kRegisteredSourceName));
 }
 
 // Adversarial test case: Missing all port connections.
@@ -605,8 +609,9 @@ GTEST_TEST(SceneGraphConnectionTest, FullPoseUpdateNoConnections) {
   DRAKE_EXPECT_THROWS_MESSAGE(
       SceneGraphTester::FullPoseUpdate(*scene_graph, sg_context),
       std::logic_error,
-      "Source \\d+ has registered frames but does not provide pose values on "
-      "the input port.");
+      fmt::format("Source '{}' \\(id: \\d+\\) has registered dynamic frames "
+                  "but is not connected .+",
+                  GeometrySourceSystem::kRegisteredSourceName));
 }
 
 // Confirms that the SceneGraph can be instantiated on AutoDiff type.