Modify diagram builder error message on algebraic loops (RobotLocomot…

…ion#7564)

* Modify diagram builder error message on algebraic loops

drake/systems/framework/diagram_builder.h

@@ -1,8 +1,10 @@
 #pragma once
 
+#include <algorithm>
 #include <map>
 #include <memory>
 #include <set>
+#include <sstream>
 #include <stdexcept>
 #include <unordered_set>
 #include <utility>
@@ -211,29 +213,29 @@ class DiagramBuilder {
   }
 
   // Helper method to do the algebraic loop test. It recursively performs the
-  // depth-first search on the graph to find cycles. The "stack" is really just
-  // a set because we need to do relatively "cheap" lookups for membership in
-  // the stack. The stack-like property of the set is maintained by this method.
+  // depth-first search on the graph to find cycles.
   static bool HasCycleRecurse(
       const PortIdentifier& n,
       const std::map<PortIdentifier, std::set<PortIdentifier>>& edges,
-      std::set<PortIdentifier>* visited, std::set<PortIdentifier>* stack) {
+      std::set<PortIdentifier>* visited,
+      std::vector<PortIdentifier>* stack) {
     DRAKE_ASSERT(visited->count(n) == 0);
     visited->insert(n);
 
     auto edge_iter = edges.find(n);
     if (edge_iter != edges.end()) {
-      DRAKE_ASSERT(stack->count(n) == 0);
-      stack->insert(n);  // Push onto the stack.
+      DRAKE_ASSERT(std::find(stack->begin(), stack->end(), n) == stack->end());
+      stack->push_back(n);
       for (const auto& target : edge_iter->second) {
         if (visited->count(target) == 0 &&
             HasCycleRecurse(target, edges, visited, stack)) {
           return true;
-        } else if (stack->count(target) > 0) {
+        } else if (std::find(stack->begin(), stack->end(), target) !=
+                   stack->end()) {
           return true;
         }
       }
-      stack->erase(n);  // Pop from the stack.
+      stack->pop_back();
     }
     return false;
   }
@@ -302,11 +304,29 @@ class DiagramBuilder {
 
     // Evaluate the graph for cycles.
     std::set<PortIdentifier> visited;
-    std::set<PortIdentifier> stack;
+    std::vector<PortIdentifier> stack;
     for (const auto& node : nodes) {
       if (visited.count(node) == 0) {
         if (HasCycleRecurse(node, edges, &visited, &stack)) {
-          throw std::logic_error("Algebraic loop detected in DiagramBuilder.");
+          std::stringstream ss;
+
+          auto port_to_stream = [&ss](const auto& id) {
+            ss << "  " << id.first->get_name() << ":";
+            if (id.second < 0)
+              ss << "Out(";
+            else
+              ss << "In(";
+            ss << (id.second >= 0 ? id.second : -id.second - 1) << ")";
+          };
+
+          ss << "Algebraic loop detected in DiagramBuilder:\n";
+          for (size_t i = 0; i < stack.size() - 1; ++i) {
+            port_to_stream(stack[i]);
+            ss << " depends on\n";
+          }
+          port_to_stream(stack.back());
+
+          throw std::runtime_error(ss.str());
         }
       }
     }

drake/systems/framework/test/diagram_builder_test.cc

@@ -1,5 +1,7 @@
 #include "drake/systems/framework/diagram_builder.h"
 
+#include <regex>
+
 #include <Eigen/Dense>
 #include <gtest/gtest.h>
 
@@ -14,6 +16,16 @@ namespace drake {
 namespace systems {
 namespace {
 
+#define EXPECT_ERROR_MESSAGE(expression, exception, reg_exp) \
+try { \
+  expression; \
+  GTEST_NONFATAL_FAILURE_("\t" #expression " failed to throw " #exception); \
+} catch (const exception& err) { \
+  auto matcher = [](const char* s, const std::string& re) { \
+    return std::regex_match(s, std::regex(re.c_str())); }; \
+  EXPECT_PRED2(matcher, err.what(), reg_exp); \
+}
+
 // Tests ::empty().
 GTEST_TEST(DiagramBuilderTest, Empty) {
   DiagramBuilder<double> builder;
@@ -69,10 +81,14 @@ class ConstAndEcho final : public LeafSystem<T> {
 GTEST_TEST(DiagramBuilderTest, AlgebraicLoop) {
   DiagramBuilder<double> builder;
   auto adder = builder.AddSystem<Adder>(1 /* inputs */, 1 /* size */);
-  adder->set_name("adder");
+  const std::string name{"adder"};
+  adder->set_name(name);
   // Connect the output port to the input port.
   builder.Connect(adder->get_output_port(), adder->get_input_port(0));
-  EXPECT_THROW(builder.Build(), std::logic_error);
+  EXPECT_ERROR_MESSAGE(builder.Build(), std::runtime_error,
+                       "Algebraic loop detected in DiagramBuilder:\n"
+                       "\\s+" + name + ":Out\\(0\\).*\n."
+                       "\\s*" + name + ":In\\(0\\)");
 }
 
 // Tests that a cycle which is not an algebraic loop is recognized as valid.
@@ -123,9 +139,13 @@ GTEST_TEST(DiagramBuilderTest, CycleAtLoopPortLevel) {
   // diagram has a cycle at the *system* level, but there is no algebraic loop.
 
   auto echo = builder.AddSystem<ConstAndEcho>();
-  echo->set_name("echo");
+  const std::string name{"echo"};
+  echo->set_name(name);
   builder.Connect(echo->get_echo_output_port(), echo->get_vec_input_port());
-  EXPECT_THROW(builder.Build(), std::logic_error);
+  EXPECT_ERROR_MESSAGE(builder.Build(), std::runtime_error,
+                       "Algebraic loop detected in DiagramBuilder:\n"
+                           "\\s+" + name + ":Out\\(0\\).*\n."
+                           "\\s*" + name + ":In\\(0\\)");
 }
 
 // Tests that a cycle which is not an algebraic loop is recognized as valid.