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,10 +60,15 @@ 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); },
-           py::return_value_policy::reference_internal)
+      .def(
+          "get_agent",
+          [](AgentBlueprint* self, const AgentSimulation& simulation) { return &self->GetAgent(simulation); },
+          py::return_value_policy::reference_internal)
       .def("get_mutable_agent", &AgentBlueprint::GetMutableAgent, py::return_value_policy::reference_internal);
 
   py::class_<SimpleCarBlueprint, AgentBlueprint>(m, "SimpleCarBlueprint")

python/delphyne/simulation.cc

@@ -112,11 +112,12 @@ PYBIND11_MODULE(simulation, m) {
 
   py::class_<AgentSimulationBuilder>(m, "AgentSimulationBuilder")
       .def(py::init([](void) { return std::make_unique<AgentSimulationBuilder>(); }))
-      .def("add_agent",
-           [](AgentSimulationBuilder* self, std::unique_ptr<AgentBlueprint> blueprint) {
-             return self->AddAgent(std::move(blueprint));
-           },
-           py::return_value_policy::reference_internal)
+      .def(
+          "add_agent",
+          [](AgentSimulationBuilder* self, std::unique_ptr<AgentBlueprint> blueprint) {
+            return self->AddAgent(std::move(blueprint));
+          },
+          py::return_value_policy::reference_internal)
       .def("set_road_geometry",
            py::overload_cast<std::unique_ptr<const ::maliput::api::RoadGeometry>>(
                &AgentSimulationBuilder::SetRoadGeometry),
@@ -143,12 +144,14 @@ PYBIND11_MODULE(simulation, m) {
 
   py::class_<AgentSimulation>(m, "AgentSimulation")
       .def("get_collisions", &AgentSimulation::GetCollisions, py::return_value_policy::reference_internal)
-      .def("get_agent_by_name",
-           [](AgentSimulation* self, const std::string& name) { return &self->GetAgentByName(name); },
-           py::return_value_policy::reference_internal)
-      .def("get_mutable_agent_by_name",
-           [](AgentSimulation* self, const std::string& name) { return self->GetMutableAgentByName(name); },
-           py::return_value_policy::reference_internal)
+      .def(
+          "get_agent_by_name",
+          [](AgentSimulation* self, const std::string& name) { return &self->GetAgentByName(name); },
+          py::return_value_policy::reference_internal)
+      .def(
+          "get_mutable_agent_by_name",
+          [](AgentSimulation* self, const std::string& name) { return self->GetMutableAgentByName(name); },
+          py::return_value_policy::reference_internal)
       .def("get_diagram", &AgentSimulation::GetDiagram, py::return_value_policy::reference_internal)
       .def("get_context", &AgentSimulation::GetContext, py::return_value_policy::reference_internal)
       .def("get_mutable_context", &AgentSimulation::GetMutableContext, py::return_value_policy::reference_internal)

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;
 };
 
 /*****************************************************************************