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

Jump to bottom

Class-based loadable agent params #410

Merged
merged 1 commit into from
May 23, 2018
Merged

Class-based loadable agent params #410

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
4 changes: 2 additions & 2 deletions backend/agent_plugin_loader.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@
#include <ignition/common/PluginLoader.hh>
#include <ignition/common/SystemPaths.hh>

#include "../include/delphyne/agent_plugin_base.h"
#include "../include/delphyne/types.h"
#include "include/delphyne/agent_plugin_base.h"
#include "include/delphyne/types.h"

namespace delphyne {
namespace {
Expand Down
2 changes: 1 addition & 1 deletion backend/agent_plugin_loader.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
#include <memory>
#include <string>

#include "../include/delphyne/agent_plugin_base.h"
#include "include/delphyne/agent_plugin_base.h"

namespace delphyne {

Expand Down
37 changes: 24 additions & 13 deletions backend/automotive_simulator.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,6 @@
#include "backend/translation_systems/lcm_viewer_draw_to_ign_model_v.h"
#include "backend/translation_systems/lcm_viewer_load_robot_to_ign_model_v.h"

#include "../include/delphyne/linb-any"

namespace delphyne {

using drake::automotive::SimpleCarStateIndices;
Expand All @@ -55,7 +53,6 @@ using drake::systems::RungeKutta2Integrator;
using drake::systems::System;
using drake::systems::SystemOutput;


template <typename T>
AutomotiveSimulator<T>::AutomotiveSimulator() {
aggregator_ =
Expand Down Expand Up @@ -124,24 +121,38 @@ void AutomotiveSimulator<T>::ConnectCarOutputsAndPriusVis(

template <typename T>
int AutomotiveSimulator<T>::AddLoadableAgent(
const std::string& plugin_library_name,
const std::map<std::string, linb::any>& parameters, const std::string& name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state) {
return AddLoadableAgent(plugin_library_name, "", parameters, name,
std::move(initial_state));
const std::string& plugin_library_name, const std::string& agent_name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state,
const drake::maliput::api::RoadGeometry* road) {
return AddLoadableAgent(plugin_library_name, "", agent_name,
std::move(initial_state), road,
std::make_unique<AgentPluginParams>());
}

template <typename T>
int AutomotiveSimulator<T>::AddLoadableAgent(
const std::string& plugin_library_name, const std::string& agent_name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state,
const drake::maliput::api::RoadGeometry* road,
std::unique_ptr<AgentPluginParams> parameters) {
return AddLoadableAgent(plugin_library_name, "", agent_name,
std::move(initial_state), road,
std::move(parameters));
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@basicNew how do you feel about dropping this method and coercing the user to always specify both plugin_library_name and plugin_name?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is actually quite handy to have this; as a matter of fact, we are not using the "full" version of it anywhere :). I would vote for leaving it.

}

template <typename T>
int AutomotiveSimulator<T>::AddLoadableAgent(
const std::string& plugin_library_name, const std::string& plugin_name,
const std::map<std::string, linb::any>& parameters, const std::string& name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state) {
const std::string& agent_name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state,
const drake::maliput::api::RoadGeometry* road,
std::unique_ptr<AgentPluginParams> parameters) {
/*********************
* Checks
*********************/
DELPHYNE_DEMAND(!has_started());
DELPHYNE_DEMAND(aggregator_ != nullptr);
CheckNameUniqueness(name);
CheckNameUniqueness(agent_name);

/*********************
* Load Agent Plugin
Expand All @@ -161,8 +172,8 @@ int AutomotiveSimulator<T>::AddLoadableAgent(
*********************/
int id = unique_system_id_++;

if (agent->Configure(name, id, parameters, builder_.get(), aggregator_,
car_vis_applicator_) < 0) {
if (agent->Configure(agent_name, id, builder_.get(), aggregator_,
car_vis_applicator_, road, std::move(parameters)) < 0) {
return -1;
}
agents_[id] = std::move(agent);
Expand Down
27 changes: 15 additions & 12 deletions backend/automotive_simulator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,7 @@
#include "backend/scene_system.h"
#include "backend/system.h"

#include "../include/delphyne/agent_plugin_base.h"
#include "../include/delphyne/linb-any"
#include "include/delphyne/agent_plugin_base.h"

namespace delphyne {

Expand All @@ -55,8 +54,6 @@ namespace delphyne {
template <typename T>
class AutomotiveSimulator {
public:
/// A constructor that configures this object to use DrakeLcm, which
/// encapsulates a _real_ LCM instance.
AutomotiveSimulator();

/// Returns the DiagramBuilder.
Expand Down Expand Up @@ -84,18 +81,25 @@ class AutomotiveSimulator {
/// @return The ID of the agent that was just added to the simulation, or -1
/// on error.
int AddLoadableAgent(
const std::string& plugin_library_name,
const std::map<std::string, linb::any>& parameters,
const std::string& name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state);
const std::string& plugin_library_name, const std::string& agent_name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state,
const drake::maliput::api::RoadGeometry* road,
std::unique_ptr<AgentPluginParams> parameters);

/// Specify the exact plugin name if there should be more than one plugin
/// in the plugin library.
int AddLoadableAgent(
const std::string& plugin_library_name, const std::string& plugin_name,
const std::map<std::string, linb::any>& parameters,
const std::string& name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state);
const std::string& agent_name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state,
const drake::maliput::api::RoadGeometry* road,
std::unique_ptr<AgentPluginParams> parameters);

/// A handy overload in case the plugin doesn't require any extra parameters.
int AddLoadableAgent(
const std::string& plugin_library_name, const std::string& agent_name,
std::unique_ptr<drake::systems::BasicVector<T>> initial_state,
const drake::maliput::api::RoadGeometry* road);

/// Sets the RoadGeometry for this simulation.
///
Expand Down Expand Up @@ -196,7 +200,6 @@ class AutomotiveSimulator {
void InitializeLoadableAgents();

// For both building and simulation.
std::unique_ptr<drake::lcm::DrakeLcmInterface> lcm_{};
std::unique_ptr<const drake::maliput::api::RoadGeometry> road_{};

// === Start for building. ===
Expand Down
68 changes: 35 additions & 33 deletions backend/python_bindings.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,10 @@
#include "backend/simulation_run_stats.h"
#include "backend/simulation_runner.h"

#include "../include/delphyne/linb-any"
#include "include/delphyne/agent_plugin_base.h"
#include "src/agents/mobil_car.h"
#include "src/agents/rail_car.h"
#include "src/agents/trajectory_car.h"

namespace py = pybind11;

Expand All @@ -26,6 +29,10 @@ using delphyne::AutomotiveSimulator;
using delphyne::RoadBuilder;
using delphyne::SimulatorRunner;
using delphyne::InteractiveSimulationStats;
using delphyne::AgentPluginParams;
using delphyne::RailCarAgentParams;
using delphyne::TrajectoryCarAgentParams;
using delphyne::MobilCarAgentParams;
using drake::automotive::LaneDirection;
using drake::automotive::MaliputRailcarParams;
using drake::automotive::MaliputRailcarState;
Expand All @@ -37,6 +44,20 @@ PYBIND11_MODULE(python_bindings, m) {
py::module::import("pydrake.systems.framework");
py::module::import("pydrake.maliput.api");

py::class_<AgentPluginParams>(m, "AgentPluginParams");

py::class_<RailCarAgentParams, AgentPluginParams>(m, "RailCarAgentParams")
.def(py::init<
std::unique_ptr<drake::automotive::LaneDirection>,
std::unique_ptr<drake::automotive::MaliputRailcarParams<double>>>());

py::class_<TrajectoryCarAgentParams, AgentPluginParams>(
m, "TrajectoryCarAgentParams")
.def(py::init<std::unique_ptr<drake::automotive::Curve2<double>>>());

py::class_<MobilCarAgentParams, AgentPluginParams>(m, "MobilCarAgentParams")
.def(py::init<bool>());

py::class_<MaliputRailcarState<double>, BasicVector<double>>(
m, "MaliputRailcarState")
.def(py::init<>())
Expand All @@ -58,22 +79,6 @@ PYBIND11_MODULE(python_bindings, m) {
&MaliputRailcarParams<double>::velocity_limit_kp,
&MaliputRailcarParams<double>::set_velocity_limit_kp);

// TODO(basicNew): Properly fill this binding with the remaining methods and
// overloaded constructors.
// Note: Since we are using linb::any in combination with bare pointers to
// pass generic parameters to the loadable agents module, we have to make
// sure python doesn't garbage collect temp objects while they are being
// used on the C++ side, hence the `py::keep_alive`. See
// http://pybind11.readthedocs.io/en/stable/advanced/functions.html#keep-alive
py::class_<linb::any>(m, "Any")
.def(py::init<bool&&>())
// Keep alive, ownership: `self` keeps `RoadGeometry` alive.
.def(py::init<const RoadGeometry*&&>(), py::keep_alive<1, 2>())
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Even if we go in this direction, you'll probably want to keep anything maliput related (road related) around so that the user can dig into the roads (i.e. world ground truth) in scenario setup, or callback scripts.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure I follow; is this related to the python bindings discussion we were having on slack or is it a different thing?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was working under the mistaken assumption that the road was instantiated in python. From a look at the PR though, looks like it's getting built by RoadBuilder, which means being instantiated in c++ with ownership transferred to the simulator. This is fine.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, got it.

// Keep alive, ownership: `self` keeps `LaneDirection` alive.
.def(py::init<LaneDirection*&&>(), py::keep_alive<1, 2>())
// Keep alive, ownership: `self` keeps `MaliputRailcarParams` alive.
.def(py::init<MaliputRailcarParams<double>*&&>(), py::keep_alive<1, 2>());

py::class_<InteractiveSimulationStats>(m, "InteractiveSimulationStats")
.def(py::init<>())
.def("TotalElapsedSimtime",
Expand Down Expand Up @@ -116,31 +121,28 @@ PYBIND11_MODULE(python_bindings, m) {
.def("AddMonolaneFromFile", &RoadBuilder<double>::AddMonolaneFromFile)
.def("AddMultilaneFromFile", &RoadBuilder<double>::AddMultilaneFromFile);

// Note: Since AddLoadableAgent uses a map of (string, linb::any) in
// combination with bare pointers to pass generic parameters, we have to make
// sure python doesn't garbage collect temp objects while they are being
// used on the C++ side, hence the `py::keep_alive`. See
// http://pybind11.readthedocs.io/en/stable/advanced/functions.html#keep-alive
py::class_<AutomotiveSimulator<double>>(m, "AutomotiveSimulator")
.def(py::init(
[](void) { return std::make_unique<AutomotiveSimulator<double>>(); }))
.def("Start", &AutomotiveSimulator<double>::Start)
.def("AddLoadableAgent",
py::overload_cast<
const std::string&, const std::map<std::string, linb::any>&,
const std::string&,
std::unique_ptr<drake::systems::BasicVector<double>>>(
&AutomotiveSimulator<double>::AddLoadableAgent),
py::keep_alive<1, 3>()) // Keep alive, ownership: `self` keeps
// `parameters` alive.
const std::string&, const std::string&,
std::unique_ptr<drake::systems::BasicVector<double>>,
const RoadGeometry*>(
&AutomotiveSimulator<double>::AddLoadableAgent))
.def("AddLoadableAgent",
py::overload_cast<
const std::string&, const std::string&,
const std::map<std::string, linb::any>&, const std::string&,
std::unique_ptr<drake::systems::BasicVector<double>>>(
&AutomotiveSimulator<double>::AddLoadableAgent),
py::keep_alive<1, 3>()); // Keep alive, ownership: `self` keeps
// `parameters` alive.
std::unique_ptr<drake::systems::BasicVector<double>>,
const RoadGeometry*, std::unique_ptr<AgentPluginParams>>(
&AutomotiveSimulator<double>::AddLoadableAgent))
.def("AddLoadableAgent",
py::overload_cast<
const std::string&, const std::string&, const std::string&,
std::unique_ptr<drake::systems::BasicVector<double>>,
const RoadGeometry*, std::unique_ptr<AgentPluginParams>>(
&AutomotiveSimulator<double>::AddLoadableAgent));
}

} // namespace
5 changes: 2 additions & 3 deletions backend/translation_systems/drake_to_ign.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ class DrakeToIgn : public drake::systems::LeafSystem<double> {
// @param[in] position The position array
// @param[out] ign_position The ignition position message.
static void PositionArrayToIgnition(const float position[3],
ignition::msgs::Vector3d* ign_position) {
ignition::msgs::Vector3d* ign_position) {
DELPHYNE_DEMAND(ign_position != nullptr);

ign_position->set_x(position[0]);
Expand All @@ -98,8 +98,7 @@ class DrakeToIgn : public drake::systems::LeafSystem<double> {
// @param[in] quaternion The orientation array.
// @param[out] ign_quaternion The ign quaternion message.
static void QuaternionArrayToIgnition(
const float quaternion[4],
ignition::msgs::Quaternion* ign_quaternion) {
const float quaternion[4], ignition::msgs::Quaternion* ign_quaternion) {
DELPHYNE_DEMAND(ign_quaternion != nullptr);

ign_quaternion->set_w(quaternion[0]);
Expand Down
4 changes: 2 additions & 2 deletions backend/translation_systems/lcm_viewer_draw_to_ign_model_v.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,12 +48,12 @@ void LcmViewerDrawToIgnModelV::DoDrakeToIgnTranslation(
// Checks position size and translates.
DELPHYNE_DEMAND(lcm_message.position[i].size() == kPositionVectorSize);
PositionArrayToIgnition(lcm_message.position[i].data(),
pose->mutable_position());
pose->mutable_position());

// Checks orientation size and translates.
DELPHYNE_DEMAND(lcm_message.quaternion[i].size() == kOrientationVectorSize);
QuaternionArrayToIgnition(lcm_message.quaternion[i].data(),
pose->mutable_orientation());
pose->mutable_orientation());
}
}

Expand Down
2 changes: 1 addition & 1 deletion backend/translation_systems/lcm_viewer_load_robot_to_ign_model_v.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ void LcmViewerLoadRobotToIgnModelV::DoDrakeToIgnTranslation(
ignition::msgs::Pose* pose = new_visual->mutable_pose();
PositionArrayToIgnition(geometry.position, pose->mutable_position());
QuaternionArrayToIgnition(geometry.quaternion,
pose->mutable_orientation());
pose->mutable_orientation());

ignition::msgs::Material* material = new_visual->mutable_material();
LcmColorToIgnition(geometry.color, material->mutable_diffuse());
Expand Down
1 change: 0 additions & 1 deletion include/delphyne/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,5 @@
delphyne_install_includes(
${PROJECT_NAME}${PROJECT_MAJOR_VERSION}/${PROJECT_NAME}/
agent_plugin_base.h
linb-any
types.h
)
Loading