system_output.h

Go to the documentation of this file.

#pragma once

#include <memory>
#include <set>
#include <stdexcept>
#include <string>
#include <vector>

#include "drake/drakeSystemFramework_export.h"
#include "drake/systems/framework/value.h"
#include "drake/systems/framework/vector_interface.h"

namespace drake {
namespace systems {

// TODO(david-german-tri): Consider moving to its own file.
class DRAKESYSTEMFRAMEWORK_EXPORT OutputPortListenerInterface {
 public:
  virtual ~OutputPortListenerInterface() {}

  virtual void Invalidate() = 0;
};

class OutputPort {
 public:
  virtual ~OutputPort() = default;

  void add_dependent(OutputPortListenerInterface* dependent) {
    dependents_.insert(dependent);
  }

  void remove_dependent(OutputPortListenerInterface* dependent) {
    dependents_.erase(dependent);
  }

  long long get_version() const { return version_; }

  void NoteValueModification() {
    ++version_;
    for (OutputPortListenerInterface* dependent : dependents_)
      dependent->Invalidate();
  }

 protected:
  OutputPort() {}

 private:
  // OutputPort objects are neither copyable nor moveable.
  OutputPort(const OutputPort& other) = delete;
  OutputPort& operator=(const OutputPort& other) = delete;
  OutputPort(OutputPort&& other) = delete;
  OutputPort& operator=(OutputPort&& other) = delete;

  // The rate at which this port produces output, in seconds.
  // If zero, the port is continuous.
  double sample_time_sec_{0.};

  // The list of consumers that should be notified when the value on this
  // output port changes.
  std::set<OutputPortListenerInterface*> dependents_;

  // A counter that is bumped every time this port's value changes.
  long long version_{0};
};

template <typename T>
class VectorOutputPort : public OutputPort {
 public:
  explicit VectorOutputPort(std::unique_ptr<VectorInterface<T>> data)
      : OutputPort(), vector_data_(std::move(data)) {}

  const VectorInterface<T>* get_vector_data() const {
    return vector_data_.get();
  }

  VectorInterface<T>* GetMutableVectorData() {
    NoteValueModification();
    return vector_data_.get();
  }

 private:
  // VectorOutputPort objects are neither copyable nor moveable.
  VectorOutputPort(const VectorOutputPort& other) = delete;
  VectorOutputPort& operator=(const VectorOutputPort& other) = delete;
  VectorOutputPort(VectorOutputPort&& other) = delete;
  VectorOutputPort& operator=(VectorOutputPort&& other) = delete;

  // The port data for this vector-valued port.
  std::unique_ptr<VectorInterface<T>> vector_data_;
};

template <typename T>
class SystemOutput {
 public:
  explicit SystemOutput(int num_ports) : ports_(num_ports) {}

  void set_port(int port_num, std::unique_ptr<OutputPort> port) {
    RangeCheck(port_num, "reset_port");
    ports_[port_num] = std::move(port);
  }

  int get_num_ports() const { return (int)ports_.size(); }

  bool is_empty_port(int port_num) const {
    RangeCheck(port_num, "empty_port");
    return !ports_[port_num];
  }

  bool is_vector_port(int port_num) const {
    RangeCheck(port_num, "is_vector_port");
    const OutputPort* port = ports_[port_num].get();
    if (port == nullptr) return false;
    return dynamic_cast<const VectorOutputPort<T>*>(port) != nullptr;
  }

  const OutputPort& get_port(int port_num) const {
    RangeCheck(port_num, "get_port");
    return *ports_[port_num];
  }

  OutputPort* get_mutable_port(int port_num) {
    RangeCheck(port_num, "get_port");
    return ports_[port_num].get();
  }

  const VectorOutputPort<T>& get_vector_port(int port_num) const {
    const OutputPort& port = get_port(port_num);
    auto vector_port = dynamic_cast<const VectorOutputPort<T>*>(&port);
    if (vector_port == nullptr) {
      throw std::logic_error("SystemOutput::get_vector_port(): Port " +
                             std::to_string(port_num) +
                             " is not a VectorOutputPort.");
    }
    return *vector_port;
  }

  VectorOutputPort<T>* get_mutable_vector_port(int port_num) {
    const VectorOutputPort<T>& port = get_vector_port(port_num);
    return const_cast<VectorOutputPort<T>*>(&port);
  }

 private:
  void RangeCheck(int port_num, const char* name) const {
    if (0 <= port_num && port_num < get_num_ports()) return;
    throw std::out_of_range(GetMethod(name) + "output port " +
                            std::to_string(port_num) +
                            " is out of range (there are " +
                            std::to_string(get_num_ports()) + " ports).");
  }
  void RangeNullCheck(int port_num, const char* name) const {
    RangeCheck(port_num, name);
    if (!ports_[port_num])
      throw std::logic_error(GetMethod(name) + "output port " +
                            std::to_string(port_num) + " is empty.");
  }
  static std::string GetMethod(const char* name) {
    using namespace std::string_literals;
    return "SystemOutput::"s + std::string(name) + "(): "s;
  }

  std::vector<std::unique_ptr<OutputPort>> ports_;
};

}  // namespace systems
}  // namespace drake