Add a parameter-free unicycle car system and agent

python/delphyne/agents.cc

@@ -19,6 +19,7 @@
 #include "agents/rail_car.h"
 #include "agents/simple_car.h"
 #include "agents/trajectory_agent.h"
+#include "agents/unicycle_car.h"
 #include "delphyne/mi6/agent_base.h"
 #include "delphyne/mi6/agent_base_blueprint.h"
 #include "delphyne/mi6/agent_simulation.h"
@@ -38,6 +39,7 @@ using delphyne::RailCar;
 using delphyne::RailCarBlueprint;
 using delphyne::SimpleCarBlueprint;
 using delphyne::TrajectoryAgentBlueprint;
+using delphyne::UnicycleCarBlueprint;
 
 namespace {
 
@@ -82,6 +84,10 @@ PYBIND11_MODULE(agents, m) {
                     const std::vector<std::vector<double>>&>(),
            "Construct and configure a trajectory agent", py::arg("name"), py::arg("times"), py::arg("headings"),
            py::arg("translations"));
+
+  py::class_<UnicycleCarBlueprint, AgentBlueprint>(m, "UnicycleCarBlueprint")
+      .def(py::init<const std::string&, double, double, double, double>(), "Construct and configure a unicycle car",
+           py::arg("name"), py::arg("x"), py::arg("y"), py::arg("heading"), py::arg("speed"));
 }
 
 /*****************************************************************************

python/delphyne/behaviours/agents.py

@@ -163,3 +163,34 @@ def setup(self, *, builder):
             )
         )
 
+
+class UnicycleCar(py_trees.behaviours.Success):
+    """Introduce a unicycle car in the road."""
+
+    def __init__(self, name=py_trees.common.Name.AUTO_GENERATED,
+                 initial_pose=(0., 0., 0.), speed=1.):
+        """
+        :param name: a name for the car.
+        :param initial_pose: initial car (SE2) pose in the world frame.
+            That is, its (x, y, heading) coordinates, either as a tuple of
+            floats or as a callable expression that takes a road geometry
+            and returns a tuple of floats.
+        :param speed: for the agent as measured in the world frame, in m/s.
+        """
+        super().__init__(name)
+        self.initial_pose = initial_pose
+        self.speed = speed
+
+    def setup(self, *, builder):
+        initial_x, initial_y, heading = resolve(
+            self.initial_pose, builder.get_road_geometry()
+        )
+        builder.add_agent(
+            delphyne.agents.UnicycleCarBlueprint(
+                name=self.name,
+                x=initial_x,          # initial x-coordinate (m)
+                y=initial_y,          # initial y-coordinate (m)
+                heading=heading,      # heading (radians)
+                speed=self.speed      # speed in the direction of travel (m/s)
+            )
+        )

src/agents/CMakeLists.txt

@@ -7,6 +7,7 @@ add_library(agents
   rail_car.cc
   simple_car.cc
   trajectory_agent.cc
+  unicycle_car.cc
 )
 set_target_properties(agents PROPERTIES OUTPUT_NAME ${PROJECT_NAME}-agents)
 

src/agents/unicycle_car.cc

@@ -0,0 +1,91 @@
+/**
+ * @file src/agents/simple_car
+ *
+ * Copyright 2020 Toyota Research Institute
+ */
+/*****************************************************************************
+ ** Includes
+ *****************************************************************************/
+
+#include "agents/unicycle_car.h"
+
+#include <string>
+#include <utility>
+
+#include "backend/ign_subscriber_system.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>
+
+/*****************************************************************************
+ ** Namespaces
+ *****************************************************************************/
+
+namespace delphyne {
+
+/*****************************************************************************
+ ** Implementation
+ *****************************************************************************/
+
+UnicycleCarBlueprint::UnicycleCarBlueprint(const std::string& name, double x, double y, double heading, double speed)
+    : BasicAgentBlueprint(name), initial_conditions_(x, y, heading, speed) {}
+
+std::unique_ptr<Agent::Diagram> UnicycleCarBlueprint::DoBuildDiagram(
+    const maliput::api::RoadGeometry* road_geometry) const {
+  maliput::common::unused(road_geometry);
+
+  AgentBlueprint::DiagramBuilder builder(this->name());
+
+  /*********************
+   * Context
+   *********************/
+  typedef SimpleCarState<double> ContextContinuousState;
+  ContextContinuousState context_continuous_state;
+  context_continuous_state.set_x(initial_conditions_.x);
+  context_continuous_state.set_y(initial_conditions_.y);
+  context_continuous_state.set_heading(initial_conditions_.heading);
+  context_continuous_state.set_velocity(initial_conditions_.speed);
+
+  /*********************
+   * Unicycle Car System
+   *********************/
+  typedef UnicycleCar<double> UnicycleCarSystem;
+  UnicycleCarSystem* simple_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));
+
+  auto input_command_translator = builder.AddSystem(std::make_unique<IgnAngularRateAccelerationCommandToDrake>());
+
+  // Ignition anglular rate and acceleration commands received through the subscriber are translated
+  // to Drake.
+  builder.Connect(*input_command_subscriber, *input_command_translator);
+
+  // And then the translated Drake command is sent to the car.
+  builder.Connect(*input_command_translator, *simple_car_system);
+
+  /*********************
+   * Diagram Outputs
+   *********************/
+  builder.ExportStateOutput(simple_car_system->state_output());
+  builder.ExportPoseOutput(simple_car_system->pose_output());
+  builder.ExportVelocityOutput(simple_car_system->velocity_output());
+
+  return builder.Build();
+}
+
+/*****************************************************************************
+ ** Trailers
+ *****************************************************************************/
+
+}  // namespace delphyne

src/agents/unicycle_car.h

@@ -0,0 +1,56 @@
+/**
+ * @file src/agents/simple_car.h
+ *
+ * Copyright 2020 Toyota Research Institute
+ */
+/*****************************************************************************
+** Includes
+*****************************************************************************/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <maliput/api/road_geometry.h>
+#include "delphyne/mi6/agent_base_blueprint.h"
+
+/*****************************************************************************
+** Namespaces
+*****************************************************************************/
+
+namespace delphyne {
+
+/*****************************************************************************
+** Interfaces
+*****************************************************************************/
+
+/// @brief A very simple vehicle agent that can be teleoperated.
+class UnicycleCarBlueprint : public BasicAgentBlueprint {
+ public:
+  DELPHYNE_NO_COPY_NO_MOVE_NO_ASSIGN(UnicycleCarBlueprint)
+
+  explicit UnicycleCarBlueprint(const std::string& name, double x, double y, double heading, double speed);
+
+ private:
+  // Container for the agent's initial configuration.
+  //
+  // Note: this is independent of whatever computational mechanisms
+  // are used internally and is a useful construct for recording and
+  // logging / streaming to debug configuration errors.
+  struct InitialConditions {
+    double x{0.0};        // scene x-coordinate (m)
+    double y{0.0};        // scene y-coordinate (m)
+    double heading{0.0};  // scene heading (around z-axis (radians)
+    double speed{0.0};    // speed in axis defined by the heading (m/s)
+    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;
+};
+
+/*****************************************************************************
+** Trailers
+*****************************************************************************/
+
+}  // namespace delphyne

src/gen/angular_rate_acceleration_command.cc

@@ -0,0 +1,19 @@
+// Copyright 2020 Toyota Research Institute
+
+#include "gen/angular_rate_acceleration_command.h"
+
+namespace delphyne {
+
+const int AngularRateAccelerationCommandIndices::kNumCoordinates;
+const int AngularRateAccelerationCommandIndices::kAngularRate;
+const int AngularRateAccelerationCommandIndices::kAcceleration;
+
+const std::vector<std::string>& 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();
+}
+
+}  // namespace delphyne

src/gen/angular_rate_acceleration_command.h

@@ -0,0 +1,155 @@
+// Copyright 2020 Toyota Research Institute
+
+#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 acceleration defaults to 0.0 m/s^2.
+  AngularRateAccelerationCommand() : drake::systems::BasicVector<T>(K::kNumCoordinates) {
+    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) {
+    ThrowIfEmpty();
+    this->SetAtIndex(K::kAngularRate, 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;
+  }
+  /// 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);
+  }
+  // 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;
+  }
+  //@}
+
+  /// See AngularRateAccelerationCommandIndices::GetCoordinateNames().
+  static const std::vector<std::string>& GetCoordinateNames() {
+    return AngularRateAccelerationCommandIndices::GetCoordinateNames();
+  }
+
+  /// Returns whether the current values of this vector are well-formed.
+  drake::boolean<T> IsValid() const {
+    using std::isnan;
+    drake::boolean<T> result{true};
+    result = result && !isnan(angular_rate());
+    result = result && !isnan(acceleration());
+    return result;
+  }
+
+ private:
+  void ThrowIfEmpty() const {
+    if (this->values().size() == 0) {
+      throw std::out_of_range(
+          "The AngularRateAccelerationCommand vector has been moved-from; "
+          "accessor methods may no longer be used");
+    }
+  }
+};
+
+}  // namespace delphyne
+
+#undef DRAKE_VECTOR_GEN_NODISCARD