LCM echo timing test

BUILD.bazel

cc_library(
  name = "network_timing_test_utils",
  srcs = ["test/network_timing_test_utils.cc",],
  hdrs = ["test/network_timing_test_utils.h",],
  deps = [
    "@drake//systems/framework",
    "//lcmtypes:lcmt_robot",
    "@drake//lcmtypes:drake_signal",
    "@drake//systems/lcm:lcm_driven_loop",
  ]
)

cc_binary(
    name = "simple_signal_publish",
    srcs = ["test/simple_signal_publish.cc"],
    deps = [
        ":network_timing_test_utils",
        "@drake//systems/analysis:simulator",
        "@drake//lcm",
        "@drake//systems/lcm:lcm_pubsub_system",
        "@gflags",
    ],
)

cc_binary(
    name = "simple_signal_echo",
    srcs = ["test/simple_signal_echo.cc"],
    deps = [
        ":network_timing_test_utils",
        "@drake//systems/analysis:simulator",
        "@drake//lcm",
        "@drake//systems/lcm:lcm_pubsub_system",
        "@gflags",
    ],
)

cc_binary(
    name = "log_delay_test",
    srcs = ["test/log_delay_test.cc"],
    deps = [
        "@drake//lcm",
        "@gflags",
        "@eigen",
        "@drake//lcmtypes:drake_signal",
    ],
)

log_delay_test

#include <iostream>
#include <Eigen/Dense>
#include <gflags/gflags.h>
#include "lcm/lcm-cpp.hpp"

#include "drake/lcmt_drake_signal.hpp"

DEFINE_string(file, "", "Log file name.");
DEFINE_string(channel_ping, "PING", "LCM channel of the ping.");
DEFINE_string(channel_pong, "PONG", "LCM channel of the pong.");
DEFINE_int64(max_count, 5000, "Max number of messages to read.");

// A quick script to evaluate the delay caused by a LCM driven loop.
// Measures the difference between the logged time and the lcm timestamp field
// for both PING and PONG.
// The difference between these two deltas is roughly the delay
// in the loop. This approach to calculating delay does not require
// finding the one-to-one matches between messages on each channel, but instead
// relies on the fact that the message timestamp field should be identical.
int main(int argc, char* argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  lcm::LogFile log(FLAGS_file, "r");

  auto event = log.readNextEvent();

  int count = 0;

  int count_ping = 0;
  Eigen::VectorXd diff_ping(FLAGS_max_count);

  int count_pong = 0;
  Eigen::VectorXd diff_pong(FLAGS_max_count);

  while (event != NULL &&
         count < FLAGS_max_count) {
     count++;

    if (event->channel == FLAGS_channel_ping) {
      drake::lcmt_drake_signal msg;
      msg.decode(event->data, 0, event->datalen);
      diff_ping(count_ping) = event->timestamp - msg.timestamp;
      count_ping++;
    }

    if (event->channel == FLAGS_channel_pong) {
      drake::lcmt_drake_signal msg;
      msg.decode(event->data, 0, event->datalen);
      diff_pong(count_pong) = event->timestamp - msg.timestamp;
      count_pong++;
    }

    event = log.readNextEvent();
  }

  diff_ping.conservativeResize(count_ping);
  diff_pong.conservativeResize(count_pong);

  std::cout << "PING (timestamp - utime) mean: " <<
      diff_ping.mean() << std::endl;
  std::cout << "PONG (timestamp - utime) mean: " <<
      diff_pong.mean() << std::endl;
  std::cout << "Difference of means (delay estimate, us): " <<
      diff_pong.mean() - diff_ping.mean() << std::endl;
}

network_timing_test_utils.cc

#include "examples/Cassie/networking/test/network_timing_test_utils.h"

namespace dairlib {
namespace systems {

using drake::systems::Context;
using drake::systems::LeafSystem;

using std::chrono::duration_cast;
using std::chrono::steady_clock;
using std::chrono::system_clock;
using std::chrono::microseconds;


SignalSender::SignalSender() {
  this->DeclareAbstractOutputPort("lcmt_drake_signal",
      &SignalSender::Output);

  start_ = steady_clock::now();
}

void SignalSender::Output(const Context<double>& context,
    drake::lcmt_drake_signal* output) const {
  output->timestamp =
      (duration_cast<microseconds>(steady_clock::now() - start_)).count();
}

}  // namespace systems
}  // namespace dairlib

network_timing_test_utils.h

#pragma once

#include "drake/systems/framework/leaf_system.h"
#include "drake/systems/lcm/lcm_driven_loop.h"
#include "drake/lcmt_drake_signal.hpp"

namespace dairlib {
namespace systems {

class SignalSender : public drake::systems::LeafSystem<double> {
 public:
  SignalSender();

 private:
  void Output(const drake::systems::Context<double>& context,
                    drake::lcmt_drake_signal* output) const;

  std::chrono::time_point<std::chrono::steady_clock> start_;
};

/**
 * A translator class for Lcm message types that have a "utime" field, which
 * is in micro seconds.
 */
template <typename MessageType>
class TimestampMessageToSeconds :
    public drake::systems::lcm::LcmMessageToTimeInterface {
 public:
  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TimestampMessageToSeconds)
  TimestampMessageToSeconds() {}
  ~TimestampMessageToSeconds() {}

  double GetTimeInSeconds(const drake::AbstractValue& abstract_value)
      const override {
    const MessageType& msg = abstract_value.GetValue<MessageType>();
    return static_cast<double>(msg.timestamp) / 1e6;
  }
};

}  // namespace systems
}  // namespace dairlib

simple_signal_echo.cc

#include <memory>
#include <string>

#include <gflags/gflags.h>
#include "drake/lcm/drake_lcm.h"
#include "drake/systems/lcm/lcm_publisher_system.h"
#include "drake/systems/lcm/lcm_subscriber_system.h"
#include "drake/systems/lcm/lcm_driven_loop.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/lcmt_drake_signal.hpp"

#include "examples/Cassie/networking/test/network_timing_test_utils.h"

namespace dairlib {

using drake::systems::lcm::LcmPublisherSystem;
using drake::systems::lcm::LcmSubscriberSystem;
using drake::systems::TriggerType;
using drake::lcmt_drake_signal;
using systems::TimestampMessageToSeconds;


DEFINE_double(publish_rate, 1000, "Publishing frequency (Hz)");

int do_main(int argc, char* argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);

  drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0");
  drake::systems::DiagramBuilder<double> builder;

  auto sub = builder.AddSystem(
      LcmSubscriberSystem::Make<lcmt_drake_signal>("PING", &lcm));
  auto pub = builder.AddSystem(LcmPublisherSystem::Make<lcmt_drake_signal>(
      "PONG", &lcm, {TriggerType::kForced}));
  builder.Connect(*sub, *pub);

  auto diagram = builder.Build();
  drake::systems::lcm::LcmDrivenLoop loop(*diagram, *sub, nullptr, &lcm,
      std::make_unique<TimestampMessageToSeconds<lcmt_drake_signal>>());

  loop.set_publish_on_every_received_message(true);

  // Starts the loop.
  loop.RunToSecondsAssumingInitialized(1e60);
  return 0;
}

}  // namespace dairlib

int main(int argc, char* argv[]) { return dairlib::do_main(argc, argv); }

simple_signal_publish.cc

#include <memory>
#include <string>

#include <gflags/gflags.h>
#include "drake/lcm/drake_lcm.h"
#include "drake/systems/lcm/lcm_publisher_system.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/lcmt_drake_signal.hpp"

#include "examples/Cassie/networking/test/network_timing_test_utils.h"

namespace dairlib {

using drake::systems::lcm::LcmPublisherSystem;
using drake::lcmt_drake_signal;

DEFINE_double(publish_rate, 1000, "Publishing frequency (Hz)");

int do_main(int argc, char* argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);

  drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0");
  drake::systems::DiagramBuilder<double> builder;

  auto sender = builder.AddSystem<systems::SignalSender>();
  auto pub = builder.AddSystem(LcmPublisherSystem::Make<lcmt_drake_signal>(
      "PING", &lcm, 1.0 / FLAGS_publish_rate));
  builder.Connect(*sender, *pub);

  auto diagram = builder.Build();
  drake::systems::Simulator<double> simulator(*diagram);

  simulator.set_publish_every_time_step(false);
  simulator.set_publish_at_initialization(false);
  simulator.set_target_realtime_rate(1.0);
  simulator.Initialize();

  lcm.StartReceiveThread();

  simulator.StepTo(std::numeric_limits<double>::infinity());
  return 0;
}

}  // namespace dairlib

int main(int argc, char* argv[]) { return dairlib::do_main(argc, argv); }