Add lcm::Subscribe sugar that decodes (RobotLocomotion#8487)

examples/acrobot/test/acrobot_lcm_msg_generator.cc

@@ -27,9 +27,9 @@ int DoMain() {
   // Decode channel_x into msg_x.
   std::mutex msg_x_mutex;  // Guards msg_x.
   lcmt_acrobot_x msg_x{};
-  lcm.Subscribe(channel_x, [&](const void* buffer, int size) {
+  lcm::Subscribe<lcmt_acrobot_x>(&lcm, channel_x, [&](const auto& received) {
     std::lock_guard<std::mutex> lock(msg_x_mutex);
-    msg_x.decode(buffer, 0, size);
+    msg_x = received;
   });
 
   lcm.StartReceiveThread();

lcm/BUILD.bazel

@@ -87,6 +87,7 @@ drake_cc_googletest(
     deps = [
         ":lcmt_drake_signal_utils",
         ":mock",
+        "//common/test_utilities",
     ],
 )
 

lcm/drake_lcm_interface.h

@@ -3,6 +3,7 @@
 #include <cstdint>
 #include <functional>
 #include <limits>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -96,5 +97,42 @@ void Publish(DrakeLcmInterface* lcm, const std::string& channel,
   lcm->Publish(channel, bytes.data(), num_bytes, time_sec);
 }
 
+/**
+ * Subscribes to an LCM channel named @p channel and decodes messages of type
+ * @p Message.
+ *
+ * @param lcm The LCM service on which to subscribe.
+ * Must not be null.
+ *
+ * @param channel The channel on which to subscribe.
+ * Must not be the empty string.
+ *
+ * @param handler The callback when a message is received and decoded without
+ * error.
+ *
+ * @param on_error The callback when a message is received and cannot be
+ * decoded; if no error handler is given, an exception is thrown instead.
+ *
+ * @note Depending on the specific DrakeLcmInterface implementation, the
+ * handler might be invoked on a different thread than this function.
+ */
+template <typename Message>
+void Subscribe(DrakeLcmInterface* lcm, const std::string& channel,
+               std::function<void(const Message&)> handler,
+               std::function<void()> on_error = {}) {
+  DRAKE_THROW_UNLESS(lcm != nullptr);
+  lcm->Subscribe(channel, [=](const void* bytes, int size) {
+    Message received{};
+    const int size_decoded = received.decode(bytes, 0, size);
+    if (size_decoded == size) {
+      handler(received);
+    } else if (on_error) {
+      on_error();
+    } else {
+      throw std::runtime_error("Error decoding message on " + channel);
+    }
+  });
+}
+
 }  // namespace lcm
 }  // namespace drake

lcm/test/drake_lcm_interface_test.cc

@@ -1,10 +1,11 @@
 #include "drake/lcm/drake_lcm_interface.h"
 
 #include <string>
-#include <vector>
 
 #include <gtest/gtest.h>
 
+#include "drake/common/drake_assert.h"
+#include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/lcm/drake_mock_lcm.h"
 #include "drake/lcm/lcmt_drake_signal_utils.h"
 #include "drake/lcmt_drake_signal.hpp"
@@ -13,20 +14,89 @@ namespace drake {
 namespace lcm {
 namespace {
 
-GTEST_TEST(DrakeLcmInterfaceTest, PublishTest) {
-  // Publish using the helper function.
-  DrakeMockLcm lcm;
-  const std::string name = "NAME";
-  lcmt_drake_signal original{};
-  original.timestamp = 123;
-  Publish(&lcm, name, original);
-
-  // Make sure it came out okay.  (Manually decode so as to be slightly less
-  // dependent on the DrakeMockLcm sugar.)
-  const std::vector<uint8_t>& bytes = lcm.get_last_published_message(name);
-  lcmt_drake_signal decoded{};
-  EXPECT_EQ(decoded.decode(bytes.data(), 0, bytes.size()), bytes.size());
-  EXPECT_TRUE(CompareLcmtDrakeSignalMessages(decoded, original));
+class DrakeLcmInterfaceTest : public ::testing::Test {
+ public:
+  using Message = lcmt_drake_signal;
+
+  DrakeLcmInterfaceTest() {
+    lcm_.EnableLoopBack();
+
+    sample_.timestamp = 123;
+    sample_.dim = 1;
+    sample_.coord.emplace_back("x");
+    sample_.val.emplace_back(1.0);
+
+    const int num_bytes = sample_.getEncodedSize();
+    DRAKE_DEMAND(num_bytes >= 0);
+    sample_bytes_.resize(static_cast<size_t>(num_bytes));
+    sample_.encode(sample_bytes_.data(), 0, num_bytes);
+  }
+
+ protected:
+  DrakeMockLcm lcm_;
+  const std::string channel_ = "NAME";
+
+  // A convenient, populated sample message and its encoded form.
+  Message sample_{};
+  std::vector<uint8_t> sample_bytes_;
+};
+
+TEST_F(DrakeLcmInterfaceTest, FreeFunctionTest) {
+  // Subscribe using the helper free-function.
+  Message received{};
+  Subscribe<Message>(&lcm_, channel_, [&](const Message& message) {
+    received = message;
+  });
+
+  // Publish using the helper free-function.
+  Publish(&lcm_, channel_, sample_);
+  EXPECT_TRUE(CompareLcmtDrakeSignalMessages(received, sample_));
+}
+
+TEST_F(DrakeLcmInterfaceTest, DefaultErrorHandlingTest) {
+  // Subscribe using the helper free-function, using default error-handling.
+  Message received{};
+  Subscribe<Message>(&lcm_, channel_, [&](const Message& message) {
+    received = message;
+  });
+
+  // Publish successfully.
+  lcm_.Publish(channel_, sample_bytes_.data(), sample_bytes_.size(), {});
+  EXPECT_TRUE(CompareLcmtDrakeSignalMessages(received, sample_));
+  received = {};
+
+  // Corrupt the message.
+  std::vector<uint8_t> corrupt_bytes = sample_bytes_;
+  corrupt_bytes.at(0) = 0;
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      lcm_.Publish(channel_, corrupt_bytes.data(), corrupt_bytes.size(), {}),
+      std::runtime_error,
+      "Error decoding message on NAME");
+  EXPECT_TRUE(CompareLcmtDrakeSignalMessages(received, Message{}));
+}
+
+TEST_F(DrakeLcmInterfaceTest, CustomErrorHandlingTest) {
+  // Subscribe using the helper free-function, using default error-handling.
+  Message received{};
+  bool error = false;
+  Subscribe<Message>(&lcm_, channel_, [&](const Message& message) {
+    received = message;
+  }, [&]() {
+    error = true;
+  });
+
+  // Publish successfully.
+  lcm_.Publish(channel_, sample_bytes_.data(), sample_bytes_.size(), {});
+  EXPECT_TRUE(CompareLcmtDrakeSignalMessages(received, sample_));
+  EXPECT_FALSE(error);
+  received = {};
+
+  // Corrupt the message.
+  std::vector<uint8_t> corrupt_bytes = sample_bytes_;
+  corrupt_bytes.at(0) = 0;
+  lcm_.Publish(channel_, corrupt_bytes.data(), corrupt_bytes.size(), {});
+  EXPECT_TRUE(CompareLcmtDrakeSignalMessages(received, Message{}));
+  EXPECT_TRUE(error);
 }
 
 }  // namespace