validate, fix tests

include/delphyne/mi6/agent_diagram_builder.h

@@ -6,7 +6,10 @@
 ** Includes
 *****************************************************************************/
 
+#include <map>
 #include <memory>
+#include <stdexcept>
+#include <string>
 
 #include <drake/systems/framework/diagram_builder.h>
 #include <drake/systems/framework/framework_common.h>  // OutputPortIndex
@@ -16,6 +19,7 @@
 #include <drake/systems/rendering/pose_bundle.h>
 
 #include "delphyne/mi6/diagram_bundle.h"
+#include "delphyne/macros.h"
 #include "delphyne/types.h"
 
 /*****************************************************************************
@@ -44,7 +48,7 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder<T> {
  public:
   /// @brief Default constructor.
   /// @param[in] name : Convenient descriptive name for the diagram.
-  AgentDiagramBuilder(const std::string& name) : name_(name) {
+  explicit AgentDiagramBuilder(const std::string& name) : name_(name) {
     traffic_poses_ =
         this->AddSystem(std::make_unique<drake::systems::PassThrough<double>>(
             drake::systems::Value<
@@ -86,6 +90,21 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder<T> {
     // inputs: "traffic_poses"
     // Consider putting the check in a AgentDiagramBundle::Validate()
     // which would be a subclass of DiagramBundle
+    DELPHYNE_VALIDATE(
+        outputs_.count("state") == 1,
+        std::runtime_error,
+        "A state output port has not been exported (see "
+        "AgentDiagramBuilder::ExportStateOutput)");
+    DELPHYNE_VALIDATE(
+        outputs_.count("pose") == 1,
+        std::runtime_error,
+        "A pose output port has not been exported (see "
+        "AgentDiagramBuilder::ExportPoseOutput)");
+    DELPHYNE_VALIDATE(
+        outputs_.count("velocity") == 1,
+        std::runtime_error,
+        "A velocity output port has not been exported (see "
+        "AgentDiagramBuilder::ExportPoseOutput)");
     DiagramBundle<T> bundle;
     bundle.diagram = drake::systems::DiagramBuilder<T>::Build();
     bundle.diagram->set_name(name_);

include/delphyne/mi6/diagram_bundle.h

@@ -8,6 +8,7 @@
 
 #include <map>
 #include <memory>
+#include <string>
 
 #include <drake/systems/framework/diagram_builder.h>
 #include <drake/systems/framework/framework_common.h>  // OutputPortIndex

src/agents/trajectory_agent.cc

@@ -37,13 +37,6 @@ TrajectoryAgent::TrajectoryAgent(
     const std::vector<double>& headings,
     const std::vector<std::vector<double>>& translations)
     : delphyne::Agent(name) {
-  // TODO(daniel.stonier) stop using this, make use of an initial value on
-  // the pose output
-  const drake::automotive::PoseVelocity initial_car_pose_velocity =
-      trajectory_->value(times.back());
-  initial_world_pose_ =
-      drake::Translation3<double>(initial_car_pose_velocity.translation()) *
-      initial_car_pose_velocity.rotation();
 
   Eigen::Quaternion<double> zero_heading(
       Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitZ()));
@@ -66,6 +59,14 @@ TrajectoryAgent::TrajectoryAgent(
   trajectory_ = std::make_unique<drake::automotive::Trajectory>(
       drake::automotive::Trajectory::Make(times, eigen_orientations,
                                           eigen_translations));
+
+  // TODO(daniel.stonier) stop using this, make use of an initial value on
+  // the pose output
+  const drake::automotive::PoseVelocity initial_car_pose_velocity =
+      trajectory_->value(times.front());
+  initial_world_pose_ =
+      drake::Translation3<double>(initial_car_pose_velocity.translation()) *
+      initial_car_pose_velocity.rotation();
 }
 
 Agent::DiagramBundle TrajectoryAgent::BuildDiagram() const {

test/regression/cpp/automotive_simulator_test.cc

@@ -187,7 +187,7 @@ TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCar) {
   // Simulate an external system sending a driving command to the car at
   // full throttle
   using ignition::msgs::AutomotiveDrivingCommand;
-  const std::string kTeleopTopicName{"teleop/0"};
+  const std::string kTeleopTopicName{"teleop/bob"};
   ignition::transport::Node node;
   auto publisher = node.Advertise<AutomotiveDrivingCommand>(kTeleopTopicName);
 
@@ -405,8 +405,9 @@ TEST_F(AutomotiveSimulatorTest, TestTrajectoryAgent) {
 
 TEST_F(AutomotiveSimulatorTest, TestBadRailcars) {
   auto simulator = std::make_unique<AutomotiveSimulator<double>>();
-  std::unique_ptr<const drake::maliput::dragway::RoadGeometry> dragway =
-      CreateDragway("TestDragway", 1);
+
+  auto dragway =
+      simulator->SetRoadGeometry(CreateDragway("TestDragway", 1));
 
   const double kR{0.5};
 
@@ -419,28 +420,24 @@ TEST_F(AutomotiveSimulatorTest, TestBadRailcars) {
       0.0,   // speed
       0.0,   // nominal_speed
       *dragway);
-  const int id1 = simulator->AddAgent(std::move(agent_1));
   EXPECT_ARGUMENT_THROW(simulator->AddAgent(std::move(agent_1)),
                         "Rail cars need a road geometry to drive on, make sure "
                         "the simulation is configured with one.");
 
-  // sim is using a different road geometry
-  simulator->SetRoadGeometry(std::move(dragway));
   std::unique_ptr<const drake::maliput::dragway::RoadGeometry>
       different_dragway = CreateDragway("DifferentDragway", 2);
-  auto agent_2 = std::make_unique<delphyne::RailCar>(
-      "bar", *(different_dragway->junction(0)->segment(0)->lane(0)),
-      true,  // lane_direction,
-      0.0,   // position
-      kR,    // offset
-      0.0,   // speed
-      0.0,   // nominal_speed
-      *dragway);
-  const int id2 = simulator->AddAgent(std::move(agent_2));
 
-  EXPECT_RUNTIME_THROW(simulator->AddAgent(std::move(agent_2)),
-                       "The provided initial lane is not on the same road "
-                       "geometry as that used by the simulation");
+  EXPECT_ARGUMENT_THROW(
+      std::make_unique<delphyne::RailCar>(
+          "bar", *(different_dragway->junction(0)->segment(0)->lane(0)),
+          true,  // lane_direction,
+          0.0,   // position
+          kR,    // offset
+          0.0,   // speed
+          0.0,   // nominal_speed
+          *dragway),
+      "The provided initial lane is not on the same road "
+      "geometry as that used by the simulation");
 }
 
 // Covers railcar behavior.