From 995bec12df5a124965fc31b395a5bc5abbccaefe Mon Sep 17 00:00:00 2001 From: Jon Date: Mon, 12 Feb 2018 14:42:14 -0500 Subject: [PATCH] Add bindings for a small subset of maliput api and dragway backend --- bindings/pydrake/maliput/BUILD.bazel | 96 +++++++++++++++++++ bindings/pydrake/maliput/__init__.py | 1 + bindings/pydrake/maliput/api_py.cc | 30 ++++++ bindings/pydrake/maliput/dragway_py.cc | 42 ++++++++ bindings/pydrake/maliput/test/maliput_test.py | 73 ++++++++++++++ 5 files changed, 242 insertions(+) create mode 100644 bindings/pydrake/maliput/BUILD.bazel create mode 100644 bindings/pydrake/maliput/__init__.py create mode 100644 bindings/pydrake/maliput/api_py.cc create mode 100644 bindings/pydrake/maliput/dragway_py.cc create mode 100644 bindings/pydrake/maliput/test/maliput_test.py diff --git a/bindings/pydrake/maliput/BUILD.bazel b/bindings/pydrake/maliput/BUILD.bazel new file mode 100644 index 000000000000..ada0f16b7fb8 --- /dev/null +++ b/bindings/pydrake/maliput/BUILD.bazel @@ -0,0 +1,96 @@ +# -*- python -*- + +load("@drake//tools/install:install.bzl", "install") +load("//tools/lint:lint.bzl", "add_lint_tests") +load( + "//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", +) +load( + "//tools/skylark:drake_py.bzl", + "drake_py_binary", + "drake_py_library", + "drake_py_test", +) + +package(default_visibility = [ + "//bindings/pydrake:__subpackages__", +]) + +# This determines how `PYTHONPATH` is configured, and how to install the +# bindings. +PACKAGE_INFO = get_pybind_package_info("//bindings") + +# @note Symbols are NOT imported directly into +# `__init__.py` to simplify dependency management, meaning that +# classes are organized by their directory structure rather than +# by C++ namespace. +drake_py_library( + name = "module_py", + srcs = ["__init__.py"], + imports = PACKAGE_INFO.py_imports, + deps = [ + "//bindings/pydrake:common_py", + ], +) + +drake_pybind_library( + name = "api_py", + cc_deps = [ + "//automotive/maliput/api", + ], + cc_so_name = "api", + cc_srcs = ["api_py.cc"], + package_info = PACKAGE_INFO, + py_deps = [ + ":module_py", + ], +) + +drake_pybind_library( + name = "dragway_py", + cc_deps = [ + "//automotive/maliput/dragway", + ], + cc_so_name = "dragway", + cc_srcs = ["dragway_py.cc"], + package_info = PACKAGE_INFO, + py_deps = [ + ":module_py", + ], +) + +PY_LIBRARIES_WITH_INSTALL = [ + ":maliput_py", +] + +PY_LIBRARIES = [ + ":module_py", +] + +drake_py_library( + name = "maliput", + imports = PACKAGE_INFO.py_imports, + deps = PY_LIBRARIES_WITH_INSTALL + PY_LIBRARIES, +) + +install( + name = "install", + targets = PY_LIBRARIES, + py_dest = PACKAGE_INFO.py_dest, + deps = get_drake_py_installs(PY_LIBRARIES_WITH_INSTALL), +) + +drake_py_test( + name = "maliput_test", + size = "small", + deps = [ + ":api_py", + ":dragway_py", + "//bindings/pydrake/systems", + ], +) + +add_lint_tests() diff --git a/bindings/pydrake/maliput/__init__.py b/bindings/pydrake/maliput/__init__.py new file mode 100644 index 000000000000..8df4b86ebc5a --- /dev/null +++ b/bindings/pydrake/maliput/__init__.py @@ -0,0 +1 @@ +# Blank Python module. diff --git a/bindings/pydrake/maliput/api_py.cc b/bindings/pydrake/maliput/api_py.cc new file mode 100644 index 000000000000..98ff41f7c33b --- /dev/null +++ b/bindings/pydrake/maliput/api_py.cc @@ -0,0 +1,30 @@ +#include +#include + +#include "drake/automotive/maliput/api/lane_data.h" +#include "drake/automotive/maliput/api/road_geometry.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" + +namespace drake { +namespace pydrake { + +PYBIND11_MODULE(api, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::maliput::api; + + m.doc() = "Bindings for the Maliput API."; + + py::class_(m, "RoadGeometryId") + .def(py::init()); + + py::class_ (m, "GeoPosition") + .def(py::init()) + .def("xyz", &GeoPosition::xyz); + + py::class_(m, "LanePosition") + .def(py::init()) + .def("srh", &LanePosition::srh); +} + +} // namespace pydrake +} // namespace drake diff --git a/bindings/pydrake/maliput/dragway_py.cc b/bindings/pydrake/maliput/dragway_py.cc new file mode 100644 index 000000000000..9e2699a16f89 --- /dev/null +++ b/bindings/pydrake/maliput/dragway_py.cc @@ -0,0 +1,42 @@ +#include +#include + +#include "drake/automotive/maliput/dragway/lane.h" +#include "drake/automotive/maliput/dragway/junction.h" +#include "drake/automotive/maliput/dragway/road_geometry.h" +#include "drake/automotive/maliput/dragway/segment.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" + +namespace drake { +namespace pydrake { + +PYBIND11_MODULE(dragway, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::maliput; + + m.doc() = "Bindings for the Dragway backend."; + + py::class_(m, "RoadGeometry") + .def(py::init()) + .def("junction", &dragway::RoadGeometry::junction, py_reference_internal, + // Keep alive, reference: `return` keeps `self` alive. + py::keep_alive<0, 1>()); + + py::class_(m, "Junction") + .def("segment", &dragway::Junction::segment, py_reference_internal, + // Keep alive, reference: `return` keeps `self` alive. + py::keep_alive<0, 1>()); + + py::class_(m, "Segment") + .def("lane", &dragway::Segment::lane, py_reference_internal, + // Keep alive, reference: `return` keeps `self` alive. + py::keep_alive<0, 1>()); + + py::class_(m, "Lane") + .def("ToLanePosition", &dragway::Lane::ToLanePosition) + .def("ToGeoPosition", &dragway::Lane::ToGeoPosition); +} + +} // namespace pydrake +} // namespace drake diff --git a/bindings/pydrake/maliput/test/maliput_test.py b/bindings/pydrake/maliput/test/maliput_test.py new file mode 100644 index 000000000000..29ed2fbe314d --- /dev/null +++ b/bindings/pydrake/maliput/test/maliput_test.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import copy +import unittest +import numpy as np + +import pydrake.systems.framework as framework +from pydrake.maliput.api import ( + RoadGeometryId, + LanePosition, + GeoPosition, + ) +import pydrake.maliput.dragway as dragway + + +# Tests the bindings for the API and backend implementations. +class TestMaliput(unittest.TestCase): + def test_api(self): + srh = [4., 5., 6.] + lane_pos = LanePosition(srh[0], srh[1], srh[2]) + self.assertTrue(len(lane_pos.srh()) == 3) + for i in [0, 1, 2]: + self.assertTrue(lane_pos.srh()[i] == srh[i]) + xyz = [1., 2., 3.] + geo_pos = GeoPosition(xyz[0], xyz[1], xyz[2]) + self.assertTrue(len(geo_pos.xyz()) == 3) + for i in [0, 1, 2]: + self.assertTrue(geo_pos.xyz()[i] == xyz[i]) + + def test_dragway(self): + kNumLanes = 2 + kLength = 100. + kLaneWidth = 4. + kShoulderWidth = 1. + kHeight = 5. + kTol = 1e-6 + + # Instantiate a two-lane straight road. + rg_id = RoadGeometryId("two_lane_road") + rg = dragway.RoadGeometry( + rg_id, kNumLanes, kLength, kLaneWidth, kShoulderWidth, kHeight, + kTol, kTol) + segment = rg.junction(0).segment(0) + lane_0 = segment.lane(0) + lane_1 = segment.lane(1) + + # Test the . + lane_pos = LanePosition(0., 0., 0.) + geo_pos_result = lane_0.ToGeoPosition(lane_pos) + geo_pos_expected = GeoPosition(0., -kLaneWidth / 2., 0.) + for i in [0, 1, 2]: + self.assertTrue(geo_pos_result.xyz()[i] == + geo_pos_expected.xyz()[i]) + + geo_pos = GeoPosition(1., kLaneWidth / 2., 3.) + nearest_pos = GeoPosition(0., 0., 0.) + distance = 0. + lane_pos_result = \ + lane_1.ToLanePosition(geo_pos, nearest_pos, distance) + lane_pos_expected = LanePosition(1., 0., 3.) + for i in [0, 1, 2]: + self.assertTrue(lane_pos_result.srh()[i] == + lane_pos_expected.srh()[i]) + self.assertTrue(nearest_pos.xyz()[i] == geo_pos.xyz()[i]) + self.assertTrue(distance == 0.) + + # TODO(jadecastro) Add more maliput backends as needed. + + +if __name__ == '__main__': + unittest.main()