cascade3.h

Go to the documentation of this file.

#pragma once

#include <memory>
#include <vector>

#include "drake/systems/framework/system3.h"
#include "drake/systems/framework/primitives/diagram3.h"

namespace drake {
namespace systems {

template <typename T>
class Cascade3 : public Diagram3<T> {
 public:
  Cascade3(const std::string& name, std::unique_ptr<System3<T>> first,
           std::unique_ptr<System3<T>> second)
      : Diagram3<T>(name) {
    if (!first || !second)
      throw std::logic_error("Cascade: empty Systems not allowed.");

    const int out1 = first->get_num_output_ports();
    const int in2 = second->get_num_input_ports();

    if (in2 != out1) {
      throw std::logic_error(
          "Cascade3: first System has "
          + std::to_string(out1) + " output ports but second System has "
          + std::to_string(in2) + " input ports. They must match.");
    }

    auto first_system = this->AddSubsystem(std::move(first));
    auto second_system = this->AddSubsystem(std::move(second));

    // Input port numbers for Cascade match first system's.
    for (int port = 0; port < first_system->get_num_input_ports(); ++port) {
      const int port_num = this->InheritInputPort(first_system, port);
      DRAKE_ABORT_UNLESS(port_num == port);
    }

    // Connect up the interior ports.
    for (int port = 0; port < out1; ++port)
      this->Connect(first_system, port, second_system, port);

    // Output port numbers for Cascade match second system's.
    for (int port = 0; port < second_system->get_num_output_ports(); ++port) {
      const int port_num = this->InheritOutputPort(second_system, port);
      DRAKE_ABORT_UNLESS(port_num == port);
    }
  }
};

}  // namespace systems
}  // namespace drake