Added options to specifiy the publish triggers for LcmPublisherSystem. (

RobotLocomotion#10721)

* Added options to specifiy the publish triggers for LcmPublisherSystem.

* Removed ambiguous overload.

* Reworked constructor signatures per feedback.

* Nit style fixes.

* Hopefully fixed pydoc issue.

* Hopefully fixed pydoc issue.

* Hopeful fix for xenial

* Changes from peer review.

* Changes from peer review.

* 0 -> 0.0

bindings/pydrake/systems/lcm_py.cc

@@ -148,7 +148,7 @@ PYBIND11_MODULE(lcm, m) {
             py::arg("channel"), py::arg("serializer"), py::arg("lcm"),
             py::arg("publish_period") = 0.0,
             // Keep alive: `self` keeps `DrakeLcmInterface` alive.
-            py::keep_alive<1, 3>(), doc.LcmPublisherSystem.ctor.doc)
+            py::keep_alive<1, 3>(), doc.LcmPublisherSystem.ctor.doc_4args)
         .def("set_publish_period",
             [](Class* self, double period) {
               WarnDeprecated("set_publish_period() is deprecated");

systems/lcm/lcm_publisher_system.cc

@@ -29,7 +29,9 @@ LcmPublisherSystem::LcmPublisherSystem(
     const LcmAndVectorBaseTranslator* translator,
     std::unique_ptr<const LcmAndVectorBaseTranslator> owned_translator,
     std::unique_ptr<SerializerInterface> serializer,
-    DrakeLcmInterface* lcm, double publish_period)
+    DrakeLcmInterface* lcm,
+    const TriggerTypeSet& publish_triggers,
+    double publish_period)
     : channel_(channel),
       translator_(owned_translator ? owned_translator.get() : translator),
       owned_translator_(std::move(owned_translator)),
@@ -39,11 +41,21 @@ LcmPublisherSystem::LcmPublisherSystem(
   DRAKE_DEMAND((translator_ != nullptr) != (serializer_.get() != nullptr));
   DRAKE_DEMAND(lcm_);
   DRAKE_DEMAND(publish_period >= 0.0);
+  DRAKE_DEMAND(!publish_triggers.empty());
+
+  // Check that publish_triggers does not contain an unsupported trigger.
+  for (const auto& trigger : publish_triggers) {
+      DRAKE_THROW_UNLESS((trigger == TriggerType::kForced) ||
+        (trigger == TriggerType::kPeriodic) ||
+        (trigger == TriggerType::kPerStep));
+  }
 
   // Declare a forced publish so that any time Publish(.) is called on this
   // system (or a Diagram containing it), a message is emitted.
-  this->DeclareForcedPublishEvent(
-    &LcmPublisherSystem::PublishInputAsLcmMessage);
+  if (publish_triggers.find(TriggerType::kForced) != publish_triggers.end()) {
+    this->DeclareForcedPublishEvent(
+      &LcmPublisherSystem::PublishInputAsLcmMessage);
+  }
 
   if (translator_ != nullptr) {
     // If the translator provides a specific storage type (i.e., if it returns
@@ -63,37 +75,63 @@ LcmPublisherSystem::LcmPublisherSystem(
   }
 
   set_name(make_name(channel_));
-
-  if (publish_period > 0.0) {
-    this->disable_internal_per_step_publish_events_ = true;
+  if (publish_triggers.find(TriggerType::kPeriodic) != publish_triggers.end()) {
+    DRAKE_THROW_UNLESS(publish_period > 0.0);
     const double offset = 0.0;
     this->DeclarePeriodicPublishEvent(
         publish_period, offset,
         &LcmPublisherSystem::PublishInputAsLcmMessage);
   } else {
+    // publish_period > 0 without TriggerType::kPeriodic has no meaning and is
+    // likely a mistake.
+    DRAKE_THROW_UNLESS(publish_period == 0.0);
+  }
+
+  if (publish_triggers.find(TriggerType::kPerStep) != publish_triggers.end()) {
     this->DeclarePerStepEvent(
-        systems::PublishEvent<double>([this](
-            const systems::Context<double>& context,
-            const systems::PublishEvent<double>&) {
-          // TODO(edrumwri) Remove this code once set_publish_period(.) has
-          // been removed; it exists so that one does not get both a per-step
-          // publish and a periodic publish if a user constructs the publisher
-          // the "old" way (construction followed by set_publish_period()).
-          if (this->disable_internal_per_step_publish_events_)
-            return;
-
-          this->PublishInputAsLcmMessage(context);
-        }));
+    systems::PublishEvent<double>([this](
+        const systems::Context<double>& context,
+        const systems::PublishEvent<double>&) {
+      // TODO(edrumwri) Remove this code once set_publish_period(.) has
+      // been removed; it exists so that one does not get both a per-step
+      // publish and a periodic publish if a user constructs the publisher
+      // the "old" way (construction followed by set_publish_period()).
+      if (this->disable_internal_per_step_publish_events_)
+        return;
+
+      this->PublishInputAsLcmMessage(context);
+    }));
   }
 }
 
+LcmPublisherSystem::LcmPublisherSystem(
+    const std::string& channel,
+    const LcmAndVectorBaseTranslator* translator,
+    std::unique_ptr<const LcmAndVectorBaseTranslator> owned_translator,
+    std::unique_ptr<SerializerInterface> serializer,
+    DrakeLcmInterface* lcm, double publish_period)
+    : LcmPublisherSystem(channel, translator, std::move(owned_translator),
+      std::move(serializer), lcm,
+      (publish_period > 0.0) ?
+      TriggerTypeSet({TriggerType::kForced, TriggerType::kPeriodic}) :
+      TriggerTypeSet({TriggerType::kForced, TriggerType::kPerStep}),
+      publish_period) {}
+
 LcmPublisherSystem::LcmPublisherSystem(
     const std::string& channel, std::unique_ptr<SerializerInterface> serializer,
     drake::lcm::DrakeLcmInterface* lcm,
     double publish_period)
     : LcmPublisherSystem(channel, nullptr, nullptr, std::move(serializer),
                          lcm, publish_period) {}
 
+LcmPublisherSystem::LcmPublisherSystem(
+    const std::string& channel, std::unique_ptr<SerializerInterface> serializer,
+    drake::lcm::DrakeLcmInterface* lcm,
+    const TriggerTypeSet& publish_triggers,
+    double publish_period)
+    : LcmPublisherSystem(channel, nullptr, nullptr, std::move(serializer),
+                         lcm, publish_triggers, publish_period) {}
+
 LcmPublisherSystem::LcmPublisherSystem(
     const std::string& channel,
     const LcmAndVectorBaseTranslator& translator,

systems/lcm/lcm_publisher_system.h

@@ -2,12 +2,15 @@
 
 #include <memory>
 #include <string>
+#include <unordered_set>
 #include <vector>
 
 #include "drake/common/drake_copyable.h"
 #include "drake/common/drake_deprecated.h"
 #include "drake/common/drake_throw.h"
+#include "drake/common/hash.h"
 #include "drake/lcm/drake_lcm_interface.h"
+#include "drake/systems/framework/event.h"
 #include "drake/systems/framework/leaf_system.h"
 #include "drake/systems/lcm/lcm_and_vector_base_translator.h"
 #include "drake/systems/lcm/lcm_translator_dictionary.h"
@@ -23,6 +26,8 @@ class DrakeLcm;
 namespace systems {
 namespace lcm {
 
+using TriggerTypeSet = std::unordered_set<TriggerType, DefaultHash>;
+
 /**
  * Publishes an LCM message containing information from its input port.
  * Optionally sends a one-time initialization message. Publishing can be set up
@@ -48,6 +53,10 @@ class LcmPublisherSystem : public LeafSystem<double> {
    * A factory method that returns an %LcmPublisherSystem that takes
    * Value<LcmMessage> message objects on its sole abstract-valued input port.
    *
+   * Sets the default set of publish triggers: 
+   *   if publish_period = 0, publishes on forced events and per step,
+   *   if publish_period > 0, publishes on forced events and periodically.
+   *
    * @tparam LcmMessage message type to serialize, e.g., lcmt_drake_signal.
    *
    * @param[in] channel The LCM channel on which to publish.
@@ -73,10 +82,49 @@ class LcmPublisherSystem : public LeafSystem<double> {
         publish_period);
   }
 
+  /**
+   * A factory method for an %LcmPublisherSystem that takes LCM message objects
+   * on its sole abstract-valued input port. The LCM message type is determined
+   * by the provided `serializer`.
+   *
+   * @param[in] channel The LCM channel on which to publish.
+   *
+   * @param[in] serializer The serializer that converts between byte vectors
+   * and LCM message objects.
+   *
+   * @param lcm A pointer to the LCM subsystem to use, which must
+   * remain valid for the lifetime of this object. If null, a
+   * drake::lcm::DrakeLcm object is allocated and maintained internally, but
+   * see the note in the class comments.
+   *
+   * @param publish_triggers Set of triggers that determine when messages will
+   * be published. Supported TriggerTypes are {kForced, kPeriodic, kPerStep}. 
+   * Will throw an error if empty or if unsupported types are provided.
+   *
+   * @param publish_period Period that messages will be published (optional).
+   * publish_period should only be non-zero if one of the publish_triggers is
+   * kPeriodic.
+   *   
+   * @pre publish_period is non-negative.
+   * @pre trigger_types contains a subset of {kForced, kPeriodic, kPerStep}.
+   * @pre publish_period > 0 if and only if trigger_types contains kPeriodic.
+   */
+  template <typename LcmMessage>
+  static std::unique_ptr<LcmPublisherSystem> Make(
+      const std::string& channel,
+      drake::lcm::DrakeLcmInterface* lcm,
+      const TriggerTypeSet& publish_triggers,
+      double publish_period = 0.0) {
+    return std::make_unique<LcmPublisherSystem>(
+        channel, std::make_unique<Serializer<LcmMessage>>(), lcm,
+        publish_triggers, publish_period);
+  }
+
   /**
    * A constructor for an %LcmPublisherSystem that takes LCM message objects on
    * its sole abstract-valued input port. The LCM message type is determined by
-   * the provided `serializer`.
+   * the provided `serializer`. Will publish on forced events and either 
+   * periodic or per-step events, as determined by publish_period.
    *
    * @param[in] channel The LCM channel on which to publish.
    *
@@ -99,6 +147,39 @@ class LcmPublisherSystem : public LeafSystem<double> {
                      drake::lcm::DrakeLcmInterface* lcm,
                      double publish_period = 0.0);
 
+  /**
+   * A constructor for an %LcmPublisherSystem that takes LCM message objects on
+   * its sole abstract-valued input port. The LCM message type is determined by
+   * the provided `serializer`.
+   *
+   * @param[in] channel The LCM channel on which to publish.
+   *
+   * @param[in] serializer The serializer that converts between byte vectors
+   * and LCM message objects.
+   *
+   * @param lcm A pointer to the LCM subsystem to use, which must
+   * remain valid for the lifetime of this object. If null, a
+   * drake::lcm::DrakeLcm object is allocated and maintained internally, but
+   * see the note in the class comments.
+   *
+   * @param publish_triggers Set of triggers that determine when messages will
+   * be published. Supported TriggerTypes are {kForced, kPeriodic, kPerStep}. 
+   * Will throw an exception if empty or if unsupported types are provided.
+   *
+   * @param publish_period Period that messages will be published (optional).
+   * publish_period should only be non-zero if one of the publish_triggers is
+   * kPerStep.
+   *   
+   * @pre publish_period is non-negative.
+   * @pre publish_period > 0 iff trigger_types contains kPeriodic.
+   * @pre trigger_types contains a subset of {kForced, kPeriodic, kPerStep}.
+   */
+  LcmPublisherSystem(const std::string& channel,
+      std::unique_ptr<SerializerInterface> serializer,
+      drake::lcm::DrakeLcmInterface* lcm,
+      const TriggerTypeSet& publish_triggers,
+      double publish_period = 0.0);
+
   DRAKE_DEPRECATED(
       "The LcmAndVectorBaseTranslator and its related code are deprecated, "
       "and will be removed on 2019-05-01.")
@@ -203,6 +284,18 @@ class LcmPublisherSystem : public LeafSystem<double> {
 
   // All constructors delegate to here. If the lcm pointer is null, we'll
   // allocate and maintain a DrakeLcm object internally.
+  LcmPublisherSystem(const std::string& channel,
+                     const LcmAndVectorBaseTranslator* translator,
+                     std::unique_ptr<const LcmAndVectorBaseTranslator>
+                         owned_translator,
+                     std::unique_ptr<SerializerInterface> serializer,
+                     drake::lcm::DrakeLcmInterface* lcm,
+                     const TriggerTypeSet& publish_triggers,
+                     double publish_period);
+
+  // Constructors which do not specify publish_triggers delegate to here.
+  // This will generate the default publish_triggers before calling
+  // the more general constructor.
   LcmPublisherSystem(const std::string& channel,
                      const LcmAndVectorBaseTranslator* translator,
                      std::unique_ptr<const LcmAndVectorBaseTranslator>

systems/lcm/test/lcm_publisher_system_test.cc

@@ -258,6 +258,73 @@ GTEST_TEST(LcmPublisherSystemTest, TestPerStepPublish) {
   EXPECT_EQ(transmission_count, 1 + simulator.get_num_steps_taken());
 }
 
+// When constructed via a publish_triggers set, tests that per-step publish
+// generates the expected number of publishes.
+GTEST_TEST(LcmPublisherSystemTest, TestPerStepPublishTrigger) {
+  lcm::DrakeMockLcm lcm;
+  const std::string channel_name = "channel_name";
+  int transmission_count = 0;
+  lcm.Subscribe(channel_name, [&transmission_count](const void*, int) {
+    ++transmission_count;
+  });
+  lcm.EnableLoopBack();
+
+  auto dut = LcmPublisherSystem::Make<lcmt_drake_signal>(channel_name,
+      &lcm, {TriggerType::kPerStep});
+
+  unique_ptr<Context<double>> context = dut->AllocateContext();
+
+  context->FixInputPort(kPortNumber,
+    AbstractValue::Make<lcmt_drake_signal>(lcmt_drake_signal()));
+
+  // Prepares to integrate.
+  drake::systems::Simulator<double> simulator(*dut, std::move(context));
+  simulator.set_publish_every_time_step(false);
+  simulator.set_publish_at_initialization(false);
+  simulator.Initialize();
+
+  // Check that a message was transmitted during initialization.
+  EXPECT_EQ(transmission_count, 1);
+
+  // Ensure that the integrator takes at least a few steps.
+  // Since there is no internal continuous state for the system, the integrator
+  // will not subdivide its steps.
+  for (double time = 0.0; time < 1; time += 0.25)
+    simulator.StepTo(time);
+
+  // Check that we get exactly the number of publishes desired: one (at
+  // initialization) plus another for each step.
+  EXPECT_EQ(transmission_count, 1 + simulator.get_num_steps_taken());
+}
+
+// When constructed via a publish_triggers set, tests that forced publish
+// generates the expected number of publishes.
+GTEST_TEST(LcmPublisherSystemTest, TestForcedPublishTrigger) {
+  lcm::DrakeMockLcm lcm;
+  const std::string channel_name = "channel_name";
+  int transmission_count = 0;
+  int force_publish_count = 3;
+  lcm.Subscribe(channel_name, [&transmission_count](const void*, int) {
+    ++transmission_count;
+  });
+  lcm.EnableLoopBack();
+
+  auto dut = LcmPublisherSystem::Make<lcmt_drake_signal>(channel_name,
+      &lcm, {TriggerType::kForced});
+
+  unique_ptr<Context<double>> context = dut->AllocateContext();
+
+  context->FixInputPort(kPortNumber,
+    AbstractValue::Make<lcmt_drake_signal>(lcmt_drake_signal()));
+
+  for (int i = 0; i < force_publish_count; i++) {
+    dut->Publish(*context);
+  }
+
+  // Check that we get exactly the number of publishes desired.
+  EXPECT_EQ(transmission_count, force_publish_count);
+}
+
 // Tests that the published LCM message has the expected timestamps.
 GTEST_TEST(LcmPublisherSystemTest, TestPublishPeriod) {
   const double kPublishPeriod = 1.5;  // Seconds between publications.
@@ -303,6 +370,47 @@ GTEST_TEST(LcmPublisherSystemTest, TestPublishPeriod) {
   EXPECT_EQ(transmission_count, 3);
 }
 
+// When constructed via a publish_triggers set, tests that periodic publishing
+// generates the expected number of publishes.
+GTEST_TEST(LcmPublisherSystemTest, TestPublishPeriodTrigger) {
+  const double kPublishPeriod = 1.5;  // Seconds between publications.
+
+  lcm::DrakeMockLcm lcm;
+  const std::string channel_name = "channel_name";
+  int transmission_count = 0;
+  lcm.Subscribe(channel_name, [&transmission_count](const void*, int) {
+    ++transmission_count;
+  });
+  lcm.EnableLoopBack();
+
+  // Instantiates the "device under test".
+  auto dut = LcmPublisherSystem::Make<lcmt_drake_signal>(channel_name,
+      &lcm, {TriggerType::kPeriodic},
+      kPublishPeriod);
+  unique_ptr<Context<double>> context = dut->AllocateContext();
+
+  context->FixInputPort(kPortNumber,
+    AbstractValue::Make<lcmt_drake_signal>(lcmt_drake_signal()));
+
+  // Prepares to integrate.
+  drake::systems::Simulator<double> simulator(*dut, std::move(context));
+  simulator.set_publish_every_time_step(false);
+  simulator.set_publish_at_initialization(false);
+  simulator.Initialize();
+
+  // Check that a message was transmitted during initialization.
+  EXPECT_EQ(transmission_count, 1);
+
+  for (double time = 0.0; time < 4; time += 0.01) {
+    simulator.StepTo(time);
+    EXPECT_NEAR(simulator.get_mutable_context().get_time(), time, 1e-10);
+  }
+
+  // Check that we get the expected number of messages: one at initialization
+  // plus two from periodic publishing.
+  EXPECT_EQ(transmission_count, 3);
+}
+
 // Tests that the published LCM message has the expected timestamps using the
 // deprecated set_publish_period() method.
 GTEST_TEST(LcmPublisherSystemTest, TestPublishPeriodDeprecated) {