diff --git a/bindings/pydrake/BUILD.bazel b/bindings/pydrake/BUILD.bazel index 6f3fbbd664d1..17808cce8a3d 100644 --- a/bindings/pydrake/BUILD.bazel +++ b/bindings/pydrake/BUILD.bazel @@ -116,6 +116,7 @@ PY_LIBRARIES_WITH_INSTALL = [ ":math_py", ":symbolic_py", "//bindings/pydrake/examples", + "//bindings/pydrake/maliput", "//bindings/pydrake/multibody", "//bindings/pydrake/solvers", "//bindings/pydrake/systems", diff --git a/bindings/pydrake/maliput/BUILD.bazel b/bindings/pydrake/maliput/BUILD.bazel new file mode 100644 index 000000000000..45be7d5337fa --- /dev/null +++ b/bindings/pydrake/maliput/BUILD.bazel @@ -0,0 +1,91 @@ +# -*- 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_so_name = "api", + cc_srcs = ["api_py.cc"], + package_info = PACKAGE_INFO, + py_deps = [ + ":module_py", + ], +) + +drake_pybind_library( + name = "dragway_py", + cc_so_name = "dragway", + cc_srcs = ["dragway_py.cc"], + package_info = PACKAGE_INFO, + py_deps = [ + ":module_py", + ], +) + +PY_LIBRARIES_WITH_INSTALL = [ + ":api_py", + ":dragway_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..4e8f890fe131 --- /dev/null +++ b/bindings/pydrake/maliput/api_py.cc @@ -0,0 +1,52 @@ +#include +#include + +#include "drake/automotive/maliput/api/junction.h" +#include "drake/automotive/maliput/api/lane.h" +#include "drake/automotive/maliput/api/lane_data.h" +#include "drake/automotive/maliput/api/road_geometry.h" +#include "drake/automotive/maliput/api/segment.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."; + + // TODO(jadecastro) These bindings are work-in-progress. Expose additional + // Maliput API features, as necessary (see #7918). + + py::class_(m, "RoadGeometryId") + .def(py::init()) + .def("string", &RoadGeometryId::string, py_reference_internal); + + py::class_ (m, "GeoPosition") + .def(py::init(), py::arg("x"), py::arg("y"), + py::arg("z")) + .def("xyz", &GeoPosition::xyz, py_reference_internal); + + py::class_(m, "LanePosition") + .def(py::init(), py::arg("s"), py::arg("r"), + py::arg("h")) + .def("srh", &LanePosition::srh, py_reference_internal); + + py::class_(m, "RoadGeometry") + .def("junction", &RoadGeometry::junction, py_reference_internal); + + py::class_(m, "Junction") + .def("segment", &Junction::segment, py_reference_internal); + + py::class_(m, "Segment") + .def("lane", &Segment::lane, py_reference_internal); + + py::class_(m, "Lane") + .def("ToLanePosition", &Lane::ToLanePosition) + .def("ToGeoPosition", &Lane::ToGeoPosition); +} + +} // 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..22a6d73253e5 --- /dev/null +++ b/bindings/pydrake/maliput/dragway_py.cc @@ -0,0 +1,36 @@ +#include +#include + +#include "drake/automotive/maliput/api/road_geometry.h" +#include "drake/automotive/maliput/dragway/road_geometry.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" + +namespace drake { +namespace pydrake { + +using std::make_unique; +using std::unique_ptr; + +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"); + + m.def("create_dragway", + [](api::RoadGeometryId road_id, int num_lanes, double length, + double lane_width, double shoulder_width, double maximum_height, + double linear_tolerance, double angular_tolerance) { + return make_unique( + road_id, num_lanes, length, lane_width, shoulder_width, + maximum_height, linear_tolerance, angular_tolerance); + }, py::arg("road_id"), py::arg("num_lanes"), py::arg("length"), + py::arg("lane_width"), py::arg("shoulder_width"), + py::arg("maximum_height"), py::arg("linear_tolerance"), + py::arg("angular_tolerance")); +} + +} // 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..51661b18c898 --- /dev/null +++ b/bindings/pydrake/maliput/test/maliput_test.py @@ -0,0 +1,95 @@ +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, + RoadGeometry, + ) +from pydrake.maliput.dragway import ( + create_dragway, + ) + + +# Tests the bindings for the API and backend implementations. +class TestMaliput(unittest.TestCase): + def test_api(self): + # Test the containers' constructors and accessors. + srh = [4., 5., 6.] + lane_pos = LanePosition(srh[0], srh[1], srh[2]) + self.assertTrue(len(lane_pos.srh()) == 3) + self.assertTrue(np.allclose(lane_pos.srh(), srh)) + + lane_pos_alt = LanePosition(s=4., r=5., h=6.) + self.assertTrue(np.allclose(lane_pos_alt.srh(), srh)) + + xyz = [1., 2., 3.] + geo_pos = GeoPosition(xyz[0], xyz[1], xyz[2]) + self.assertTrue(len(geo_pos.xyz()) == 3) + self.assertTrue(np.allclose(geo_pos.xyz(), xyz)) + + geo_pos_alt = GeoPosition(x=1., y=2., z=3.) + self.assertTrue(np.allclose(geo_pos_alt.xyz(), xyz)) + + # Check that the getters are read-only. + with self.assertRaises(ValueError): + lane_pos.srh()[0] = 0. + with self.assertRaises(ValueError): + geo_pos.xyz()[0] = 0. + + # Test RoadGeometryId accessors. + string = "foo" + rg_id = RoadGeometryId(string) + self.assertTrue(rg_id.string() == string) + + 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 = create_dragway(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) + + # Alternate constructor. + create_dragway( + road_id=rg_id, num_lanes=kNumLanes, length=kLength, + lane_width=kLaneWidth, shoulder_width=kShoulderWidth, + maximum_height=kHeight, linear_tolerance=kTol, + angular_tolerance=kTol) + + # Test the Lane <-> Geo space coordinate conversion. + lane_pos = LanePosition(0., 0., 0.) + geo_pos_result = lane_0.ToGeoPosition(lane_pos) + geo_pos_expected = GeoPosition(0., -kLaneWidth / 2., 0.) + self.assertTrue(np.allclose(geo_pos_result.xyz(), + geo_pos_expected.xyz())) + + 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.) + self.assertTrue(np.allclose(lane_pos_result.srh(), + lane_pos_expected.srh())) + self.assertTrue(np.allclose(nearest_pos.xyz(), geo_pos.xyz())) + self.assertTrue(distance == 0.) + + # TODO(jadecastro) Add more maliput backends as needed. + + +if __name__ == '__main__': + unittest.main()