Add LCM Publish method that both encodes and sends (RobotLocomotion#8425

)

PR 7178 had many unfixed defects.
This is a first downpayment on fixing them.

A virtual method cannot have a default argument per GSG, but that PR added
one on Publish. Instead of fixing all of the call sites to pass 0.0 explicitly, we port
a major portion of them to some new sugar that both serializes and publishes,
which is a nicer approach anyway.

This also sets the stage for 8256 where subscription-related sugar methods will
be introduced.

examples/acrobot/test/acrobot_lcm_msg_generator.cc

@@ -63,10 +63,8 @@ int DoMain() {
 
   const std::string channel_x = "acrobot_xhat";
   const std::string channel_u = "acrobot_u";
-  lcmt_acrobot_x msg_x;
-  lcmt_acrobot_u msg_u;
-  std::vector<uint8_t> buffer_x(msg_x.getEncodedSize());
-  std::vector<uint8_t> buffer_u(msg_u.getEncodedSize());
+  lcmt_acrobot_x msg_x{};
+  lcmt_acrobot_u msg_u{};
 
   MessageHandler handler;
   lcm.Subscribe(channel_x, &handler);
@@ -79,9 +77,7 @@ int DoMain() {
     msg_x.theta2 = t * M_PI / 6;
     msg_x.theta1Dot = std::sin(msg_x.theta1);
     msg_x.theta2Dot = std::cos(msg_x.theta2);
-
-    msg_x.encode(&buffer_x[0], 0, msg_x.getEncodedSize());
-    lcm.Publish(channel_x, &buffer_x[0], msg_x.getEncodedSize());
+    Publish(&lcm, channel_x, msg_x);
 
     // Publishes msg_u using received msg_x.
     if (handler.get_receive_channel() == channel_x) {
@@ -92,8 +88,7 @@ int DoMain() {
       msg_u.tau = received_msg.theta1 + received_msg.theta2;
 
       // Publish msg_u.
-      msg_u.encode(&buffer_u[0], 0, msg_u.getEncodedSize());
-      lcm.Publish(channel_u, &buffer_u[0], msg_u.getEncodedSize());
+      Publish(&lcm, channel_u, msg_u);
     }
     sleep_for(milliseconds(500));
     t++;

examples/rod2d/rod2d_sim.cc

@@ -55,7 +55,6 @@ int main(int argc, char* argv[]) {
 
   // Emit a one-time load message.
   Serializer<drake::lcmt_viewer_load_robot> load_serializer;
-  std::vector<uint8_t> message_bytes;
 
   // TODO(edrumwri): Remove the DRAKE_VIEWER_DRAW, DRAKE_VIEWER_LOAD_ROBOT
   //                 magic strings as soon as they are a named constant within
@@ -94,7 +93,7 @@ int main(int argc, char* argv[]) {
       Eigen::Isometry3d::Identity(), Eigen::Vector4d(0.7, 0.7, 0.7, 1));
 
   // Create the load message.
-  drake::lcmt_viewer_load_robot message;
+  drake::lcmt_viewer_load_robot message{};
   message.num_links = 1;
   message.link.resize(1);
   message.link.back().name = "rod";
@@ -104,11 +103,7 @@ int main(int argc, char* argv[]) {
   message.link.back().geom[0] = MakeGeometryData(rod_vis);
 
   // Send a load mesage.
-  const int message_length = message.getEncodedSize();
-  message_bytes.resize(message_length);
-  message.encode(message_bytes.data(), 0, message_length);
-  lcm.Publish("DRAKE_VIEWER_LOAD_ROBOT", message_bytes.data(),
-              message_bytes.size());
+  Publish(&lcm, "DRAKE_VIEWER_LOAD_ROBOT", message);
 
   // Set the names of the systems.
   rod->set_name("rod");

examples/valkyrie/test/valkyrie_ik_test.cc

@@ -265,17 +265,13 @@ GTEST_TEST(ValkyrieIK_Test, ValkyrieIK_Test_StandingPose_Test) {
   systems::BasicVector<double> q_draw(x);
 
   lcm::DrakeLcm lcm;
-  std::vector<uint8_t> message_bytes;
 
   lcmt_viewer_load_robot load_msg =
       multibody::CreateLoadRobotMessage<double>(*tree);
-  const int length = load_msg.getEncodedSize();
-  message_bytes.resize(length);
-  load_msg.encode(message_bytes.data(), 0, length);
-  lcm.Publish("DRAKE_VIEWER_LOAD_ROBOT", message_bytes.data(),
-              message_bytes.size());
+  Publish(&lcm, "DRAKE_VIEWER_LOAD_ROBOT", load_msg);
 
   systems::ViewerDrawTranslator posture_drawer(*tree);
+  std::vector<uint8_t> message_bytes;
   posture_drawer.Serialize(0, q_draw, &message_bytes);
   lcm.Publish("DRAKE_VIEWER_DRAW", message_bytes.data(),
               message_bytes.size());

geometry/geometry_visualization.cc

@@ -179,12 +179,8 @@ void DispatchLoadMessage(const GeometryState<double>& state) {
   }
 
   // Send a load message.
-  const int message_length = message.getEncodedSize();
-  std::vector<uint8_t> message_bytes(message_length);
-  message.encode(message_bytes.data(), 0, message_length);
   DrakeLcm lcm;
-  lcm.Publish("DRAKE_VIEWER_LOAD_ROBOT", message_bytes.data(),
-              message_bytes.size());
+  Publish(&lcm, "DRAKE_VIEWER_LOAD_ROBOT", message);
 }
 
 void DispatchLoadMessage(const GeometrySystem<double>& system) {

lcm/BUILD.bazel

@@ -82,6 +82,14 @@ drake_cc_library(
 
 # === test/ ===
 
+drake_cc_googletest(
+    name = "drake_lcm_interface_test",
+    deps = [
+        ":lcmt_drake_signal_utils",
+        ":mock",
+    ],
+)
+
 drake_cc_googletest(
     name = "drake_lcm_test",
     # Flaky because LCM self-test can fail (PR #7311)

lcm/drake_lcm_interface.h

@@ -1,9 +1,13 @@
 #pragma once
 
+#include <cstdint>
 #include <limits>
 #include <string>
+#include <vector>
 
 #include "drake/common/drake_copyable.h"
+#include "drake/common/drake_optional.h"
+#include "drake/common/drake_throw.h"
 #include "drake/lcm/drake_lcm_message_handler_interface.h"
 
 namespace drake {
@@ -64,5 +68,31 @@ class DrakeLcmInterface {
   }
 };
 
+/**
+ * Publishes an LCM message on channel @p channel.
+ *
+ * @param lcm The LCM service on which to publish the message.
+ * Must not be null.
+ *
+ * @param channel The channel on which to publish the message.
+ * Must not be the empty string.
+ *
+ * @param message The message to publish.
+ *
+ * @param time_sec Time in seconds when the publish event occurred.
+ * If unknown, use the default value of drake::nullopt.
+ */
+template <typename Message>
+void Publish(DrakeLcmInterface* lcm, const std::string& channel,
+             const Message& message, optional<double> time_sec = {}) {
+  DRAKE_THROW_UNLESS(lcm != nullptr);
+  const int num_bytes = message.getEncodedSize();
+  DRAKE_THROW_UNLESS(num_bytes >= 0);
+  const size_t size_bytes = static_cast<size_t>(num_bytes);
+  std::vector<uint8_t> bytes(size_bytes);
+  message.encode(bytes.data(), 0, num_bytes);
+  lcm->Publish(channel, bytes.data(), num_bytes, time_sec.value_or(0.0));
+}
+
 }  // namespace lcm
 }  // namespace drake

lcm/test/drake_lcm_interface_test.cc

@@ -0,0 +1,34 @@
+#include "drake/lcm/drake_lcm_interface.h"
+
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "drake/lcm/drake_mock_lcm.h"
+#include "drake/lcm/lcmt_drake_signal_utils.h"
+#include "drake/lcmt_drake_signal.hpp"
+
+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));
+}
+
+}  // namespace
+}  // namespace lcm
+}  // namespace drake

lcm/test/drake_lcm_log_test.cc

@@ -28,7 +28,7 @@ class TestHandler : public DrakeLcmMessageHandlerInterface {
 
  private:
   const std::string name_;
-  MsgType msg_;
+  MsgType msg_{};
 };
 
 // Generates a log file using the write-only interface, then plays it back
@@ -37,18 +37,14 @@ GTEST_TEST(LcmLogTest, LcmLogTestSaveAndRead) {
   auto w_log = std::make_unique<DrakeLcmLog>("test.log", true);
   const std::string channel_name("test_channel");
 
-  drake::lcmt_drake_signal msg;
+  drake::lcmt_drake_signal msg{};
   msg.dim = 1;
   msg.val.push_back(0.1);
   msg.coord.push_back("test");
   msg.timestamp = 1234;
 
-  std::vector<uint8_t> buffer(msg.getEncodedSize());
-  EXPECT_EQ(msg.encode(&buffer[0], 0, msg.getEncodedSize()),
-            msg.getEncodedSize());
-
   const double log_time = 111;
-  w_log->Publish(channel_name, buffer.data(), buffer.size(), log_time);
+  Publish(w_log.get(), channel_name, msg, log_time);
   // Finish writing.
   w_log.reset();
 

lcm/test/drake_lcm_test.cc

@@ -93,10 +93,6 @@ TEST_F(DrakeLcmTest, PublishTest) {
 
   MessageSubscriber subscriber(channel_name, dut.get_lcm_instance());
 
-  std::vector<uint8_t> buffer(message_.getEncodedSize());
-  EXPECT_EQ(message_.encode(&buffer[0], 0, message_.getEncodedSize()),
-            message_.getEncodedSize());
-
   // Start the LCM recieve thread after all objects it can potentially use like
   // subscribers are instantiated. Since objects are destructed in the reverse
   // order of construction, this ensures the LCM receive thread stops before any
@@ -120,7 +116,7 @@ TEST_F(DrakeLcmTest, PublishTest) {
   // We must periodically call dut.Publish(...) since we do not know when the
   // receiver will actually receive the message.
   while (!done && count++ < kMaxCount) {
-    dut.Publish(channel_name, &buffer[0], message_.getEncodedSize());
+    Publish(&dut, channel_name, message_);
 
     // Gets the received message.
     const drake::lcmt_drake_signal received_message =
@@ -226,8 +222,8 @@ TEST_F(DrakeLcmTest, EmptyChannelTest) {
   MessageHandler handler;
   EXPECT_THROW(dut.Subscribe("", &handler), std::exception);
 
-  const uint8_t buffer{};
-  EXPECT_THROW(dut.Publish("", &buffer, 1), std::exception);
+  lcmt_drake_signal message{};
+  EXPECT_THROW(Publish(&dut, "", message), std::exception);
 }
 
 }  // namespace

lcm/test/drake_mock_lcm_test.cc

@@ -43,7 +43,7 @@ GTEST_TEST(DrakeMockLcmTest, PublishTest) {
 // message.
 GTEST_TEST(DrakeMockLcmTest, DecodeLastPublishedMessageAsTest) {
   const string channel_name = "DecodeLastPublishedMessageAsTestChannel";
-  lcmt_drake_signal original_message;
+  lcmt_drake_signal original_message{};
   original_message.dim = 1;
   original_message.val.push_back(3.14);
   original_message.coord.push_back("foo");
@@ -213,8 +213,8 @@ GTEST_TEST(DrakeMockLcmTest, EmptyChannelTest) {
   MockMessageHandler handler;
   EXPECT_THROW(dut.Subscribe("", &handler), std::exception);
 
-  const uint8_t buffer{};
-  EXPECT_THROW(dut.Publish("", &buffer, 1), std::exception);
+  lcmt_drake_signal message{};
+  EXPECT_THROW(Publish(&dut, "", message), std::exception);
 }
 
 }  // namespace

lcm/test/lcmt_drake_signal_utils_test.cc

@@ -27,8 +27,8 @@ class LcmtDrakeSignalUtilsTest : public ::testing::Test {
     message2_ = message1_;
   }
 
-  drake::lcmt_drake_signal message1_;
-  drake::lcmt_drake_signal message2_;
+  drake::lcmt_drake_signal message1_{};
+  drake::lcmt_drake_signal message2_{};
 };
 
 // Tests CompareLcmtDrakeSignalMessages()'s ability to determine that two

manipulation/util/simple_tree_visualizer.cc

@@ -25,13 +25,7 @@ SimpleTreeVisualizer::SimpleTreeVisualizer(const RigidBodyTreed& tree,
   const lcmt_viewer_load_robot load_message(
       multibody::CreateLoadRobotMessage<double>(tree_));
 
-  const int lcm_message_length = load_message.getEncodedSize();
-  std::vector<uint8_t> lcm_message_bytes{};
-  lcm_message_bytes.resize(lcm_message_length);
-  load_message.encode(lcm_message_bytes.data(), 0, lcm_message_length);
-
-  lcm_->Publish("DRAKE_VIEWER_LOAD_ROBOT", lcm_message_bytes.data(),
-                lcm_message_length);
+  Publish(lcm_, "DRAKE_VIEWER_LOAD_ROBOT", load_message);
 }
 
 void SimpleTreeVisualizer::visualize(const VectorX<double>& position_vector) {

multibody/rigid_body_plant/drake_visualizer.cc

@@ -96,7 +96,7 @@ void DrakeVisualizer::PlaybackTrajectory(
 
     // Publishes onto the specified LCM channel.
     lcm_->Publish("DRAKE_VIEWER_DRAW", message_bytes.data(),
-                  message_bytes.size());
+                  message_bytes.size(), sim_time);
 
     const TimePoint earliest_next_frame = prev_time + Duration(kFrameLength);
     std::this_thread::sleep_until(earliest_next_frame);
@@ -111,7 +111,7 @@ void DrakeVisualizer::PlaybackTrajectory(
   std::vector<uint8_t> message_bytes;
   draw_message_translator_.Serialize(sim_time, data, &message_bytes);
   lcm_->Publish("DRAKE_VIEWER_DRAW", message_bytes.data(),
-                message_bytes.size());
+                message_bytes.size(), sim_time);
 }
 
 void DrakeVisualizer::DoPublish(
@@ -140,17 +140,11 @@ void DrakeVisualizer::DoPublish(
 
   // Publishes onto the specified LCM channel.
   lcm_->Publish("DRAKE_VIEWER_DRAW", message_bytes.data(),
-                message_bytes.size());
+                message_bytes.size(), context.get_time());
 }
 
 void DrakeVisualizer::PublishLoadRobot() const {
-  const int lcm_message_length = load_message_.getEncodedSize();
-  std::vector<uint8_t> lcm_message_bytes{};
-  lcm_message_bytes.resize(lcm_message_length);
-  load_message_.encode(lcm_message_bytes.data(), 0, lcm_message_length);
-
-  lcm_->Publish("DRAKE_VIEWER_LOAD_ROBOT", lcm_message_bytes.data(),
-                lcm_message_length);
+  drake::lcm::Publish(lcm_, "DRAKE_VIEWER_LOAD_ROBOT", load_message_);
 }
 
 }  // namespace systems

multibody/rigid_body_plant/frame_visualizer.cc

@@ -51,10 +51,7 @@ void FrameVisualizer::DoPublish(
     msg.quaternion[i][3] = static_cast<float>(quat.z());
   }
 
-  const int length = msg.getEncodedSize();
-  std::vector<uint8_t> msgbytes(length);
-  msg.encode(msgbytes.data(), 0, length);
-  lcm_->Publish(lcm_channel_, msgbytes.data(), msgbytes.size());
+  drake::lcm::Publish(lcm_, lcm_channel_, msg, context.get_time());
 }
 
 }  // namespace systems

perception/estimators/dev/test/articulated_icp_test.cc

@@ -47,9 +47,7 @@ class ArticulatedIcpVisualizer : public PointCloudVisualizer {
   void Init() {
     const lcmt_viewer_load_robot load_msg(
         (multibody::CreateLoadRobotMessage<double>(scene_->tree())));
-    vector<uint8_t> bytes(load_msg.getEncodedSize());
-    load_msg.encode(bytes.data(), 0, bytes.size());
-    lcm().Publish("DRAKE_VIEWER_LOAD_ROBOT", bytes.data(), bytes.size());
+    Publish(&lcm(), "DRAKE_VIEWER_LOAD_ROBOT", load_msg);
   }
   void PublishScene(const SceneState& scene_state) {
     const ViewerDrawTranslator draw_msg_tx(scene_->tree());

perception/estimators/dev/test/test_util.cc

@@ -239,9 +239,7 @@ void PointCloudVisualizer::PublishCloud(const Matrix3Xd& points,
                                  const string& suffix) {
   bot_core::pointcloud_t pt_msg{};
   PointCloudToLcm(points, &pt_msg);
-  vector<uint8_t> bytes(pt_msg.getEncodedSize());
-  pt_msg.encode(bytes.data(), 0, bytes.size());
-  lcm_.Publish("DRAKE_POINTCLOUD_" + suffix, bytes.data(), bytes.size());
+  Publish(&lcm_, "DRAKE_POINTCLOUD_" + suffix, pt_msg);
 }
 
 void PointCloudVisualizer::PublishFrames(
@@ -267,9 +265,7 @@ void PointCloudVisualizer::PublishFrames(
     msg.quaternion[i][2] = quat.y();
     msg.quaternion[i][3] = quat.z();
   }
-  vector<uint8_t> bytes(msg.getEncodedSize());
-  msg.encode(bytes.data(), 0, bytes.size());
-  lcm_.Publish("DRAKE_DRAW_FRAMES", bytes.data(), bytes.size());
+  Publish(&lcm_, "DRAKE_DRAW_FRAMES", msg);
 }
 
 }  // namespace estimators