From 952cd1219c959b083cafdbc2dae5f88f7bffa730 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Tue, 15 Jan 2019 09:22:39 -0800 Subject: [PATCH] Modernize event handling in SignalLogger and improve documentation. (#10269) Remove now-unneeded fluff from examples. --- .../fibonacci_difference_equation_test.cc | 2 - systems/analysis/test/simulator_test.cc | 6 -- systems/discrete_systems.h | 2 - systems/lcm/test/lcm_driven_loop_test.cc | 9 +- systems/primitives/BUILD.bazel | 1 + systems/primitives/signal_log.h | 10 +- systems/primitives/signal_logger.cc | 44 +++++++-- systems/primitives/signal_logger.h | 97 +++++++++++++++---- systems/primitives/test/signal_logger_test.cc | 60 +++++++++--- 9 files changed, 178 insertions(+), 53 deletions(-) diff --git a/examples/fibonacci/test/fibonacci_difference_equation_test.cc b/examples/fibonacci/test/fibonacci_difference_equation_test.cc index 821271f909da..d8ca3e8481c8 100644 --- a/examples/fibonacci/test/fibonacci_difference_equation_test.cc +++ b/examples/fibonacci/test/fibonacci_difference_equation_test.cc @@ -22,8 +22,6 @@ GTEST_TEST(Fibonacci, CheckSequence) { auto diagram = builder.Build(); systems::Simulator simulator(*diagram); - simulator.set_publish_every_time_step(false); - simulator.set_publish_at_initialization(false); // Simulate forward to fibonacci(6): 0 1 1 2 3 5 8 simulator.StepTo(6 * FibonacciDifferenceEquation::kPeriod); diff --git a/systems/analysis/test/simulator_test.cc b/systems/analysis/test/simulator_test.cc index 6b3f4a1da962..b433623b39c6 100644 --- a/systems/analysis/test/simulator_test.cc +++ b/systems/analysis/test/simulator_test.cc @@ -1012,8 +1012,6 @@ GTEST_TEST(SimulatorTest, ExampleDiscreteSystem) { // Create a Simulator and use it to advance time until t=3*h. Simulator simulator(*diagram); - simulator.set_publish_every_time_step(false); - simulator.set_publish_at_initialization(false); simulator.StepTo(3 * ExampleDiscreteSystem::kPeriod); testing::internal::CaptureStdout(); // Not in example. @@ -1091,10 +1089,6 @@ GTEST_TEST(SimulatorTest, SinusoidalHybridSystem) { const double initial_value = std::sin(-f * h); // = sin(f*-1*h) Simulator simulator(*diagram); - // TODO(sherm1) Remove these when #10269 lands (fix to SignalLogger). - simulator.set_publish_at_initialization(false); - simulator.set_publish_every_time_step(false); - simulator.get_mutable_context().get_mutable_discrete_state()[0] = initial_value; simulator.StepTo(t_final); diff --git a/systems/discrete_systems.h b/systems/discrete_systems.h index b4b6ce3132da..ef3439be02ae 100644 --- a/systems/discrete_systems.h +++ b/systems/discrete_systems.h @@ -91,8 +91,6 @@ The following code fragment can be used to step this system forward: // Create a Simulator and use it to advance time until t=3*h. Simulator simulator(*diagram); - simulator.set_publish_every_time_step(false); - simulator.set_publish_at_initialization(false); simulator.StepTo(3 * ExampleDiscreteSystem::kPeriod); // Print out the contents of the log. diff --git a/systems/lcm/test/lcm_driven_loop_test.cc b/systems/lcm/test/lcm_driven_loop_test.cc index 1ab7bfd104f4..f06c1487837d 100644 --- a/systems/lcm/test/lcm_driven_loop_test.cc +++ b/systems/lcm/test/lcm_driven_loop_test.cc @@ -90,8 +90,11 @@ GTEST_TEST(LcmDrivenLoopTest, TestLoop) { sub->set_name("subscriber"); auto dummy = builder.AddSystem(); dummy->set_name("dummy"); + auto logger = builder.AddSystem>(1); logger->set_name("logger"); + logger->set_forced_publish_only(); // Log only when told to do so. + builder.Connect(*sub, *dummy); builder.Connect(*dummy, *logger); auto sys = builder.Build(); @@ -99,9 +102,9 @@ GTEST_TEST(LcmDrivenLoopTest, TestLoop) { // Makes the lcm driven loop. lcm::LcmDrivenLoop dut(*sys, *sub, nullptr, &lcm, std::make_unique()); - // This ensures that dut calls sys->Publish() every time it handles a - // message, which triggers the logger to save its input (message time stamp) - // to the log. + // This ensures that dut calls sys->Publish() (a.k.a. "forced publish") + // every time it handles a message, which triggers the logger to save its + // input (message time stamp) to the log. dut.set_publish_on_every_received_message(true); // Starts the publishing thread. diff --git a/systems/primitives/BUILD.bazel b/systems/primitives/BUILD.bazel index ee425f048a18..d43b14bb0962 100644 --- a/systems/primitives/BUILD.bazel +++ b/systems/primitives/BUILD.bazel @@ -542,6 +542,7 @@ drake_cc_googletest( ":linear_system", ":signal_logger", "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", "//systems/analysis:simulator", "//systems/framework", ], diff --git a/systems/primitives/signal_log.h b/systems/primitives/signal_log.h index 05ebbabd233b..cb99f14a4baf 100644 --- a/systems/primitives/signal_log.h +++ b/systems/primitives/signal_log.h @@ -7,7 +7,10 @@ namespace drake { namespace systems { /** - This class serves as an in-memory cache of time-dependent vector values. + This utility class serves as an in-memory cache of time-dependent vector + values. Note that this is a standalone class, not a Drake System. It is + primarily intended to support the Drake System primitive SignalLogger, but can + be used independently. @tparam T The vector element type, which must be a valid Eigen scalar. */ @@ -23,6 +26,9 @@ class SignalLog { */ explicit SignalLog(int input_size, int batch_allocation_size = 1000); + /** Returns the number of samples taken since construction or last reset(). */ + int num_samples() const { return num_samples_; } + /** Accesses the logged time stamps. */ Eigen::VectorBlock> sample_times() const { return const_cast&>(sample_times_).head(num_samples_); @@ -34,7 +40,7 @@ class SignalLog { return const_cast&>(data_).leftCols(num_samples_); } - /** Reset the logged data. */ + /** Clears the logged data. */ void reset() { // Resetting num_samples_ is sufficient to have all future writes and // reads re-initialized to the beginning of the data. diff --git a/systems/primitives/signal_logger.cc b/systems/primitives/signal_logger.cc index d661bb0f63c8..0a539f56d908 100644 --- a/systems/primitives/signal_logger.cc +++ b/systems/primitives/signal_logger.cc @@ -9,24 +9,56 @@ template SignalLogger::SignalLogger(int input_size, int batch_allocation_size) : log_(input_size, batch_allocation_size) { this->DeclareInputPort("data", kVectorValued, input_size); + + // Use a per-step event by default; disabled by set_publish_period() or + // set_forced_publish_only(). + this->DeclarePerStepPublishEvent(&SignalLogger::PerStepWriteToLog); + logging_mode_ = kPerStep; } template void SignalLogger::set_publish_period(double period) { - this->DeclarePeriodicPublish(period); + switch (logging_mode_) { + case kPeriodic: + throw std::logic_error( + "SignalLogger::set_publish_period(): can only be called once."); + case kForced: + throw std::logic_error( + "SignalLogger::set_publish_period(): cannot be called if " + "set_forced_publish_only() has been called."); + case kPerStep: + this->DeclarePeriodicPublishEvent(period, 0., // period, offset + &SignalLogger::WriteToLog); + logging_mode_ = kPeriodic; + return; + } +} + +template +void SignalLogger::set_forced_publish_only() { + switch (logging_mode_) { + case kForced: + return; // Already forced-publishing. + case kPeriodic: + throw std::logic_error( + "SignalLogger::set_forced_publish_only(): " + "cannot be called if set_publish_period() has been called."); + case kPerStep: + this->DeclareForcedPublishEvent(&SignalLogger::WriteToLog); + logging_mode_ = kForced; + return; + } } template -void SignalLogger::DoPublish(const Context& context, - const std::vector*>&) - const { +EventStatus SignalLogger::WriteToLog(const Context& context) const { log_.AddData(context.get_time(), this->EvalVectorInput(context, 0)->get_value()); + return EventStatus::Succeeded(); } template -const InputPort& SignalLogger::get_input_port() -const { +const InputPort& SignalLogger::get_input_port() const { return System::get_input_port(0); } diff --git a/systems/primitives/signal_logger.h b/systems/primitives/signal_logger.h index b52c1a96e095..4942c7d29685 100644 --- a/systems/primitives/signal_logger.h +++ b/systems/primitives/signal_logger.h @@ -15,11 +15,36 @@ namespace drake { namespace systems { -/// A sink block which logs its input to memory. This data is then retrievable -/// (e.g. after a simulation) via a handful of accessor methods. -/// This class essentially holds a large Eigen matrix for data storage, where -/// each column corresponds to a data point. This system saves a data point and -/// the context time whenever its Publish() method is called. +// TODO(sherm1) This System has problems as discussed in issue #10228 that +// should be fixed. +/// A discrete sink block which logs its input to memory (not thread safe). This +/// data is then retrievable (e.g. after a simulation) via a handful of accessor +/// methods. This system holds a large, mutable Eigen matrix for data storage, +/// where each column corresponds to a data point. It saves a data point and +/// the context time whenever it samples its input. +/// +/// By default, sampling is performed every time the Simulator completes a +/// trajectory-advancing substep (that is, via a per-step Publish event), with +/// the first sample occuring during Simulator::Initialize(). That means the +/// samples will generally be unevenly spaced in time. If you prefer regular +/// sampling, you may optionally specify a "publish period" in which case +/// sampling occurs periodically, with the first sample occurring at time 0. +/// Alternatively (not common), you can specify that logging should only occur +/// at "forced publish" events, meaning at explicit calls to System::Publish(). +/// The Simulator's "publish every time step" option also results in forced +/// publish events, so should be disabled if you want to control logging +/// yourself. +/// +/// @warning %SignalLogger is _not_ thread-safe because it writes to a mutable +/// buffer internally. If you have a Diagram that contains a SignalLogger, +/// even with each thread having its own Context, the threads will conflict. +/// You would have to have separate Diagrams in each thread to avoid trouble. +/// +/// @see LogOutput() for a convenient way to add %logging to a Diagram. +/// @see Simulator::set_publish_every_time_step() +/// @see Simulator::set_publish_at_initialization() +/// +/// @system{ SignalLogger, @input_port{data}, } /// /// @tparam T The vector element type, which must be a valid Eigen scalar. /// @@ -35,52 +60,84 @@ class SignalLogger : public LeafSystem { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SignalLogger) - /// Construct the signal logger system. + /// Constructs the signal logger system. + /// + /// @warning %SignalLogger may become slower and slower as the number of + /// log entries increases due to memory reallocations. You can avoid that by + /// providing a `batch_allocation_size` comparable to the total expected + /// number of log entries for your simulation. + /// /// @param input_size Dimension of the (single) input port. This corresponds /// to the number of rows of the data matrix. /// @param batch_allocation_size Storage is (re)allocated in blocks of /// input_size-by-batch_allocation_size. + /// @see LogOutput() helper function for a convenient way to add %logging. explicit SignalLogger(int input_size, int batch_allocation_size = 1000); - /// Sets the publishing period of this system. See - /// LeafSystem::DeclarePeriodicPublish() for details about the semantics of - /// parameter `period`. + /// Sets the publishing period of this system to specify periodic sampling + /// and disables the default per-step sampling. This method can only be called + /// once and only if set_forced_publish_only() hasn't been called. + /// @throws std::logic_error if called more than once, or if + /// set_forced_publish_only() has been called. + /// @pre `period` must be greater than zero. void set_publish_period(double period); + /// Limits logging to forced publish calls only, that is, explicit calls + /// to System::Publish() issued directly or by the Simulator and disables the + /// default per-step sampling. This method cannot be called if + /// set_publish_period() has been called. + /// @throws std::logic_error if set_publish_period() has been called. + void set_forced_publish_only(); + + /// Returns the number of samples taken since construction or last reset(). + int num_samples() const { return log_.num_samples(); } - /// Access the (simulation) time of the logged data. + /// Provides access to the sample times of the logged data. Time is taken + /// from the Context when the log entry is added. Eigen::VectorBlock> sample_times() const { return log_.sample_times(); } - /// Access the logged data. - Eigen::Block, Eigen::Dynamic, Eigen::Dynamic, true> data() - const { + /// Provides access to the logged data. + Eigen::Block, Eigen::Dynamic, Eigen::Dynamic, true> + data() const { return log_.data(); } + /// Clears the logged data. void reset() { log_.reset(); } - /// Returns the only input port. + /// Returns the only input port. The port's name is "data" so you can also + /// access this with GetInputPort("data"). const InputPort& get_input_port() const; private: - // Logging is done in this method. - void DoPublish(const Context& context, - const std::vector*>& events) - const override; + enum LoggingMode { kPerStep, kPeriodic, kForced }; + + // Logging is done in this event handler. + EventStatus WriteToLog(const Context& context) const; + + // This event handler logs only if we're in per-step logging mode. + EventStatus PerStepWriteToLog(const Context& context) const { + if (logging_mode_ == kPerStep) + return WriteToLog(context); + return EventStatus::DidNothing(); + } + + LoggingMode logging_mode_{kPerStep}; - mutable SignalLog log_; + mutable SignalLog log_; // TODO(sherm1) Not thread safe :( }; /// Provides a convenience function for adding a SignalLogger, initialized to -/// the correct size, and connected to another output in a DiagramBuilder. +/// the correct size, and connected to an output in a DiagramBuilder. /// /// @code /// DiagramBuilder builder; /// auto foo = builder.AddSystem("name", 3.14); /// auto logger = LogOutput(foo->get_output_port(), &builder); /// @endcode +/// @relates drake::systems::SignalLogger template SignalLogger* LogOutput(const OutputPort& src, diff --git a/systems/primitives/test/signal_logger_test.cc b/systems/primitives/test/signal_logger_test.cc index e715519820c8..86edd0cb7268 100644 --- a/systems/primitives/test/signal_logger_test.cc +++ b/systems/primitives/test/signal_logger_test.cc @@ -1,11 +1,13 @@ #include "drake/systems/primitives/signal_logger.h" +#include #include #include #include "drake/common/eigen_types.h" #include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" #include "drake/systems/analysis/simulator.h" #include "drake/systems/framework/diagram_builder.h" #include "drake/systems/primitives/constant_vector_source.h" @@ -17,6 +19,7 @@ namespace { // Log the output of a simple linear system (with a known solution). GTEST_TEST(TestSignalLogger, LinearSystemTest) { + using std::exp; DiagramBuilder builder; // xdot = -x. y = x. (No inputs). @@ -45,17 +48,20 @@ GTEST_TEST(TestSignalLogger, LinearSystemTest) { simulator.get_mutable_integrator()->set_target_accuracy(1e-4); simulator.Initialize(); - // The Simulator internally calls Publish(), which triggers data logging. + // The Simulator schedules SignalLogger's default per-step event, which + // performs data logging. simulator.StepTo(3); // Gets the time stamps when each data point is saved. const auto& t = logger->sample_times(); - EXPECT_EQ(t.size(), simulator.get_num_publishes()); // Gets the logged data. const auto& x = logger->data(); EXPECT_EQ(x.cols(), t.size()); + // Check that num_samples() makes sense. + EXPECT_EQ(logger->num_samples(), t.size()); + // Now check the data (against the known solution to the diff eq). const Eigen::MatrixXd expected_x = exp(-t.transpose().array()); @@ -68,19 +74,16 @@ GTEST_TEST(TestSignalLogger, LinearSystemTest) { // Test that reset makes everything empty. logger->reset(); + EXPECT_EQ(logger->num_samples(), 0); EXPECT_EQ(logger->sample_times().size(), 0); EXPECT_EQ(logger->data().cols(), 0); - const int num_prev_publishes = simulator.get_num_publishes(); simulator.StepTo(4.); - - EXPECT_EQ(logger->sample_times().size(), - simulator.get_num_publishes() - num_prev_publishes); - EXPECT_EQ(logger->data().cols(), logger->sample_times().size()); + EXPECT_EQ(logger->data().cols(), logger->num_samples()); } -// Test that set_publish_period triggers properly even for logging a constant -// signal. +// Test that set_publish_period() causes correct triggering even for logging +// a constant signal. GTEST_TEST(TestSignalLogger, SetPublishPeriod) { // Add System and Connect DiagramBuilder builder; @@ -91,13 +94,46 @@ GTEST_TEST(TestSignalLogger, SetPublishPeriod) { // Construct Simulator Simulator simulator(*diagram); - simulator.set_publish_every_time_step(false); - simulator.set_publish_at_initialization(false); // Run simulation simulator.StepTo(1); - EXPECT_EQ(simulator.get_num_publishes(), 11); + EXPECT_EQ(logger->num_samples(), 11); + + // Check that we can only call set_publish_period() once. + DRAKE_EXPECT_THROWS_MESSAGE(logger->set_publish_period(0.2), std::logic_error, + ".*can only be called once.*"); + + // And that forced-publish can't be specified if there is a period set. + DRAKE_EXPECT_THROWS_MESSAGE(logger->set_forced_publish_only(), + std::logic_error, + ".*cannot be called if set_publish_period.*"); +} + +// Test that set_forced_publish_only() triggers on an explicit Publish(). +GTEST_TEST(TestSignalLogger, SetForcedPublishOnly) { + // Add System and Connect + DiagramBuilder builder; + auto system = builder.AddSystem>(2.0); + auto logger = LogOutput(system->get_output_port(), &builder); + logger->set_forced_publish_only(); + auto diagram = builder.Build(); + + auto context = diagram->CreateDefaultContext(); + EXPECT_EQ(logger->num_samples(), 0); + + diagram->Publish(*context); + EXPECT_EQ(logger->num_samples(), 1); + + // Harmless to call this again. + EXPECT_NO_THROW(logger->set_forced_publish_only()); + diagram->Publish(*context); + EXPECT_EQ(logger->num_samples(), 2); + + // Check that set_publish_period() can't be called after forced-publish. + DRAKE_EXPECT_THROWS_MESSAGE( + logger->set_publish_period(0.1), std::logic_error, + ".*cannot be called if set_forced_publish_only.*"); } } // namespace