diff --git a/include/delphyne/mi6/CMakeLists.txt b/include/delphyne/mi6/CMakeLists.txt index 59d0723cf..e3d4de23c 100644 --- a/include/delphyne/mi6/CMakeLists.txt +++ b/include/delphyne/mi6/CMakeLists.txt @@ -21,8 +21,8 @@ target_link_libraries(mi6 install( FILES agent_base.h + agent_diagram.h agent_diagram_builder.h - diagram_bundle.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}${PROJECT_MAJOR_VERSION}/${PROJECT_NAME}/mi6 ) diff --git a/include/delphyne/mi6/agent_base.h b/include/delphyne/mi6/agent_base.h index f4f2d8981..35ed63ca6 100644 --- a/include/delphyne/mi6/agent_base.h +++ b/include/delphyne/mi6/agent_base.h @@ -11,10 +11,19 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include "delphyne/macros.h" +#include "delphyne/mi6/agent_diagram.h" #include "delphyne/mi6/agent_diagram_builder.h" -#include "delphyne/mi6/diagram_bundle.h" #include "delphyne/types.h" /***************************************************************************** @@ -44,9 +53,9 @@ namespace delphyne { template class AgentBase { public: - /// Type for packaging diagram and indices together in a bundle. - using DiagramBundle = delphyne::DiagramBundle; - /// Specific builder for this agent. Use inside BuildDiagram(). + /// Specific diagram for this agent. As returned by BuildDiagram(). + using Diagram = AgentDiagram; + /// Specific builder for this agent. To be used inside BuildDiagram(). using DiagramBuilder = AgentDiagramBuilder; /// @brief Constructor initialising common agent parameters. @@ -64,7 +73,65 @@ class AgentBase { /// @return DiagramBundle : The diagram along with the relevant input /// and output indices for an agent diagram that allow it to be /// wired up with systems in the simulator diagram. - virtual std::unique_ptr BuildDiagram() const = 0; + Diagram* TakePartIn(drake::systems::DiagramBuilder* builder, int id) { + // TODO(hidmic): should we be passing the simulator itself instead? + id_ = id; + diagram_ = builder->AddSystem(BuildDiagram()); + return diagram_; + } + + const Diagram& get_diagram() const { return *diagram_; } + + Diagram* get_mutable_diagram() { return diagram_; } + + drake::Isometry3 + GetPose(const drake::systems::Context& context) const { + DELPHYNE_VALIDATE(diagram_ != nullptr, std::runtime_error, + "This agent is not associated to any simulation!"); + const drake::systems::OutputPort& pose_output_port = + diagram_->get_output_port("pose"); + std::unique_ptr port_value = + pose_output_port.Allocate(); + pose_output_port.Calc(context, port_value.get()); + // TODO(hidmic): figure out why type assertions fail if + // trying to get the port value with the correct type in + // a single step (presumably related to the fact that + // BasicVector defines Clone() but its subclasses only + // inherit it, see drake::is_cloneable). + using drake::systems::BasicVector; + using drake::systems::rendering::PoseVector; + const BasicVector& base_vector = + port_value->template GetValueOrThrow>(); + const PoseVector& pose_vector = + dynamic_cast&>(base_vector); + return pose_vector.get_isometry(); + } + + drake::TwistVector + GetVelocity(const drake::systems::Context& context) const { + DELPHYNE_VALIDATE(diagram_ != nullptr, std::runtime_error, + "This agent is not associated to any simulation!"); + const drake::systems::OutputPort& vel_output_port = + diagram_->get_output_port("velocity"); + std::unique_ptr port_value = + vel_output_port.Allocate(); + vel_output_port.Calc(context, port_value.get()); + // TODO(hidmic): figure out why type assertions fail if + // trying to get the port value with the correct type in + // a single step (presumably related to the fact that + // BasicVector defines Clone() but its subclasses only + // inherit it, see drake::is_cloneable). + using drake::systems::BasicVector; + using drake::systems::rendering::FrameVelocity; + const BasicVector& base_vector = + port_value->template GetValueOrThrow>(); + const FrameVelocity& frame_velocity = + dynamic_cast&>(base_vector); + return frame_velocity.get_velocity().get_coeffs(); + } + + /// @brief ID accessor + int id() const { return id_; } /// @brief Name accessor const std::string& name() const { return name_; } @@ -93,13 +160,19 @@ class AgentBase { } protected: + int id_; std::string name_; + // TODO(daniel.stonier) stop using this, make use of an // initial value on the pose output (used by geometry settings for // the collision subsystem) drake::Isometry3 initial_world_pose_; private: + virtual std::unique_ptr BuildDiagram() const = 0; + + Diagram* diagram_{nullptr}; + // TODO(daniel.stonier) dubious whether we should have // simulator specific machinery here std::set geometry_ids_{}; diff --git a/include/delphyne/mi6/agent_diagram.h b/include/delphyne/mi6/agent_diagram.h new file mode 100644 index 000000000..bddf4c8a4 --- /dev/null +++ b/include/delphyne/mi6/agent_diagram.h @@ -0,0 +1,119 @@ +// Copyright 2018 Toyota Research Institute + +#pragma once + +/***************************************************************************** +** Includes +*****************************************************************************/ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include // OutputPortIndex +#include +#include + +#include "delphyne/macros.h" +#include "delphyne/types.h" + +/***************************************************************************** +** Namespaces +*****************************************************************************/ + +namespace delphyne { + +/***************************************************************************** +** Interfaces +*****************************************************************************/ + +namespace detail { + +/// @brief A generic bundle containing and describing a diagram. +/// +/// There's no easy way to introspect on the input/output ports of a +/// diagram or even enumerate the indices of the port since they are +/// dynamically generated at build time, so this passes along the +/// indices with the diagram so that it can be encapsulated inside other +/// diagrams and wired up with that diagram's systems. +/// +/// To avoid zombie diagram bundles, this class should be exclusively +/// used with unique pointers. Copy/Move/Assign capabilities are +/// disabled. +/// +/// @tparam One of double, delphyne::AutoDiff or delphyne::Symbolic. +template +class NamedPortSystem : public Base { + public: + static_assert(std::is_base_of, Base>::value, + "Only a System can have their ports mapped."); + + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(NamedPortSystem); + + template + explicit NamedPortSystem(Args... args) : + Base(std::forward(args)...) { } + + void set_input_names( + std::map inputs_mapping) { + inputs_mapping_ = inputs_mapping; + } + + void set_output_names( + std::map outputs_mapping) { + outputs_mapping_ = outputs_mapping; + } + + const drake::systems::InputPortDescriptor& + get_input_port(int port_index) const { + // Caveat: `System::get_input_port` is not virtual. + return Base::get_input_port(port_index); + } + + const drake::systems::InputPortDescriptor& + get_input_port(const std::string& port_name) const { + DELPHYNE_VALIDATE(inputs_mapping_.count(port_name) != 0, std::runtime_error, + "Input port \"" + port_name + "\" could not be found."); + return get_input_port(inputs_mapping_.at(port_name)); + } + + const drake::systems::OutputPort& + get_output_port(int port_index) const { + // Caveat: `System::get_output_port` is not virtual. + return Base::get_output_port(port_index); + } + + const drake::systems::OutputPort& + get_output_port(const std::string& port_name) const { + DELPHYNE_VALIDATE( + outputs_mapping_.count(port_name) != 0, std::runtime_error, + "Output port \"" + port_name + "\" could not be found."); + return this->get_output_port(outputs_mapping_.at(port_name)); + } + + private: + std::map inputs_mapping_{}; + std::map outputs_mapping_{}; +}; + +} // namespace detail + +template +class AgentDiagram : + public detail::NamedPortSystem, T> { + public: + explicit AgentDiagram(drake::systems::DiagramBuilder* builder) { + builder->BuildInto(this); + } +}; + +/***************************************************************************** +** Trailers +*****************************************************************************/ + +} // namespace delphyne diff --git a/include/delphyne/mi6/agent_diagram_builder.h b/include/delphyne/mi6/agent_diagram_builder.h index 115e07253..81730fc86 100644 --- a/include/delphyne/mi6/agent_diagram_builder.h +++ b/include/delphyne/mi6/agent_diagram_builder.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include // OutputPortIndex @@ -19,7 +20,7 @@ #include #include "delphyne/macros.h" -#include "delphyne/mi6/diagram_bundle.h" +#include "delphyne/mi6/agent_diagram.h" #include "delphyne/types.h" /***************************************************************************** @@ -53,7 +54,7 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder { this->AddSystem(std::make_unique>( drake::systems::Value< drake::systems::rendering::PoseBundle>(0))); - inputs_["traffic_poses"] = + inputs_mapping_["traffic_poses"] = this->ExportInput(traffic_poses_->get_input_port()); } @@ -67,10 +68,10 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder { /// than once before building. void ExportStateOutput(const drake::systems::OutputPort& output) { DELPHYNE_VALIDATE( - outputs_.count("state") == 0, std::runtime_error, + outputs_mapping_.count("state") == 0, std::runtime_error, "A state output port has already been exported and this diagram " "enforces that there can be only one."); - outputs_["state"] = this->ExportOutput(output); + outputs_mapping_["state"] = this->ExportOutput(output); } /// @brief Export the specified pose output port. @@ -83,10 +84,10 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder { /// than once before building. void ExportPoseOutput(const drake::systems::OutputPort& output) { DELPHYNE_VALIDATE( - outputs_.count("pose") == 0, std::runtime_error, + outputs_mapping_.count("pose") == 0, std::runtime_error, "A pose output port has already been exported and this diagram " "enforces that there can be only one."); - outputs_["pose"] = this->ExportOutput(output); + outputs_mapping_["pose"] = this->ExportOutput(output); } /// @brief Export the specified velocity output port. @@ -99,10 +100,10 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder { /// than once before building. void ExportVelocityOutput(const drake::systems::OutputPort& output) { DELPHYNE_VALIDATE( - outputs_.count("velocity") == 0, std::runtime_error, + outputs_mapping_.count("velocity") == 0, std::runtime_error, "A velocity output port has already been exported and this diagram " "enforces that there can be only one."); - outputs_["velocity"] = this->ExportOutput(output); + outputs_mapping_["velocity"] = this->ExportOutput(output); } /// @brief Connect the input traffic poses port to internal systems. @@ -117,32 +118,32 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder { } /// @brief Build the diagram bundle and append the indices. - std::unique_ptr> Build() { + std::unique_ptr> Build() { // Check that all indices have been set // inputs: "traffic_poses" // outputs: "state", "pose", "velocity" - DELPHYNE_VALIDATE(outputs_.count("state") == 1, std::runtime_error, + DELPHYNE_VALIDATE(outputs_mapping_.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, + DELPHYNE_VALIDATE(outputs_mapping_.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)"); - auto bundle = std::make_unique>(); - bundle->diagram = drake::systems::DiagramBuilder::Build(); - bundle->diagram->set_name(name_); - bundle->outputs = outputs_; - bundle->inputs = inputs_; - return bundle; + DELPHYNE_VALIDATE( + outputs_mapping_.count("velocity") == 1, std::runtime_error, + "A velocity output port has not been exported (see " + "AgentDiagramBuilder::ExportPoseOutput)"); + auto diagram = std::make_unique>(this); + diagram->set_name(name_); + diagram->set_input_names(inputs_mapping_); + diagram->set_output_names(outputs_mapping_); + return std::move(diagram); } private: const std::string name_; drake::systems::PassThrough* traffic_poses_; - std::map outputs_; - std::map inputs_; + std::map inputs_mapping_{}; + std::map outputs_mapping_{}; }; /***************************************************************************** diff --git a/include/delphyne/mi6/diagram_bundle.h b/include/delphyne/mi6/diagram_bundle.h deleted file mode 100644 index 3ae10b4a6..000000000 --- a/include/delphyne/mi6/diagram_bundle.h +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#pragma once - -/***************************************************************************** -** Includes -*****************************************************************************/ - -#include -#include -#include - -#include -#include // OutputPortIndex -#include - -#include "delphyne/types.h" - -/***************************************************************************** -** Namespaces -*****************************************************************************/ - -namespace delphyne { - -/***************************************************************************** -** Interfaces -*****************************************************************************/ - -/// @brief A generic bundle containing and describing a diagram. -/// -/// There's no easy way to introspect on the input/output ports of a -/// diagram or even enumerate the indices of the port since they are -/// dynamically generated at build time, so this passes along the -/// indices with the diagram so that it can be encapsulated inside other -/// diagrams and wired up with that diagram's systems. -/// -/// To avoid zombie diagram bundles, this class should be exclusively -/// used with unique pointers. Copy/Move/Assign capabilities are -/// disabled. -/// -/// @tparam One of double, delphyne::AutoDiff or delphyne::Symbolic. -template -struct DiagramBundle { - DiagramBundle() = default; - DiagramBundle(const DiagramBundle& other) = delete; - DiagramBundle& operator=(const DiagramBundle&) = delete; - - std::unique_ptr> diagram{}; - std::map outputs; - std::map inputs; -}; - -/***************************************************************************** -** Trailers -*****************************************************************************/ - -} // namespace delphyne diff --git a/python/delphyne/CMakeLists.txt b/python/delphyne/CMakeLists.txt index 4282921ac..95e44766c 100644 --- a/python/delphyne/CMakeLists.txt +++ b/python/delphyne/CMakeLists.txt @@ -64,6 +64,12 @@ install( pybind11_add_module(simulation_bindings simulation.cc) set_target_properties(simulation_bindings PROPERTIES OUTPUT_NAME simulation) +target_include_directories( + simulation_bindings + PRIVATE + $ +) + target_link_libraries(simulation_bindings PRIVATE simulation_runner diff --git a/python/delphyne/agents.cc b/python/delphyne/agents.cc index 8e3c178c4..d720b8a73 100644 --- a/python/delphyne/agents.cc +++ b/python/delphyne/agents.cc @@ -9,6 +9,7 @@ #include +#include #include #include #include @@ -36,7 +37,10 @@ PYBIND11_MODULE(agents, m) { py::module::import("pydrake.maliput.api"); py::class_(m, "AgentBase") - .def("name", &delphyne::Agent::name); + .def("id", &delphyne::Agent::id) + .def("name", &delphyne::Agent::name) + .def("get_pose", &delphyne::Agent::GetPose) + .def("get_velocity", &delphyne::Agent::GetVelocity); py::class_(m, "MobilCar") .def(py::init +#include +#include +#include + +#include +#include + +namespace pybind11 { +namespace detail { + +template ::value, int>::type = 0> +void collect_references(const Source& source, OutputIt output) { } + +template ::value, int>::type = 0> +void collect_references(const Source& source, OutputIt output) { + *output++ = cast(source); +} + +template +void collect_references(const std::vector& source, OutputIt output) { + for (const Element& e : source) { + collect_references(e, output); + } +} + +template +void collect_references(const std::pair& source, + OutputIt output) { + collect_references(source.first, output); + collect_references(source.second, output); +} + +} // namespace detail + +template +auto guard_internal_references(Return (Class::*method)(Args...)) { + return [method] (Class *that, Args... args) { + Return return_value = (that->*method)(std::forward(args)...); + std::vector visibilized; + detail::collect_references( + return_value, std::back_inserter(visibilized)); + object self = cast(that); + for (object& obj : visibilized) { + keep_alive_impl(obj, self); + } + return cast(return_value); + }; +} + +template +auto guard_internal_references(Return (Class::*method)(Args...) const) { + return [method] (Class *that, Args... args) { + Return return_value = (that->*method)(std::forward(args)...); + std::vector visibilized; + detail::collect_references( + return_value, std::back_inserter(visibilized)); + object self = cast(that); + for (object& obj : visibilized) { + keep_alive_impl(obj, self); + } + return cast(return_value); + }; +} + +} // namespace pybind11 diff --git a/python/delphyne/simulation.cc b/python/delphyne/simulation.cc index f47b5b108..33b05b3d7 100644 --- a/python/delphyne/simulation.cc +++ b/python/delphyne/simulation.cc @@ -19,6 +19,8 @@ #include "backend/simulation_run_stats.h" #include "backend/simulation_runner.h" +#include "delphyne/pybind_utils.h" + /***************************************************************************** ** Namespaces *****************************************************************************/ @@ -123,14 +125,21 @@ PYBIND11_MODULE(simulation, m) { py::class_>(m, "AutomotiveSimulator") .def(py::init( [](void) { return std::make_unique>(); })) - .def("add_agent", &AutomotiveSimulator::AddAgent) - .def("get_agent_by_id", - &AutomotiveSimulator::GetAgentById, + .def("add_agent", &AutomotiveSimulator::AddAgent, + py::return_value_policy::reference_internal) + .def("get_agent_by_name", + &AutomotiveSimulator::GetAgentByName, + py::return_value_policy::reference_internal) + .def("get_mutable_agent_by_name", + &AutomotiveSimulator::GetMutableAgentByName, + py::return_value_policy::reference_internal) + .def("get_context", &AutomotiveSimulator::GetContext, py::return_value_policy::reference_internal) - .def("get_mutable_agent_by_id", - &AutomotiveSimulator::GetMutableAgentById, + .def("get_mutable_context", + &AutomotiveSimulator::GetMutableContext, py::return_value_policy::reference_internal) - .def("get_collisions", &AutomotiveSimulator::GetCollisions) + .def("get_collisions", py::guard_internal_references( + &AutomotiveSimulator::GetCollisions)) .def("start", &AutomotiveSimulator::Start) .def("set_road_geometry", &AutomotiveSimulator::SetRoadGeometry, "Transfer a road geometry to the control of the simulator", diff --git a/src/agents/mobil_car.cc b/src/agents/mobil_car.cc index 7b4ed9dce..aea1a882d 100644 --- a/src/agents/mobil_car.cc +++ b/src/agents/mobil_car.cc @@ -53,7 +53,7 @@ MobilCar::MobilCar(const std::string& name, bool direction_of_travel, double x, drake::Vector3::UnitZ()); } -std::unique_ptr MobilCar::BuildDiagram() const { +std::unique_ptr MobilCar::BuildDiagram() const { DiagramBuilder builder(name_); /****************************************** diff --git a/src/agents/mobil_car.h b/src/agents/mobil_car.h index fa0691c20..3b3bbea53 100644 --- a/src/agents/mobil_car.h +++ b/src/agents/mobil_car.h @@ -54,8 +54,6 @@ class MobilCar : public delphyne::Agent { double y, double heading, double speed, const drake::maliput::api::RoadGeometry& road_geometry); - std::unique_ptr BuildDiagram() const; - private: struct Parameters { bool direction_of_travel{true}; @@ -73,6 +71,8 @@ class MobilCar : public delphyne::Agent { speed(speed) {} } initial_parameters_; + std::unique_ptr BuildDiagram() const override; + const drake::maliput::api::RoadGeometry& road_geometry_; }; diff --git a/src/agents/rail_car.cc b/src/agents/rail_car.cc index 61edbd5a3..505ce66ae 100644 --- a/src/agents/rail_car.cc +++ b/src/agents/rail_car.cc @@ -93,7 +93,7 @@ RailCar::RailCar(const std::string& name, const drake::maliput::api::Lane& lane, "RoadGeometry."); } -std::unique_ptr RailCar::BuildDiagram() const { +std::unique_ptr RailCar::BuildDiagram() const { DiagramBuilder builder(name_); /****************************************** diff --git a/src/agents/rail_car.h b/src/agents/rail_car.h index 10105f457..355c469ec 100644 --- a/src/agents/rail_car.h +++ b/src/agents/rail_car.h @@ -39,11 +39,10 @@ namespace delphyne { /// (how far along the track) and the agent will follow /// this track exactly - the only variance it is permitted is the speed /// with which it follows the track. -class RailCar : public delphyne::Agent { +class RailCar : public Agent { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RailCar) - /// /// @brief Default constructor /// /// @param name[in] Unique name for the agent. @@ -63,8 +62,6 @@ class RailCar : public delphyne::Agent { double speed, double nominal_speed, const drake::maliput::api::RoadGeometry& road_geometry); - std::unique_ptr BuildDiagram() const; - private: // Container for the agent's initial configuration. // @@ -90,6 +87,8 @@ class RailCar : public delphyne::Agent { nominal_speed(nominal_speed) {} } initial_parameters_; + std::unique_ptr BuildDiagram() const override; + const drake::maliput::api::RoadGeometry& road_geometry_; }; diff --git a/src/agents/simple_car.cc b/src/agents/simple_car.cc index a09488201..7cc375977 100644 --- a/src/agents/simple_car.cc +++ b/src/agents/simple_car.cc @@ -40,7 +40,7 @@ SimpleCar::SimpleCar(const std::string& name, double x, double y, drake::Vector3::UnitZ())); } -std::unique_ptr SimpleCar::BuildDiagram() const { +std::unique_ptr SimpleCar::BuildDiagram() const { DiagramBuilder builder(name_); /********************* diff --git a/src/agents/simple_car.h b/src/agents/simple_car.h index 7a94c76fa..ad9a9a190 100644 --- a/src/agents/simple_car.h +++ b/src/agents/simple_car.h @@ -28,13 +28,11 @@ namespace delphyne { *****************************************************************************/ /// @brief A very simple vehicle agent that can be teleoperated. -class SimpleCar : public delphyne::Agent { +class SimpleCar : public Agent { public: SimpleCar(const std::string& name, double x, double y, double heading, double speed); - std::unique_ptr BuildDiagram() const; - private: // Container for the agent's initial configuration. // @@ -49,6 +47,8 @@ class SimpleCar : public delphyne::Agent { Parameters(double x, double y, double heading, double speed) : x(x), y(y), heading(heading), speed(speed) {} } initial_parameters_; + + std::unique_ptr BuildDiagram() const override; }; /***************************************************************************** diff --git a/src/agents/trajectory_agent.cc b/src/agents/trajectory_agent.cc index 8a05f7bba..77a00bf5c 100644 --- a/src/agents/trajectory_agent.cc +++ b/src/agents/trajectory_agent.cc @@ -68,7 +68,7 @@ TrajectoryAgent::TrajectoryAgent( initial_car_pose_velocity.rotation(); } -std::unique_ptr TrajectoryAgent::BuildDiagram() const { +std::unique_ptr TrajectoryAgent::BuildDiagram() const { DiagramBuilder builder(name_); /****************************************** diff --git a/src/agents/trajectory_agent.h b/src/agents/trajectory_agent.h index fb9a96f6f..fae96b5e6 100644 --- a/src/agents/trajectory_agent.h +++ b/src/agents/trajectory_agent.h @@ -28,15 +28,15 @@ namespace delphyne { *****************************************************************************/ /// @brief Trajectory following agents -class TrajectoryAgent : public delphyne::Agent { +class TrajectoryAgent : public Agent { public: TrajectoryAgent(const std::string& name, const std::vector& times, const std::vector& headings, const std::vector>& translations); - std::unique_ptr BuildDiagram() const; - private: + std::unique_ptr BuildDiagram() const override; + const double initial_time_{}; std::unique_ptr trajectory_{}; }; diff --git a/src/backend/automotive_simulator.cc b/src/backend/automotive_simulator.cc index 6ea8617cd..9be52970b 100644 --- a/src/backend/automotive_simulator.cc +++ b/src/backend/automotive_simulator.cc @@ -42,6 +42,9 @@ using drake::systems::rendering::PoseBundle; using drake::systems::RungeKutta2Integrator; using drake::systems::SystemOutput; +template +constexpr double AutomotiveSimulator::kSceneTreePublishRateHz; + template AutomotiveSimulator::AutomotiveSimulator() { // Avoid the many & varied 'info' level logging messages coming from drake. @@ -113,34 +116,34 @@ std::unique_ptr AutomotiveSimulator::GetScene() { // into a shared method. template -int AutomotiveSimulator::AddAgent(std::unique_ptr> agent) { +AgentBase* AutomotiveSimulator::AddAgent( + std::unique_ptr> agent) { /********************* * Checks *********************/ DELPHYNE_VALIDATE(!has_started(), std::runtime_error, "Cannot add an agent to a running simulation"); - CheckNameUniqueness(agent->name()); - /********************* - * Unique ID - *********************/ - int id = unique_system_id_++; + const std::string name = agent->name(); + DELPHYNE_VALIDATE(agents_.count(name) == 0, std::runtime_error, + "An agent named \"" + name + "\" already exists. " + "It has id " + std::to_string(agents_[name]->id()) + "."); /********************* * Agent Diagram *********************/ - std::unique_ptr> bundle = agent->BuildDiagram(); - drake::systems::Diagram* diagram = - builder_->AddSystem(std::move(bundle->diagram)); + const int id = unique_system_id_++; + + AgentDiagram* agent_diagram = agent->TakePartIn(builder_.get(), id); drake::systems::rendering::PoseVelocityInputPortDescriptors ports = - aggregator_->AddSinglePoseAndVelocityInput(agent->name(), id); - builder_->Connect(diagram->get_output_port(bundle->outputs["pose"]), + aggregator_->AddSinglePoseAndVelocityInput(name, id); + builder_->Connect(agent_diagram->get_output_port("pose"), ports.pose_descriptor); - builder_->Connect(diagram->get_output_port(bundle->outputs["velocity"]), + builder_->Connect(agent_diagram->get_output_port("velocity"), ports.velocity_descriptor); builder_->Connect(aggregator_->get_output_port(0), - diagram->get_input_port(bundle->inputs["traffic_poses"])); + agent_diagram->get_input_port("traffic_poses")); /********************* * Agent Visuals @@ -149,21 +152,21 @@ int AutomotiveSimulator::AddAgent(std::unique_ptr> agent) { // We'll need a means of having the agents report what visual they have and // hooking that up. Also wondering why visuals are in the drake diagram? car_vis_applicator_->AddCarVis( - std::make_unique>(id, agent->name())); + std::make_unique>(id, name)); /********************* * Agent Geometry *********************/ // Wires up the Prius geometry. builder_->Connect( - diagram->get_output_port(bundle->outputs["pose"]), - WirePriusGeometry(agent->name(), agent->initial_world_pose(), + agent_diagram->get_output_port("pose"), + WirePriusGeometry(name, agent->initial_world_pose(), builder_.get(), scene_graph_, &(agent->mutable_geometry_ids()))); // save a handle to it - agents_[id] = std::move(agent); - return id; + agents_[name] = std::move(agent); + return agents_[name].get(); } template @@ -178,18 +181,41 @@ const RoadGeometry* AutomotiveSimulator::SetRoadGeometry( template const delphyne::AgentBase& -AutomotiveSimulator::GetAgentById(int agent_id) const { - DELPHYNE_VALIDATE(agents_.count(agent_id) != 0, std::runtime_error, - "No agent found with the given ID."); - return *agents_.at(agent_id); +AutomotiveSimulator::GetAgentByName(const std::string& name) const { + DELPHYNE_VALIDATE(agents_.count(name) != 0, std::runtime_error, + "No agent found with the given name."); + return *agents_.at(name); } template -delphyne::AgentBase* AutomotiveSimulator::GetMutableAgentById( - int agent_id) { - DELPHYNE_VALIDATE(agents_.count(agent_id) != 0, std::runtime_error, - "No agent found with the given ID."); - return agents_[agent_id].get(); +delphyne::AgentBase* +AutomotiveSimulator::GetMutableAgentByName(const std::string& name) { + DELPHYNE_VALIDATE(agents_.count(name) != 0, std::runtime_error, + "No agent found with the given name."); + return agents_[name].get(); +} + +template +const drake::systems::Context& +AutomotiveSimulator::GetContext(const delphyne::AgentBase& agent) const { + DELPHYNE_VALIDATE(agents_.count(agent.name()) != 0, std::runtime_error, + "Given agent is not known to this simulator."); + DELPHYNE_VALIDATE(has_started(), std::runtime_error, + "Can only get context of a running simulation."); + return diagram_->GetSubsystemContext( + agent.get_diagram(), simulator_->get_context()); +} + +template +drake::systems::Context* +AutomotiveSimulator::GetMutableContext( + const delphyne::AgentBase& agent) { + DELPHYNE_VALIDATE(agents_.count(agent.name()) != 0, std::runtime_error, + "Given agent is not known to this simulator."); + DELPHYNE_VALIDATE(has_started(), std::runtime_error, + "Can only get context of a running simulation."); + return &diagram_->GetMutableSubsystemContext( + agent.get_diagram(), &simulator_->get_mutable_context()); } template @@ -225,8 +251,9 @@ struct IsSourceOf { // Checks whether the given (agent ID, agent) pair is the source of the // associated `object`. bool operator()( - const std::pair>>& id_agent) { - return id_agent.second->is_source_of(object); + const std::pair>>& name_agent) { + return name_agent.second->is_source_of(object); } // Associated object to check the source of. @@ -236,8 +263,8 @@ struct IsSourceOf { } // namespace template -const std::vector> AutomotiveSimulator::GetCollisions() - const { +const std::vector> + AutomotiveSimulator::GetCollisions() const { DELPHYNE_VALIDATE(has_started(), std::runtime_error, "Can only get collisions on a running simulation"); using drake::geometry::GeometryId; @@ -245,7 +272,7 @@ const std::vector> AutomotiveSimulator::GetCollisions() using drake::geometry::PenetrationAsPointPair; const std::vector> collisions = scene_query_->GetValue>().ComputePointPairPenetration(); - std::vector> agents_colliding; + std::vector> agents_in_collision; for (const auto& collision : collisions) { const auto it_A = std::find_if(agents_.begin(), agents_.end(), IsSourceOf(collision.id_A)); @@ -255,9 +282,9 @@ const std::vector> AutomotiveSimulator::GetCollisions() IsSourceOf(collision.id_B)); DELPHYNE_VALIDATE(it_B != agents_.end(), std::runtime_error, "Could not find second agent in list of agents"); - agents_colliding.emplace_back(it_A->first, it_B->first); + agents_in_collision.emplace_back(it_A->second.get(), it_B->second.get()); } - return agents_colliding; + return agents_in_collision; } template @@ -415,16 +442,6 @@ double AutomotiveSimulator::get_current_simulation_time() const { return drake::ExtractDoubleOrThrow(simulator_->get_context().get_time()); } -template -void AutomotiveSimulator::CheckNameUniqueness(const std::string& name) { - for (const auto& agent : agents_) { - DELPHYNE_VALIDATE(agent.second->name() != name, std::runtime_error, - "An agent named \"" + name + - "\" already exists. It has id " + - std::to_string(agent.first) + "."); - } -} - template PoseBundle AutomotiveSimulator::GetCurrentPoses() const { DELPHYNE_VALIDATE( diff --git a/src/backend/automotive_simulator.h b/src/backend/automotive_simulator.h index 015c31a08..d996364a8 100644 --- a/src/backend/automotive_simulator.h +++ b/src/backend/automotive_simulator.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,9 @@ namespace delphyne { +template +using AgentCollisionPair = std::pair*, AgentBase*>; + /// AutomotiveSimulator is a helper class for constructing and running /// automotive-related simulations. /// @@ -79,7 +83,7 @@ class AutomotiveSimulator { * @param[in] agent The user provided agent to add to the simulation. * @return A simulator generated unique id for the agent. */ - int AddAgent(std::unique_ptr> agent); + AgentBase* AddAgent(std::unique_ptr> agent); /// Returns an immutable reference to the agent with the /// given @p agent_id. @@ -88,7 +92,7 @@ class AutomotiveSimulator { /// by AddAgent(). /// @throw std::runtime_error if no agent with given ID /// is known to to the simulator. - const delphyne::AgentBase& GetAgentById(int agent_id) const; + const AgentBase& GetAgentByName(const std::string& name) const; /// Returns an mutable reference to the agent with the /// given @p agent_id. @@ -97,7 +101,11 @@ class AutomotiveSimulator { /// by AddAgent(). /// @throw std::runtime_error if no agent with given ID /// is known to to the simulator. - delphyne::AgentBase* GetMutableAgentById(int agent_id); + AgentBase* GetMutableAgentByName(const std::string& name); + + const drake::systems::Context& GetContext(const AgentBase& agent) const; + + drake::systems::Context* GetMutableContext(const AgentBase& agent); /// Sets the RoadGeometry for this simulation. /// @@ -129,7 +137,7 @@ class AutomotiveSimulator { /// order. /// @pre Start() has been called. /// @throw std::runtime_error if any of the preconditions is not met. - const std::vector> GetCollisions() const; + const std::vector> GetCollisions() const; /// Calls Build() on the diagram (if it has not been build already) and /// initializes the Simulator. No further changes to the diagram may occur @@ -156,12 +164,6 @@ class AutomotiveSimulator { // update the scene tree widget tree. static constexpr double kSceneTreePublishRateHz{4.0}; - // Verifies that the provided `name` of an agent is unique among all agents - // that have been added to the `AutomotiveSimulator`. Throws a - // std::runtime_error if it is not unique meaning an agent of the same name - // was already added. - void CheckNameUniqueness(const std::string& name); - // Generates the URDF model of the road network and loads it into the // `RigidBodyTree`. Member variable `road_` must be set prior to calling this // method. @@ -225,7 +227,7 @@ class AutomotiveSimulator { int unique_system_id_{0}; // Maps from simulator generated unique id's to the agents. - std::map>> agents_; + std::map>> agents_; // For simulation. std::unique_ptr> diagram_{}; diff --git a/src/backend/simulation_runner.cc b/src/backend/simulation_runner.cc index 8efd8d03c..821bfafe6 100644 --- a/src/backend/simulation_runner.cc +++ b/src/backend/simulation_runner.cc @@ -415,8 +415,8 @@ void SimulatorRunner::RunInteractiveSimulationLoopStep() { // is not paused. if (collisions_enabled_ && !paused_) { // Computes collisions between agents. - const std::vector> agents_in_collision = - simulator_->GetCollisions(); + const std::vector> + agents_in_collision = simulator_->GetCollisions(); if (!agents_in_collision.empty()) { // Pauses simulation if necessary. PauseSimulation(); diff --git a/src/backend/simulation_runner.h b/src/backend/simulation_runner.h index d321865ad..6369c7f55 100644 --- a/src/backend/simulation_runner.h +++ b/src/backend/simulation_runner.h @@ -113,7 +113,7 @@ class SimulatorRunner { // @brief On agent collision callback function type. // @see AutomotiveSimulator::GetCollisions() using CollisionCallback = - std::function>&)>; + std::function>&)>; /// @brief Default constructor. /// diff --git a/test/regression/cpp/automotive_simulator_test.cc b/test/regression/cpp/automotive_simulator_test.cc index 6722296ec..45ac00d83 100644 --- a/test/regression/cpp/automotive_simulator_test.cc +++ b/test/regression/cpp/automotive_simulator_test.cc @@ -172,17 +172,16 @@ TEST_F(AutomotiveSimulatorTest, BasicTest) { auto agent_bob = std::make_unique("bob", 0.0, 0.0, 0.0, 0.0); - const int agent_bob_id = simulator->AddAgent(std::move(agent_bob)); - EXPECT_EQ(simulator->GetAgentById(agent_bob_id).name(), "bob"); + simulator->AddAgent(std::move(agent_bob)); + EXPECT_EQ(simulator->GetAgentByName("bob").name(), "bob"); auto agent_alice = std::make_unique("alice", 0.0, 0.0, 0.0, 0.0); - const int agent_alice_id = simulator->AddAgent(std::move(agent_alice)); - EXPECT_EQ(simulator->GetAgentById(agent_alice_id).name(), "alice"); + simulator->AddAgent(std::move(agent_alice)); + EXPECT_EQ(simulator->GetAgentByName("alice").name(), "alice"); // Verifies that passing an unknown agent ID is an error. - const int agent_x_id = -1; - EXPECT_THROW(simulator->GetAgentById(agent_x_id), std::runtime_error); + EXPECT_THROW(simulator->GetAgentByName("agent_x"), std::runtime_error); } // Covers simple-car, Start and StepBy @@ -192,9 +191,7 @@ TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCar) { simulator->SetRoadGeometry(CreateDragway("TestDragway", 1)); auto agent = std::make_unique("bob", 0.0, 0.0, 0.0, 0.0); - const int id = simulator->AddAgent(std::move(agent)); - - EXPECT_EQ(id, 0); + simulator->AddAgent(std::move(agent)); // Finish all initialization, so that we can test the post-init state. simulator->Start(kRealtimeFactor); @@ -250,8 +247,7 @@ TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCarInitialState) { auto agent = std::make_unique("bob", kX, kY, kHeading, kVelocity); - const int id = simulator->AddAgent(std::move(agent)); - EXPECT_EQ(id, 0); + simulator->AddAgent(std::move(agent)); // Set up a monitor to check for ignition::msgs::SimpleCarState // messages coming from the agent. @@ -299,37 +295,35 @@ TEST_F(AutomotiveSimulatorTest, TestMobilControlledSimpleCar) { // +----> +s, +x | MOBIL Car | | Decoy 1 | // --------------------------------------------------------------- - auto mobil = std::make_unique("MOBIL0", - true, // lane_direction, - 2.0, // x - -2.0, // y - 0.0, // heading - 10.0, // velocity - *road_geometry); - const int id_mobil = simulator->AddAgent(std::move(mobil)); - EXPECT_EQ(0, id_mobil); - - auto decoy_1 = std::make_unique( - "decoy1", *(road_geometry->junction(0)->segment(0)->lane(0)), - true, // lane_direction, - 6.0, // position (m) - 0.0, // offset (m) - 0.0, // speed (m) - 0.0, // nominal_speed (m/s) - *road_geometry); - const int id_decoy1 = simulator->AddAgent(std::move(decoy_1)); - EXPECT_EQ(1, id_decoy1); + auto mobil = simulator->AddAgent( + std::make_unique( + "MOBIL0", + true, // lane_direction, + 2.0, // x + -2.0, // y + 0.0, // heading + 10.0, // velocity + *road_geometry)); + + simulator->AddAgent( + std::make_unique( + "decoy1", *(road_geometry->junction(0)->segment(0)->lane(0)), + true, // lane_direction, + 6.0, // position (m) + 0.0, // offset (m) + 0.0, // speed (m) + 0.0, // nominal_speed (m/s) + *road_geometry)); - auto decoy_2 = std::make_unique( - "decoy2", *(road_geometry->junction(0)->segment(0)->lane(0)), - true, // lane_direction, - 20.0, // position (m) - 0.0, // offset (m) - 0.0, // speed (m/s) - 0.0, // nominal_speed (m/s) - *road_geometry); - const int id_decoy2 = simulator->AddAgent(std::move(decoy_2)); - EXPECT_EQ(2, id_decoy2); + simulator->AddAgent( + std::make_unique( + "decoy2", *(road_geometry->junction(0)->segment(0)->lane(0)), + true, // lane_direction, + 20.0, // position (m) + 0.0, // offset (m) + 0.0, // speed (m/s) + 0.0, // nominal_speed (m/s) + *road_geometry)); // Setup an ignition transport topic monitor to listen to // ignition::msgs::Model_V messages being published to @@ -353,7 +347,7 @@ TEST_F(AutomotiveSimulatorTest, TestMobilControlledSimpleCar) { // Expect the SimpleCar to start steering to the left; y value increases. const double mobil_y = - draw_message.models(id_mobil).link(0).pose().position().y(); + draw_message.models(mobil->id()).link(0).pose().position().y(); EXPECT_GE(mobil_y, -2.); } @@ -381,13 +375,9 @@ TEST_F(AutomotiveSimulatorTest, TestTrajectoryAgent) { {0.0, 0.0, 0.0}, {10.0, 0.0, 0.0}, {30.0, 0.0, 0.0}, {60.0, 0.0, 0.0}, {100.0, 0.0, 0.0}, }; - std::unique_ptr alice = - std::make_unique("alice", times, headings, - translations); - - const int id = simulator->AddAgent(std::move(alice)); - EXPECT_EQ(0, id); + auto alice = simulator->AddAgent(std::make_unique( + "alice", times, headings, translations)); // Setup an ignition transport topic monitor to listen to // ignition::msgs::Model_V messages being published to @@ -409,10 +399,7 @@ TEST_F(AutomotiveSimulatorTest, TestTrajectoryAgent) { EXPECT_EQ(GetLinkCount(draw_message), GetPriusLinkCount()); - auto alice_model = draw_message.models(id); - - // Checks the car ids - EXPECT_EQ(alice_model.id(), id); + auto alice_model = draw_message.models(alice->id()); auto link = alice_model.link(0); @@ -484,9 +471,7 @@ TEST_F(AutomotiveSimulatorTest, TestMaliputRailcar) { 0.0, // speed 0.0, // nominal_speed *road_geometry); - const int id = simulator->AddAgent(std::move(agent)); - - EXPECT_GE(id, 0); + simulator->AddAgent(std::move(agent)); // Setup an ignition transport topic monitor to listen to // ignition::msgs::Model_V messages being published to @@ -647,8 +632,8 @@ TEST_F(AutomotiveSimulatorTest, TestRailcarVelocityOutput) { 0.0, // nominal_speed *road_geometry); - int alice_id = simulator->AddAgent(std::move(alice)); - int bob_id = simulator->AddAgent(std::move(bob)); + simulator->AddAgent(std::move(alice)); + simulator->AddAgent(std::move(bob)); EXPECT_NO_THROW(simulator->Start()); @@ -664,8 +649,8 @@ TEST_F(AutomotiveSimulatorTest, TestRailcarVelocityOutput) { const drake::systems::rendering::PoseBundle poses = simulator->GetCurrentPoses(); ASSERT_EQ(poses.get_num_poses(), 2); - ASSERT_EQ(poses.get_model_instance_id(kAliceIndex), alice_id); - ASSERT_EQ(poses.get_model_instance_id(kBobIndex), bob_id); + ASSERT_EQ(poses.get_name(kAliceIndex), "alice"); + ASSERT_EQ(poses.get_name(kBobIndex), "bob"); EXPECT_FALSE(poses.get_velocity(kAliceIndex).get_value().isZero()); EXPECT_TRUE(poses.get_velocity(kBobIndex).get_value().isZero()); } @@ -768,28 +753,28 @@ TEST_F(AutomotiveSimulatorTest, TestGetCollisions) { second_lane->length(), kZeroROffset, kZeroHOffset}; const drake::maliput::api::GeoPosition agent_alice_geo_position = second_lane->ToGeoPosition(agent_alice_lane_position); - auto agent_alice = std::make_unique( - "alice", agent_alice_geo_position.x(), agent_alice_geo_position.y(), - kHeadingWest, kCruiseSpeed); - const int agent_alice_id = simulator->AddAgent(std::move(agent_alice)); + auto agent_alice = + simulator->AddAgent(std::make_unique( + "alice", agent_alice_geo_position.x(), agent_alice_geo_position.y(), + kHeadingWest, kCruiseSpeed)); // Configures agent `Smith`. const drake::maliput::api::LanePosition agent_smith_lane_position{ kZeroSOffset, kZeroROffset, kZeroHOffset}; const drake::maliput::api::GeoPosition agent_smith_geo_position = first_lane->ToGeoPosition(agent_smith_lane_position); - auto agent_smith = std::make_unique( - "smith", agent_smith_geo_position.x(), agent_smith_geo_position.y(), - kHeadingEast + kHeadingDeviation, kCruiseSpeed); - const int agent_smith_id = simulator->AddAgent(std::move(agent_smith)); + auto agent_smith = + simulator->AddAgent(std::make_unique( + "smith", agent_smith_geo_position.x(), agent_smith_geo_position.y(), + kHeadingEast + kHeadingDeviation, kCruiseSpeed)); // Finishes initialization and starts the simulation. simulator->Start(); // Verifies that no agent is in collision at the beginning // of the simulation. - std::vector> agent_pairs_in_collision = - simulator->GetCollisions(); + std::vector> + agent_pairs_in_collision = simulator->GetCollisions(); EXPECT_EQ(agent_pairs_in_collision.size(), 0); // Simulates forward in time. @@ -800,14 +785,14 @@ TEST_F(AutomotiveSimulatorTest, TestGetCollisions) { // agents are the expected ones. agent_pairs_in_collision = simulator->GetCollisions(); EXPECT_EQ(agent_pairs_in_collision.size(), 1); - const int first_colliding_agent_id = agent_pairs_in_collision[0].first; - const int second_colliding_agent_id = agent_pairs_in_collision[0].second; + const Agent* first_colliding_agent = agent_pairs_in_collision[0].first; + const Agent* second_colliding_agent = agent_pairs_in_collision[0].second; // Cannot make any assumption regarding pair order, see // delphyne::AutomotiveSimulator::GetCollisions(). - EXPECT_TRUE((first_colliding_agent_id == agent_alice_id && - second_colliding_agent_id == agent_smith_id) || - (first_colliding_agent_id == agent_smith_id && - second_colliding_agent_id == agent_alice_id)); + EXPECT_TRUE((first_colliding_agent == agent_alice && + second_colliding_agent == agent_smith) || + (first_colliding_agent == agent_smith && + second_colliding_agent == agent_alice)); } //////////////////////////////////////////////////