Factor away use of ignition in the unicycle car agent

python/delphyne/agents.cc

@@ -39,6 +39,7 @@ using delphyne::RailCar;
 using delphyne::RailCarBlueprint;
 using delphyne::SimpleCarBlueprint;
 using delphyne::TrajectoryAgentBlueprint;
+using delphyne::UnicycleCarAgent;
 using delphyne::UnicycleCarBlueprint;
 
 namespace {
@@ -59,6 +60,10 @@ PYBIND11_MODULE(agents, m) {
 
   py::class_<RailCar, Agent>(m, "RailCar").def("set_speed", &RailCar::SetSpeed);
 
+  py::class_<UnicycleCarAgent, Agent>(m, "UnicycleCarAgent")
+      .def("set_acceleration", &UnicycleCarAgent::SetAcceleration)
+      .def("set_angular_rate", &UnicycleCarAgent::SetAngularRate);
+
   py::class_<AgentBlueprint>(m, "AgentBlueprint")
       .def("get_agent",
            [](AgentBlueprint* self, const AgentSimulation& simulation) { return &self->GetAgent(simulation); },

src/agents/unicycle_car.cc

@@ -1,5 +1,5 @@
 /**
- * @file src/agents/simple_car
+ * @file src/agents/unicycle_car
  *
  * Copyright 2020 Toyota Research Institute
  */
@@ -12,10 +12,11 @@
 #include <string>
 #include <utility>
 
-#include "backend/ign_subscriber_system.h"
+#include <drake/systems/primitives/multiplexer.h>
+
+#include "gen/angular_rate_acceleration_command.h"
 #include "gen/simple_car_state.h"
 #include "systems/unicycle_car.h"
-#include "translations/ign_angular_rate_acceleration_command_to_drake.h"
 
 #include <maliput/common/maliput_unused.h>
 
@@ -30,10 +31,10 @@ namespace delphyne {
  *****************************************************************************/
 
 UnicycleCarBlueprint::UnicycleCarBlueprint(const std::string& name, double x, double y, double heading, double speed)
-    : BasicAgentBlueprint(name), initial_conditions_(x, y, heading, speed) {}
+    : TypedAgentBlueprint<UnicycleCarAgent>(name), initial_conditions_(x, y, heading, speed) {}
 
-std::unique_ptr<Agent::Diagram> UnicycleCarBlueprint::DoBuildDiagram(
-    const maliput::api::RoadGeometry* road_geometry) const {
+std::unique_ptr<UnicycleCarAgent> UnicycleCarBlueprint::DoBuildAgentInto(
+    const maliput::api::RoadGeometry* road_geometry, drake::systems::DiagramBuilder<double>* simulator_builder) const {
   maliput::common::unused(road_geometry);
 
   AgentBlueprint::DiagramBuilder builder(this->name());
@@ -52,40 +53,30 @@ std::unique_ptr<Agent::Diagram> UnicycleCarBlueprint::DoBuildDiagram(
    * Unicycle Car System
    *********************/
   typedef UnicycleCar<double> UnicycleCarSystem;
-  UnicycleCarSystem* simple_car_system =
+  UnicycleCarSystem* unicycle_car_system =
       builder.AddSystem(std::make_unique<UnicycleCarSystem>(context_continuous_state));
-  simple_car_system->set_name(this->name() + "_system");
-
-  /*********************
-   * Teleop Systems
-   *********************/
-  std::string command_channel = "teleop/" + this->name();
-  typedef IgnSubscriberSystem<ignition::msgs::AutomotiveAngularRateAccelerationCommand>
-      AngularRateAccelerationCommandSubscriber;
-  AngularRateAccelerationCommandSubscriber* input_command_subscriber =
-      builder.AddSystem(std::make_unique<AngularRateAccelerationCommandSubscriber>(command_channel));
+  unicycle_car_system->set_name(this->name() + "_system");
 
-  auto input_command_translator = builder.AddSystem(std::make_unique<IgnAngularRateAccelerationCommandToDrake>());
+  auto acceleration_setter = builder.template AddSystem(std::make_unique<delphyne::VectorSource<double>>(-1));
+  auto angular_rate_setter = builder.template AddSystem(std::make_unique<delphyne::VectorSource<double>>(-1));
 
-  // Ignition anglular rate and acceleration commands received through the subscriber are translated
-  // to Drake.
-  builder.Connect(*input_command_subscriber, *input_command_translator);
+  drake::systems::Multiplexer<double>* mux = builder.AddSystem<drake::systems::Multiplexer<double>>(
+      std::make_unique<drake::systems::Multiplexer<double>>(AngularRateAccelerationCommand<double>()));
+  mux->set_name(this->name() + "_mux");
 
-  // And then the translated Drake command is sent to the car.
-  builder.Connect(*input_command_translator, *simple_car_system);
+  builder.Connect(angular_rate_setter->output(), mux->get_input_port(0));
+  builder.Connect(acceleration_setter->output(), mux->get_input_port(1));
+  builder.Connect(mux->get_output_port(0), unicycle_car_system->get_input_port(0));
 
   /*********************
    * Diagram Outputs
    *********************/
-  builder.ExportStateOutput(simple_car_system->state_output());
-  builder.ExportPoseOutput(simple_car_system->pose_output());
-  builder.ExportVelocityOutput(simple_car_system->velocity_output());
+  builder.ExportStateOutput(unicycle_car_system->state_output());
+  builder.ExportPoseOutput(unicycle_car_system->pose_output());
+  builder.ExportVelocityOutput(unicycle_car_system->velocity_output());
 
-  return builder.Build();
+  Agent::Diagram* agent_diagram = simulator_builder->AddSystem(builder.Build());
+  return std::make_unique<UnicycleCarAgent>(agent_diagram, acceleration_setter, angular_rate_setter);
 }
 
-/*****************************************************************************
- ** Trailers
- *****************************************************************************/
-
 }  // namespace delphyne

src/agents/unicycle_car.h

@@ -15,6 +15,8 @@
 #include <maliput/api/road_geometry.h>
 #include "delphyne/mi6/agent_base_blueprint.h"
 
+#include "systems/vector_source.h"
+
 /*****************************************************************************
 ** Namespaces
 *****************************************************************************/
@@ -25,8 +27,43 @@ namespace delphyne {
 ** Interfaces
 *****************************************************************************/
 
-/// @brief A very simple vehicle agent that can be teleoperated.
-class UnicycleCarBlueprint : public BasicAgentBlueprint {
+/// @brief An agent that follows roads as if they were railroad tracks.
+///
+/// The underlying road network has a reference line for each lane which
+/// is utilised by this agent as a railroad track.
+class UnicycleCarAgent : public Agent {
+ public:
+  DELPHYNE_NO_COPY_NO_MOVE_NO_ASSIGN(UnicycleCarAgent)
+
+  /// Unicycle car agent constructor.
+  ///
+  /// @param diagram The Diagram representation of the rail car agent.
+  /// @param acceleration_setter The acceleration input to the underlying UnicycleCar
+  /// in this diagram.
+  /// @param angular_rate_setter The angular rate input to the underlying UnicycleCar
+  /// in this diagram.
+
+  explicit UnicycleCarAgent(Agent::Diagram* diagram, VectorSource<double>* acceleration_setter,
+                            VectorSource<double>* angular_rate_setter)
+      : Agent(diagram), acceleration_setter_(acceleration_setter), angular_rate_setter_(angular_rate_setter) {}
+
+  /// Sets the acceleration input to this agent.
+  ///
+  /// @param new_acceleration The new acceleration input to the agent.
+  void SetAcceleration(double new_acceleration) { acceleration_setter_->Set(new_acceleration); }
+
+  /// Sets the angular rate input to this agent.
+  ///
+  /// @param new_angular_rate The new angular rate input to the agent.
+  void SetAngularRate(double new_angular_rate) { angular_rate_setter_->Set(new_angular_rate); }
+
+ private:
+  VectorSource<double>* acceleration_setter_;
+  VectorSource<double>* angular_rate_setter_;
+};
+
+/// @brief A very simple vehicle agent with settable inputs.
+class UnicycleCarBlueprint : public TypedAgentBlueprint<UnicycleCarAgent> {
  public:
   DELPHYNE_NO_COPY_NO_MOVE_NO_ASSIGN(UnicycleCarBlueprint)
 
@@ -46,7 +83,9 @@ class UnicycleCarBlueprint : public BasicAgentBlueprint {
     InitialConditions(double x, double y, double heading, double speed) : x(x), y(y), heading(heading), speed(speed) {}
   } initial_conditions_;
 
-  std::unique_ptr<Agent::Diagram> DoBuildDiagram(const maliput::api::RoadGeometry* road_geometry) const override;
+  std::unique_ptr<UnicycleCarAgent> DoBuildAgentInto(
+      const maliput::api::RoadGeometry* road_geometry,
+      drake::systems::DiagramBuilder<double>* simulator_builder) const override;
 };
 
 /*****************************************************************************

src/gen/angular_rate_acceleration_command.h

@@ -1,140 +1,83 @@
 // Copyright 2020 Toyota Research Institute
+//
+// Note: This file has not autogenerated via the usual Drake codegen utilities, hence we only include
+// those functions which are relevant to basic usage (e.g. specialized copy/move implementations,
+// symbolic support, etc.).
 
 #pragma once
 
 #include <cmath>
-#include <stdexcept>
 #include <string>
 #include <utility>
 #include <vector>
 
-#include <Eigen/Core>
-
-#include <drake/common/drake_bool.h>
-#include <drake/common/dummy_value.h>
-#include <drake/common/symbolic.h>
 #include <drake/systems/framework/basic_vector.h>
 
 #include <maliput/common/maliput_never_destroyed.h>
 
-// TODO(jwnimmer-tri) Elevate this to drake/common.
-#if __has_cpp_attribute(nodiscard)
-#define DRAKE_VECTOR_GEN_NODISCARD [[nodiscard]]  // NOLINT(whitespace/braces)
-#else
-#define DRAKE_VECTOR_GEN_NODISCARD
-#endif
-
 namespace delphyne {
 
-/// Describes the row indices of a AngularRateAccelerationCommand.
-struct AngularRateAccelerationCommandIndices {
-  /// The total number of rows (coordinates).
-  static const int kNumCoordinates = 2;
-
-  // The index of each individual coordinate.
-  static const int kAngularRate = 0;
-  static const int kAcceleration = 1;
-
-  /// Returns a vector containing the names of each coordinate within this
-  /// class. The indices within the returned vector matches that of this class.
-  /// In other words, `AngularRateAccelerationCommandIndices::GetCoordinateNames()[i]`
-  /// is the name for `BasicVector::GetAtIndex(i)`.
-  static const std::vector<std::string>& GetCoordinateNames();
-};
-
 /// Specializes BasicVector with specific getters and setters.
 template <typename T>
 class AngularRateAccelerationCommand final : public drake::systems::BasicVector<T> {
  public:
-  /// An abbreviation for our row index constants.
-  typedef AngularRateAccelerationCommandIndices K;
-
   /// Default constructor.  Sets all rows to their default value:
-  /// @arg @c angular_rate defaults to 0.0 rad.
+  /// @arg @c angular_rate defaults to 0.0 rad/s.
   /// @arg @c acceleration defaults to 0.0 m/s^2.
-  AngularRateAccelerationCommand() : drake::systems::BasicVector<T>(K::kNumCoordinates) {
+  AngularRateAccelerationCommand() : drake::systems::BasicVector<T>(num_coordinates_) {
     this->set_angular_rate(0.0);
     this->set_acceleration(0.0);
   }
 
-  // Note: It's safe to implement copy and move because this class is final.
-
-  /// @name Implements CopyConstructible, CopyAssignable, MoveConstructible,
-  /// MoveAssignable
-  //@{
-  AngularRateAccelerationCommand(const AngularRateAccelerationCommand& other)
-      : drake::systems::BasicVector<T>(other.values()) {}
-  AngularRateAccelerationCommand(AngularRateAccelerationCommand&& other) noexcept
-      : drake::systems::BasicVector<T>(std::move(other.values())) {}
-  AngularRateAccelerationCommand& operator=(const AngularRateAccelerationCommand& other) {
-    this->values() = other.values();
-    return *this;
-  }
-  AngularRateAccelerationCommand& operator=(AngularRateAccelerationCommand&& other) noexcept {
-    this->values() = std::move(other.values());
-    other.values().resize(0);
-    return *this;
-  }
-  //@}
-
-  /// Create a symbolic::Variable for each element with the known variable
-  /// name.  This is only available for T == symbolic::Expression.
-  template <typename U = T>
-  typename std::enable_if<std::is_same<U, drake::symbolic::Expression>::value>::type SetToNamedVariables() {
-    this->set_angular_rate(drake::symbolic::Variable("angular_rate"));
-    this->set_acceleration(drake::symbolic::Variable("acceleration"));
-  }
-
   AngularRateAccelerationCommand<T>* DoClone() const final { return new AngularRateAccelerationCommand; }
 
   /// @name Getters and Setters
   //@{
   /// The desired angular rate; positive results in the vehicle turning left.
   /// @note @c angular_rate is expressed in units of rad/s.
-  const T& angular_rate() const { return this->GetAtIndex(K::kAngularRate); }
-  void set_angular_rate(const T& angular_rate) {
+  const T& angular_rate() const {
     ThrowIfEmpty();
-    this->SetAtIndex(K::kAngularRate, angular_rate);
+    return this->GetAtIndex(index_angular_rate_);
   }
-  /// Fluent setter that matches angular_rate().
-  /// Returns a copy of `this` with angular_rate set to a new value.
-  DRAKE_VECTOR_GEN_NODISCARD
-  AngularRateAccelerationCommand<T> with_angular_rate(const T& angular_rate) const {
-    AngularRateAccelerationCommand<T> result(*this);
-    result.set_angular_rate(angular_rate);
-    return result;
+  // Setter that matches angular_rate().
+  void set_angular_rate(const T& angular_rate) {
+    ThrowIfEmpty();
+    this->SetAtIndex(index_angular_rate_, angular_rate);
   }
   /// The signed acceleration, positive means speed up; negative means slow
   /// down, but should not move in reverse.
   /// @note @c acceleration is expressed in units of m/s^2.
   const T& acceleration() const {
     ThrowIfEmpty();
-    return this->GetAtIndex(K::kAcceleration);
+    return this->GetAtIndex(index_acceleration_);
   }
   // Setter that matches acceleration().
   void set_acceleration(const T& acceleration) {
     ThrowIfEmpty();
-    this->SetAtIndex(K::kAcceleration, acceleration);
-  }
-  /// Fluent setter that matches acceleration().
-  /// Returns a copy of `this` with acceleration set to a new value.
-  DRAKE_VECTOR_GEN_NODISCARD
-  AngularRateAccelerationCommand<T> with_acceleration(const T& acceleration) const {
-    AngularRateAccelerationCommand<T> result(*this);
-    result.set_acceleration(acceleration);
-    return result;
+    this->SetAtIndex(index_acceleration_, acceleration);
   }
+
+  // Getters for the intrinsics of this vector.
+  static int num_coordinates() { return num_coordinates_; }
+  static int index_angular_rate() { return index_angular_rate_; }
+  static int index_acceleration() { return index_acceleration_; }
   //@}
 
-  /// See AngularRateAccelerationCommandIndices::GetCoordinateNames().
+  /// Returns a vector containing the names of each coordinate within this
+  /// class. The indices within the returned vector matches that of this class.
+  /// In other words, `GetCoordinateNames()[i]` is the name for `BasicVector::GetAtIndex(i)`.
   static const std::vector<std::string>& GetCoordinateNames() {
-    return AngularRateAccelerationCommandIndices::GetCoordinateNames();
+    static const maliput::common::never_destroyed<std::vector<std::string>> coordinates(std::vector<std::string>{
+        "angular_rate",  // BR
+        "acceleration",  // BR
+    });
+    return coordinates.access();
   }
 
   /// Returns whether the current values of this vector are well-formed.
-  drake::boolean<T> IsValid() const {
+  bool IsValid() const {
     using std::isnan;
-    drake::boolean<T> result{true};
+    bool result{true};
     result = result && !isnan(angular_rate());
     result = result && !isnan(acceleration());
     return result;
@@ -148,8 +91,13 @@ class AngularRateAccelerationCommand final : public drake::systems::BasicVector<
           "accessor methods may no longer be used");
     }
   }
+
+  /// The total number of rows (coordinates).
+  static const int num_coordinates_{2};
+
+  // The index of each individual coordinate.
+  static const int index_angular_rate_{0};
+  static const int index_acceleration_{1};
 };
 
 }  // namespace delphyne
-
-#undef DRAKE_VECTOR_GEN_NODISCARD

src/protobuf/CMakeLists.txt

@@ -11,7 +11,6 @@ include(${PROJECT_SOURCE_DIR}/cmake/Protobuf.cmake)
 set(PROTO_MESSAGES
   agent_state.proto
   agent_state_v.proto
-  automotive_angular_rate_acceleration_command.proto
   automotive_driving_command.proto
   playback_status.proto
   rpy_angles.proto