Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[PROPOSAL] Basic Agent API #508

Closed
wants to merge 1 commit into from
Closed

[PROPOSAL] Basic Agent API #508

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion include/delphyne/mi6/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Expand Down
83 changes: 78 additions & 5 deletions include/delphyne/mi6/agent_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,19 @@
#include <string>

#include <drake/common/eigen_types.h>
#include <drake/systems/framework/context.h>
#include <drake/systems/framework/output_port.h>
#include <drake/systems/framework/diagram_builder.h>
#include <drake/systems/framework/value.h>
#include <drake/systems/framework/vector_base.h>
#include <drake/systems/framework/basic_vector.h>
#include <drake/systems/rendering/pose_vector.h>
#include <drake/systems/rendering/frame_velocity.h>
#include <drake/geometry/geometry_ids.h>

#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"

/*****************************************************************************
Expand Down Expand Up @@ -44,9 +53,9 @@ namespace delphyne {
template <typename T>
class AgentBase {
public:
/// Type for packaging diagram and indices together in a bundle.
using DiagramBundle = delphyne::DiagramBundle<T>;
/// Specific builder for this agent. Use inside BuildDiagram().
/// Specific diagram for this agent. As returned by BuildDiagram().
using Diagram = AgentDiagram<T>;
/// Specific builder for this agent. To be used inside BuildDiagram().
using DiagramBuilder = AgentDiagramBuilder<T>;

/// @brief Constructor initialising common agent parameters.
Expand All @@ -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<DiagramBundle> BuildDiagram() const = 0;
Diagram* TakePartIn(drake::systems::DiagramBuilder<T>* 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<T>
GetPose(const drake::systems::Context<T>& context) const {
DELPHYNE_VALIDATE(diagram_ != nullptr, std::runtime_error,
"This agent is not associated to any simulation!");
const drake::systems::OutputPort<T>& pose_output_port =
diagram_->get_output_port("pose");
std::unique_ptr<drake::systems::AbstractValue> 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<T>& base_vector =
port_value->template GetValueOrThrow<BasicVector<T>>();
const PoseVector<T>& pose_vector =
dynamic_cast<const PoseVector<T>&>(base_vector);
return pose_vector.get_isometry();
}

drake::TwistVector<T>
GetVelocity(const drake::systems::Context<T>& context) const {
DELPHYNE_VALIDATE(diagram_ != nullptr, std::runtime_error,
"This agent is not associated to any simulation!");
const drake::systems::OutputPort<T>& vel_output_port =
diagram_->get_output_port("velocity");
std::unique_ptr<drake::systems::AbstractValue> 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<T>& base_vector =
port_value->template GetValueOrThrow<BasicVector<T>>();
const FrameVelocity<T>& frame_velocity =
dynamic_cast<const FrameVelocity<T>&>(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_; }
Expand Down Expand Up @@ -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<double> initial_world_pose_;

private:
virtual std::unique_ptr<Diagram> BuildDiagram() const = 0;

Diagram* diagram_{nullptr};

// TODO(daniel.stonier) dubious whether we should have
// simulator specific machinery here
std::set<drake::geometry::GeometryId> geometry_ids_{};
Expand Down
119 changes: 119 additions & 0 deletions include/delphyne/mi6/agent_diagram.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,119 @@
// Copyright 2018 Toyota Research Institute

#pragma once

/*****************************************************************************
** Includes
*****************************************************************************/

#include <map>
#include <memory>
#include <string>
#include <type_traits>
#include <utility>

#include <drake/common/drake_copyable.h>
#include <drake/systems/framework/diagram.h>
#include <drake/systems/framework/diagram_builder.h>
#include <drake/systems/framework/framework_common.h> // OutputPortIndex
#include <drake/systems/framework/input_port_descriptor.h>
#include <drake/systems/framework/output_port.h>

#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 <typename Base, typename T>
class NamedPortSystem : public Base {
public:
static_assert(std::is_base_of<drake::systems::System<T>, Base>::value,
"Only a System can have their ports mapped.");

DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(NamedPortSystem);

template <typename... Args>
explicit NamedPortSystem(Args... args) :
Base(std::forward<Args>(args)...) { }

void set_input_names(
std::map<std::string, drake::systems::InputPortIndex> inputs_mapping) {
inputs_mapping_ = inputs_mapping;
}

void set_output_names(
std::map<std::string, drake::systems::OutputPortIndex> outputs_mapping) {
outputs_mapping_ = outputs_mapping;
}

const drake::systems::InputPortDescriptor<T>&
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<T>&
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<T>&
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<T>&
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<std::string, drake::systems::InputPortIndex> inputs_mapping_{};
std::map<std::string, drake::systems::OutputPortIndex> outputs_mapping_{};
};

} // namespace detail

template <typename T>
class AgentDiagram :
public detail::NamedPortSystem<drake::systems::Diagram<T>, T> {
public:
explicit AgentDiagram(drake::systems::DiagramBuilder<T>* builder) {
builder->BuildInto(this);
}
};

/*****************************************************************************
** Trailers
*****************************************************************************/

} // namespace delphyne
45 changes: 23 additions & 22 deletions include/delphyne/mi6/agent_diagram_builder.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>

#include <drake/systems/framework/diagram_builder.h>
#include <drake/systems/framework/framework_common.h> // OutputPortIndex
Expand All @@ -19,7 +20,7 @@
#include <drake/systems/rendering/pose_bundle.h>

#include "delphyne/macros.h"
#include "delphyne/mi6/diagram_bundle.h"
#include "delphyne/mi6/agent_diagram.h"
#include "delphyne/types.h"

/*****************************************************************************
Expand Down Expand Up @@ -53,7 +54,7 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder<T> {
this->AddSystem(std::make_unique<drake::systems::PassThrough<double>>(
drake::systems::Value<
drake::systems::rendering::PoseBundle<double>>(0)));
inputs_["traffic_poses"] =
inputs_mapping_["traffic_poses"] =
this->ExportInput(traffic_poses_->get_input_port());
}

Expand All @@ -67,10 +68,10 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder<T> {
/// than once before building.
void ExportStateOutput(const drake::systems::OutputPort<T>& 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.
Expand All @@ -83,10 +84,10 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder<T> {
/// than once before building.
void ExportPoseOutput(const drake::systems::OutputPort<T>& 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.
Expand All @@ -99,10 +100,10 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder<T> {
/// than once before building.
void ExportVelocityOutput(const drake::systems::OutputPort<T>& 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.
Expand All @@ -117,32 +118,32 @@ class AgentDiagramBuilder : public drake::systems::DiagramBuilder<T> {
}

/// @brief Build the diagram bundle and append the indices.
std::unique_ptr<DiagramBundle<T>> Build() {
std::unique_ptr<AgentDiagram<T>> 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<DiagramBundle<T>>();
bundle->diagram = drake::systems::DiagramBuilder<T>::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<AgentDiagram<T>>(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<double>* traffic_poses_;
std::map<std::string, drake::systems::OutputPortIndex> outputs_;
std::map<std::string, drake::systems::InputPortIndex> inputs_;
std::map<std::string, drake::systems::InputPortIndex> inputs_mapping_{};
std::map<std::string, drake::systems::OutputPortIndex> outputs_mapping_{};
};

/*****************************************************************************
Expand Down
Loading