Bind automotive PoseSelector (RobotLocomotion#8809)

* Bind automotive PoseSelector

bindings/pydrake/BUILD.bazel

@@ -79,6 +79,7 @@ drake_pybind_library(
     name = "automotive_py",
     cc_deps = [
         "//bindings/pydrake/systems:systems_pybind",
+        "//bindings/pydrake/util:wrap_pybind",
     ],
     cc_so_name = "automotive",
     cc_srcs = ["automotive_py.cc"],

bindings/pydrake/automotive_py.cc

@@ -4,11 +4,15 @@
 #include "drake/automotive/calc_ongoing_road_position.h"
 #include "drake/automotive/gen/driving_command.h"
 #include "drake/automotive/idm_controller.h"
+#include "drake/automotive/maliput/api/lane_data.h"
 #include "drake/automotive/maliput/api/road_geometry.h"
+#include "drake/automotive/pose_selector.h"
 #include "drake/automotive/pure_pursuit_controller.h"
+#include "drake/automotive/road_odometry.h"
 #include "drake/automotive/simple_car.h"
 #include "drake/bindings/pydrake/pydrake_pybind.h"
 #include "drake/bindings/pydrake/systems/systems_pybind.h"
+#include "drake/bindings/pydrake/util/wrap_pybind.h"
 #include "drake/systems/framework/basic_vector.h"
 
 namespace drake {
@@ -27,6 +31,10 @@ PYBIND11_MODULE(automotive, m) {
 
   using T = double;
 
+  py::enum_<AheadOrBehind>(m, "AheadOrBehind")
+      .value("kAhead", AheadOrBehind::kAhead)
+      .value("kBehind", AheadOrBehind::kBehind);
+
   py::enum_<RoadPositionStrategy>(m, "RoadPositionStrategy")
       .value("kCache", RoadPositionStrategy::kCache)
       .value("kExhaustiveSearch", RoadPositionStrategy::kExhaustiveSearch);
@@ -35,10 +43,40 @@ PYBIND11_MODULE(automotive, m) {
       .value("kPath", ScanStrategy::kPath)
       .value("kBranches", ScanStrategy::kBranches);
 
+  py::class_<ClosestPose<T>>(m, "ClosestPose")
+      .def(py::init<>())
+      .def(py::init<const RoadOdometry<T>&, const T&>(), py::arg("odom"),
+           py::arg("dist"),
+           // Keep alive, transitive: `self` keeps `RoadOdometry` pointer
+           // members alive.
+           py::keep_alive<1, 2>())
+      .def_readwrite("odometry", &ClosestPose<T>::odometry,
+                     py_reference_internal)
+      .def_readwrite("distance", &ClosestPose<T>::distance);
+
+  py::class_<RoadOdometry<T>> road_odometry(m, "RoadOdometry");
+  road_odometry
+      .def(py::init<>())
+      .def(py::init<const maliput::api::RoadPosition&,
+                    const systems::rendering::FrameVelocity<T>&>(),
+           py::arg("road_position"), py::arg("frame_velocity"),
+           // Keep alive, transitive: `self` keeps `RoadPosition` pointer
+           // members alive.
+           py::keep_alive<1, 2>())
+      .def(py::init<const maliput::api::Lane*,
+                    const maliput::api::LanePositionT<T>&,
+                    const systems::rendering::FrameVelocity<T>&>(),
+           py::arg("lane"), py::arg("lane_position"), py::arg("frame_velocity"),
+           // Keep alive, reference: `self` keeps `Lane*` alive.
+           py::keep_alive<1, 2>())
+      .def_readwrite("pos", &RoadOdometry<T>::pos)
+      .def_readwrite("vel", &RoadOdometry<T>::vel);
+  DefReadWriteKeepAlive(&road_odometry, "lane", &RoadOdometry<T>::lane);
+
   py::class_<LaneDirection>(m, "LaneDirection")
       .def(py::init<const maliput::api::Lane*, bool>(), py::arg("lane"),
            py::arg("with_s"))
-      .def_readwrite("lane", &LaneDirection::lane)
+      .def_readwrite("lane", &LaneDirection::lane, py_reference_internal)
       .def_readwrite("with_s", &LaneDirection::with_s);
   pysystems::AddValueInstantiation<LaneDirection>(m);
 
@@ -51,10 +89,10 @@ PYBIND11_MODULE(automotive, m) {
       .def("set_acceleration", &DrivingCommand<T>::set_acceleration);
 
   py::class_<IdmController<T>, LeafSystem<T>>(m, "IdmController")
-      .def(py::init<const maliput::api::RoadGeometry&,
-           ScanStrategy, RoadPositionStrategy, double>(), py::arg("road"),
-           py::arg("path_or_branches"), py::arg("road_position_strategy"),
-           py::arg("period_sec"))
+      .def(py::init<const maliput::api::RoadGeometry&, ScanStrategy,
+                    RoadPositionStrategy, double>(),
+           py::arg("road"), py::arg("path_or_branches"),
+           py::arg("road_position_strategy"), py::arg("period_sec"))
       .def("ego_pose_input", &IdmController<T>::ego_pose_input,
            py_reference_internal)
       .def("ego_velocity_input", &IdmController<T>::ego_velocity_input,
@@ -64,8 +102,35 @@ PYBIND11_MODULE(automotive, m) {
       .def("acceleration_output", &IdmController<T>::acceleration_output,
            py_reference_internal);
 
-  py::class_<PurePursuitController<T>, LeafSystem<T>>(
-      m, "PurePursuitController")
+  py::class_<PoseSelector<T>>(m, "PoseSelector")
+      .def_static(
+          "FindClosestPair",
+          [](const maliput::api::Lane* lane,
+             const systems::rendering::PoseVector<T>& ego_pose,
+             const systems::rendering::PoseBundle<T>& traffic_poses,
+             const T& scan_distance, ScanStrategy path_or_branches) {
+            return PoseSelector<T>::FindClosestPair(
+                lane, ego_pose, traffic_poses, scan_distance, path_or_branches);
+          },
+          py::arg("lane"), py::arg("ego_pose"), py::arg("traffic_poses"),
+          py::arg("scan_distance"), py::arg("path_or_branches"))
+      .def_static("FindSingleClosestPose",
+                  [](const maliput::api::Lane* lane,
+                     const systems::rendering::PoseVector<T>& ego_pose,
+                     const systems::rendering::PoseBundle<T>& traffic_poses,
+                     const T& scan_distance, const AheadOrBehind side,
+                     ScanStrategy path_or_branches) {
+                    return PoseSelector<T>::FindSingleClosestPose(
+                        lane, ego_pose, traffic_poses, scan_distance, side,
+                        path_or_branches);
+                  },
+                  py::arg("lane"), py::arg("ego_pose"),
+                  py::arg("traffic_poses"), py::arg("scan_distance"),
+                  py::arg("side"), py::arg("path_or_branches"))
+      .def_static("GetSigmaVelocity", &PoseSelector<T>::GetSigmaVelocity);
+
+  py::class_<PurePursuitController<T>, LeafSystem<T>>(m,
+                                                      "PurePursuitController")
       .def(py::init<>())
       .def("ego_pose_input", &PurePursuitController<T>::ego_pose_input,
            py_reference_internal)

bindings/pydrake/maliput/BUILD.bazel

@@ -38,6 +38,9 @@ drake_py_library(
 
 drake_pybind_library(
     name = "api_py",
+    cc_deps = [
+        "//bindings/pydrake/util:wrap_pybind",
+    ],
     cc_so_name = "api",
     cc_srcs = ["api_py.cc"],
     package_info = PACKAGE_INFO,

bindings/pydrake/maliput/api_py.cc

@@ -7,6 +7,7 @@
 #include "drake/automotive/maliput/api/road_geometry.h"
 #include "drake/automotive/maliput/api/segment.h"
 #include "drake/bindings/pydrake/pydrake_pybind.h"
+#include "drake/bindings/pydrake/util/wrap_pybind.h"
 
 namespace drake {
 namespace pydrake {
@@ -24,7 +25,7 @@ PYBIND11_MODULE(api, m) {
       .def(py::init<std::string>())
       .def("string", &RoadGeometryId::string, py_reference_internal);
 
-  py::class_<GeoPosition> (m, "GeoPosition")
+  py::class_<GeoPosition>(m, "GeoPosition")
       .def(py::init<double, double, double>(), py::arg("x"), py::arg("y"),
            py::arg("z"))
       .def("xyz", &GeoPosition::xyz, py_reference_internal);
@@ -34,6 +35,16 @@ PYBIND11_MODULE(api, m) {
            py::arg("h"))
       .def("srh", &LanePosition::srh, py_reference_internal);
 
+  py::class_<RoadPosition> road_position(m, "RoadPosition");
+  road_position
+      .def(py::init<>())
+      .def(py::init<const Lane*, const LanePosition&>(), py::arg("lane"),
+           py::arg("pos"),
+           // Keep alive, reference: `self` keeps `Lane*` alive.
+           py::keep_alive<1, 2>())
+      .def_readwrite("pos", &RoadPosition::pos);
+  DefReadWriteKeepAlive(&road_position, "lane", &RoadPosition::lane);
+
   py::class_<RoadGeometry>(m, "RoadGeometry")
       .def("junction", &RoadGeometry::junction, py_reference_internal);
 
@@ -43,9 +54,14 @@ PYBIND11_MODULE(api, m) {
   py::class_<Segment>(m, "Segment")
       .def("lane", &Segment::lane, py_reference_internal);
 
+  py::class_<LaneId>(m, "LaneId")
+      .def(py::init<std::string>())
+      .def("string", &LaneId::string, py_reference_internal);
+
   py::class_<Lane>(m, "Lane")
       .def("ToLanePosition", &Lane::ToLanePosition)
-      .def("ToGeoPosition", &Lane::ToGeoPosition);
+      .def("ToGeoPosition", &Lane::ToGeoPosition)
+      .def("id", &Lane::id);
 }
 
 }  // namespace pydrake

bindings/pydrake/maliput/test/maliput_test.py

@@ -6,16 +6,33 @@
 
 import pydrake.systems.framework as framework
 from pydrake.maliput.api import (
-    RoadGeometryId,
     LanePosition,
     GeoPosition,
     RoadGeometry,
+    RoadGeometryId,
+    RoadPosition,
     )
 from pydrake.maliput.dragway import (
     create_dragway,
     )
 
 
+# Instantiate and return a two-lane straight road.
+def make_test_dragway(kLaneWidth):
+    kNumLanes = 2
+    kLength = 100.
+    kShoulderWidth = 1.
+    kHeight = 5.
+    kTol = 1e-6
+
+    rg_id = RoadGeometryId("two_lane_road")
+    return 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)
+
+
 # Tests the bindings for the API and backend implementations.
 class TestMaliput(unittest.TestCase):
     def test_api(self):
@@ -36,6 +53,19 @@ def test_api(self):
         geo_pos_alt = GeoPosition(x=1., y=2., z=3.)
         self.assertTrue(np.allclose(geo_pos_alt.xyz(), xyz))
 
+        RoadPosition()
+        rg = make_test_dragway(kLaneWidth=4.)
+        lane_0 = rg.junction(0).segment(0).lane(0)
+        lane_1 = rg.junction(0).segment(0).lane(1)
+        road_pos = RoadPosition(lane=lane_0, pos=lane_pos)
+        self.assertEqual(road_pos.lane.id().string(), lane_0.id().string())
+        self.assertTrue(np.allclose(road_pos.pos.srh(), lane_pos.srh()))
+        road_pos.lane = lane_1
+        self.assertEqual(road_pos.lane.id().string(), lane_1.id().string())
+        new_srh = [42., 43., 44.]
+        road_pos.pos = LanePosition(s=new_srh[0], r=new_srh[1], h=new_srh[2])
+        self.assertTrue(np.allclose(road_pos.pos.srh(), new_srh))
+
         # Check that the getters are read-only.
         with self.assertRaises(ValueError):
             lane_pos.srh()[0] = 0.
@@ -45,31 +75,15 @@ def test_api(self):
         # Test RoadGeometryId accessors.
         string = "foo"
         rg_id = RoadGeometryId(string)
-        self.assertTrue(rg_id.string() == string)
+        self.assertEqual(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)
+        rg = make_test_dragway(kLaneWidth=kLaneWidth)
         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)