Merge pull request #177 from ToyotaResearchInstitute/more_bindings

Add bindings for AutomotiveSimulator

backend/CMakeLists.txt

@@ -1,7 +1,5 @@
 include (${project_cmake_dir}/Utils.cmake)
 include_directories(${PYTHON_INCLUDE_DIRS})
-include_directories(${pybind11_INCLUDE_DIRS})
-
 # ----------------------------------------
 # Prepare sources
 set (sources
@@ -42,13 +40,6 @@ delphyne_build_tests(${gtest_sources})
 # ----------------------------------------
 # Python bindings
 
-# Find necessary packages
-find_package(PythonLibs 2.7 REQUIRED)
-include_directories(${PYTHON_INCLUDE_DIRS})
-
-find_package(Boost COMPONENTS python REQUIRED)
-include_directories(${Boost_INCLUDE_DIR})
-
 # Build the simulation support for python bindings as a library.
 add_library(simulation-support
     STATIC

backend/python_bindings_example.py

@@ -46,7 +46,14 @@
 import time
 
 from launcher import Launcher
-from simulation_runner_py import SimulatorRunner
+
+from pydrake.common import AddResourceSearchPath
+
+from simulation_runner_py import (
+    AutomotiveSimulator,
+    SimpleCarState,
+    SimulatorRunner
+)
 
 
 def get_from_env_or_fail(var):
@@ -59,10 +66,7 @@ def get_from_env_or_fail(var):
 
     # Since it is an environment variable, the very end may have a colon;
     # strip it here
-    if value[-1] == ':':
-        value = value[:-1]
-
-    return value
+    return value.rstrip(':')
 
 
 class SimulationStats(object):
@@ -102,6 +106,15 @@ def random_print():
         print("One in five hundred")
 
 
+def create_and_init_automotive_simulator():
+    simulator = AutomotiveSimulator()
+    state = SimpleCarState()
+    state.y = 0.0
+    simulator.AddPriusSimpleCar("0", "DRIVING_COMMAND_0", state)
+
+    return simulator
+
+
 def main():
     """Spawn an automotive simulator making use of the python bindings"""
     stats = SimulationStats()
@@ -111,6 +124,10 @@ def main():
     lcm_ign_bridge = "duplex-ign-lcm-bridge"
     ign_visualizer = "visualizer"
 
+    drake_install_path = get_from_env_or_fail('DRAKE_INSTALL_PATH')
+    AddResourceSearchPath(os.path.join(drake_install_path, "share", "drake"))
+
+    simulator = create_and_init_automotive_simulator()
     try:
         launcher.launch([lcm_ign_bridge, "1"])
         teleop_config = os.path.join(delphyne_ws_dir,
@@ -120,7 +137,7 @@ def main():
                                      "layoutWithTeleop.config")
         launcher.launch([ign_visualizer, teleop_config])
 
-        runner = SimulatorRunner()
+        runner = SimulatorRunner(simulator, 0.001)
         # Add a callback to record and print statistics
         runner.AddStepCallback(stats.record_tick)
         # Add a second callback that prints a message roughly every 500 calls

backend/simulation_runner.h

@@ -39,7 +39,6 @@
 #include <ignition/msgs.hh>
 #include <ignition/transport/Node.hh>
 
-#include <Python.h>
 #include <pybind11/pybind11.h>
 
 #include "backend/automotive_simulator.h"

backend/simulation_runner_py.cc

@@ -35,41 +35,70 @@
 #include "backend/automotive_simulator.h"
 #include "backend/simulation_runner.h"
 
+namespace py = pybind11;
+
+using delphyne::backend::AutomotiveSimulator;
 using delphyne::backend::SimulatorRunner;
+using drake::automotive::SimpleCarState;
 
 // Since we are not yet exporting the AutomotiveSimulator class we need to
 // provide a ready-to-run SimulatorRunner. To do so we create a parameterless
 // constructor that sets up a simulation runner with a prius car in it. As we
 // keep adding python bindings to C++ classes this code will be moved to the
 // python scripts that launches the simulation.
 
-namespace py = pybind11;
-
 namespace {
 PYBIND11_MODULE(simulation_runner_py, m) {
   py::class_<SimulatorRunner>(m, "SimulatorRunner")
-    .def(py::init([](void) {
-      // TODO(mikaelaguedas) All this should be done in Python using pydrake
-      // and custom bindings for AutomotiveSimulator and SimpleCarState
-      drake::AddResourceSearchPath(std::string(std::getenv("DRAKE_INSTALL_PATH")) +
-                                   "/share/drake");
-
-      auto simulator =
-          std::make_unique<delphyne::backend::AutomotiveSimulator<double>>();
-
-      // Add a Prius car.
-      drake::automotive::SimpleCarState<double> state;
-      state.set_y(0.0);
-      simulator->AddPriusSimpleCar("0", "DRIVING_COMMAND_0", state);
-
-      // Instantiate the simulator runner and pass the simulator.
-      const double time_step = 0.001;
+    .def(py::init([](std::unique_ptr<AutomotiveSimulator<double>> simulator, double time_step) {
       return std::make_unique<SimulatorRunner>(std::move(simulator), time_step);
     }))
     .def("Start", &SimulatorRunner::Start)
     .def("Stop", &SimulatorRunner::Stop)
     .def("AddStepCallback", &SimulatorRunner::AddStepCallback);
   ;
+  py::class_<AutomotiveSimulator<double>>(m, "AutomotiveSimulator")
+    .def(py::init([](void) {
+        return std::make_unique<AutomotiveSimulator<double>>();
+    }))
+    .def("Start", &AutomotiveSimulator<double>::Start)
+    .def("AddPriusSimpleCar", &AutomotiveSimulator<double>::AddPriusSimpleCar)
+    .def("AddMobilControlledSimpleCar", &AutomotiveSimulator<double>::AddMobilControlledSimpleCar)
+    // TODO(mikaelarguedas) bind more method depending on what we need
+    // Needs drake::automotive::Curve2<double>&
+    // .def("AddPriusTrajectoryCar", &AutomotiveSimulator<double>::AddPriusTrajectoryCar)
+    // Needs:
+    //  - drake::automotive::LaneDirection
+    //  - drake::automotive::MaliputRailcarParams<T>
+    //  - drake::automotive::MaliputRailcarState<T>
+    // .def("AddPriusMaliputRailcar", &AutomotiveSimulator<double>::AddPriusMaliputRailcar)
+    // Needs:
+    //  - drake::automotive::LaneDirection
+    //  - drake::automotive::MaliputRailcarParams<T>
+    //  - drake::automotive::MaliputRailcarState<T>
+    // .def("AddIdmControlledPriusMaliputRailcar", &AutomotiveSimulator<double>::AddIdmControlledPriusMaliputRailcar)
+    // .def("SetMaliputRailcarAccelerationCommand", &AutomotiveSimulator<double>::SetMaliputRailcarAccelerationCommand)
+    // Needs drake::maliput::api::RoadGeometry binding
+    // .def("SetRoadGeometry", &AutomotiveSimulator<double>::SetRoadGeometry)
+    // Needs drake::maliput::api::Lane binding
+    // .def("FindLane", &AutomotiveSimulator<double>::FindLane)
+    // Needs drake::systems::System<T> binding
+    // .def("GetDiagramSystemByName", &AutomotiveSimulator<double>::GetDiagramSystemByName)
+    // .def("Build", &AutomotiveSimulator<double>::Build)
+    // .def("GetDiagram", &AutomotiveSimulator<double>::GetDiagram)
+    // .def("StepBy", &AutomotiveSimulator<double>::StepBy)
+    // Needs drake::systems::rendering::PoseBundle<T> binding
+    // .def("GetCurrentPoses", &AutomotiveSimulator<double>::GetCurrentPoses)
+  ;
+  // TODO(mikaelarguedas) Submit this to upstream Drake
+  py::class_<SimpleCarState<double>>(m, "SimpleCarState")
+    .def(py::init<>())
+    .def_property("x", &SimpleCarState<double>::x, &SimpleCarState<double>::set_x)
+    .def_property("y", &SimpleCarState<double>::y, &SimpleCarState<double>::set_y)
+    .def_property("heading", &SimpleCarState<double>::heading, &SimpleCarState<double>::set_heading)
+    .def_property("velocity", &SimpleCarState<double>::velocity, &SimpleCarState<double>::set_velocity)
+    .def("get_coordinates_names", &SimpleCarState<double>::GetCoordinateNames)
+  ;
 }
 
 }  // namespace