Add RoadPath that converts a sequence of Lane segments into a Piecewi…

…sePolynomial (#5662)

* Add RoadPath class and unit tests

drake/automotive/BUILD

@@ -189,6 +189,15 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "lane_direction",
+    srcs = ["lane_direction.cc"],
+    hdrs = ["lane_direction.h"],
+    deps = [
+        "//drake/automotive/maliput/api",
+    ],
+)
+
 drake_cc_library(
     name = "linear_car",
     srcs = ["linear_car.cc"],
@@ -206,6 +215,7 @@ drake_cc_library(
     deps = [
         ":calc_smooth_acceleration",
         ":generated_vectors",
+        ":lane_direction",
         "//drake/automotive/maliput/api",
         "//drake/math:geometric_transform",
         "//drake/systems/rendering:pose_vector",
@@ -251,6 +261,22 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "road_path",
+    srcs = ["road_path.cc"],
+    hdrs = ["road_path.h"],
+    visibility = [],
+    deps = [
+        ":lane_direction",
+        ":monolane_onramp_merge",
+        "//drake/automotive/maliput/api",
+        "//drake/common",
+        "//drake/common/trajectories:piecewise_polynomial",
+        "//drake/common/trajectories/qp_spline:spline_generation",
+        "//drake/math:saturate",
+    ],
+)
+
 drake_cc_library(
     name = "simple_car",
     srcs = ["simple_car.cc"],
@@ -707,6 +733,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "road_path_test",
+    deps = [
+        ":road_path",
+        "//drake/automotive/maliput/dragway",
+        "//drake/automotive/maliput/monolane",
+        "//drake/common:eigen_matrix_compare",
+    ],
+)
+
 drake_cc_googletest(
     name = "simple_powertrain_test",
     deps = [

drake/automotive/CMakeLists.txt

@@ -49,6 +49,7 @@ add_library_with_exports(LIB_NAME drakeAutomotive SOURCE_FILES
   maliput_railcar.cc
   monolane_onramp_merge.cc
   pose_selector.cc
+  road_path.cc
   simple_car.cc
   simple_powertrain.cc
   single_lane_ego_and_agent.cc
@@ -75,6 +76,7 @@ drake_install_headers(
   linear_car.h
   maliput_railcar.h
   monolane_onramp_merge.h
+  road_path.h
   pose_selector.h
   simple_car.h
   simple_car_to_euler_floating_joint.h

drake/automotive/lane_direction.cc

@@ -0,0 +1,4 @@
+#include "drake/automotive/lane_direction.h"
+
+// For now, this is an empty .cc file that only serves to confirm that
+// our .h file is a stand-alone header.

drake/automotive/lane_direction.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <memory>
+
+#include "drake/automotive/maliput/api/lane.h"
+
+namespace drake {
+namespace automotive {
+
+/// LaneDirection holds the lane that a MaliputRailcar is traversing and the
+/// direction in which it is moving. A MaliputRailcar can either travel in the
+/// increasing-`s` direction or in the decreasing-`s` direction.
+struct LaneDirection {
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(LaneDirection)
+
+  /// Default constructor.
+  LaneDirection() {}
+
+  /// A constructor that sets `with_s` to be `true`.
+  explicit LaneDirection(const maliput::api::Lane* lane_input)
+      : LaneDirection(lane_input, true) {}
+
+  /// Fully parameterized constructor.
+  LaneDirection(const maliput::api::Lane* lane_input, bool with_s_input)
+      : lane(lane_input), with_s(with_s_input) {}
+
+  const maliput::api::Lane* lane{nullptr};
+
+  /// True means that the MaliputRailcar's `s` coordinate increases when the
+  /// vehicle has positive speed. False means the opposite.
+  bool with_s{true};
+};
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/maliput_railcar.h

@@ -4,6 +4,7 @@
 
 #include "drake/automotive/gen/maliput_railcar_params.h"
 #include "drake/automotive/gen/maliput_railcar_state.h"
+#include "drake/automotive/lane_direction.h"
 #include "drake/automotive/maliput/api/lane.h"
 #include "drake/common/drake_copyable.h"
 #include "drake/systems/framework/leaf_system.h"
@@ -13,28 +14,6 @@
 namespace drake {
 namespace automotive {
 
-/// LaneDirection holds the lane that a MaliputRailcar is traversing and the
-/// direction in which it is moving. A MaliputRailcar can either travel in the
-/// increasing-`s` direction or in the decreasing-`s` direction.
-struct LaneDirection {
-  /// Default constructor.
-  LaneDirection() {}
-
-  /// A constructor that sets `with_s` to be `true`.
-  explicit LaneDirection(const maliput::api::Lane* lane_input)
-      : LaneDirection(lane_input, true) {}
-
-  /// Fully parameterized constructor.
-  LaneDirection(const maliput::api::Lane* lane_input, bool with_s_input)
-      : lane(lane_input), with_s(with_s_input) {}
-
-  const maliput::api::Lane* lane{nullptr};
-
-  /// True means that the MaliputRailcar's `s` coordinate increases when the
-  /// vehicle has positive speed. False means the opposite.
-  bool with_s{true};
-};
-
 /// MaliputRailcar models a vehicle that follows a maliput::api::Lane as if it
 /// were on rails and neglecting all physics.
 ///

drake/automotive/road_path.cc

@@ -0,0 +1,108 @@
+#include "drake/automotive/road_path.h"
+
+#include <vector>
+
+#include "drake/automotive/maliput/api/branch_point.h"
+#include "drake/automotive/maliput/api/lane.h"
+#include "drake/automotive/maliput/api/lane_data.h"
+#include "drake/common/cond.h"
+#include "drake/common/drake_assert.h"
+#include "drake/math/saturate.h"
+
+namespace drake {
+namespace automotive {
+
+using maliput::api::GeoPosition;
+using maliput::api::Lane;
+using maliput::api::LaneEnd;
+using maliput::api::LaneEndSet;
+using maliput::api::RoadGeometry;
+
+template <typename T>
+RoadPath<T>::RoadPath(const LaneDirection& initial_lane_direction,
+                      const T& step_size, int num_breaks)
+    : path_(MakePiecewisePolynomial(initial_lane_direction, step_size,
+                                    num_breaks)),
+      path_prime_(path_.derivative(1 /* 1st derivative */)),
+      path_double_prime_(path_.derivative(2 /* 2nd derivative */)) {}
+
+template <typename T>
+RoadPath<T>::~RoadPath() {}
+
+template <typename T>
+const PiecewisePolynomial<T>& RoadPath<T>::get_path() const {
+  return path_;
+}
+
+template <typename T>
+const T RoadPath<T>::GetClosestPathPosition(const Vector3<T>& geo_pos,
+                                            const T& s_guess) const {
+  DRAKE_ABORT();
+}
+
+template <typename T>
+const PiecewisePolynomial<T> RoadPath<T>::MakePiecewisePolynomial(
+    const LaneDirection& initial_lane_direction, const T& step_size,
+    int num_breaks) const {
+  std::vector<T> s_breaks{};
+  std::vector<MatrixX<T>> geo_knots(num_breaks, MatrixX<T>::Zero(3, 1));
+
+  LaneDirection ld = initial_lane_direction;
+  T s_lane{cond(ld.with_s, T(0.), T(ld.lane->length()))};
+  T s_break{0.};
+
+  // Loop over all the breaks and extract the knot points.
+  for (int i = 0; i < num_breaks - 1; ++i) {
+    s_breaks.emplace_back(s_break);
+    s_break += T(step_size);
+
+    GeoPosition geo_pos =
+        ld.lane->ToGeoPosition({s_lane /* s */, 0. /* r */, 0. /* h */});
+    geo_knots[i] << T(geo_pos.x), T(geo_pos.y), T(geo_pos.z);
+
+    // Take a step.
+    if (ld.with_s) {
+      s_lane += T(step_size);
+    } else {
+      s_lane -= T(step_size);
+    }
+
+    // Compute the distance in excess of the lane boundary by taking this step.
+    const T out_distance{
+        cond(ld.with_s, T(s_lane - ld.lane->length()), T(-s_lane))};
+    if (out_distance >= 0.) {
+      const LaneEndSet* lane_end_set{
+          ld.with_s ? ld.lane->GetOngoingBranches(LaneEnd::kFinish)
+                    : ld.lane->GetOngoingBranches(LaneEnd::kStart)};
+      if (lane_end_set->size() == 0) {  // There are no more ongoing lanes.
+        // If needed, add another knot point to make up the remaining distance.
+        if (out_distance != 0.) {
+          s_breaks.emplace_back(s_break + T(step_size - out_distance));
+          s_lane = cond(ld.with_s, T(ld.lane->length()), T(0.));
+          geo_pos =
+              ld.lane->ToGeoPosition({s_lane /* s */, 0. /* r */, 0. /* h */});
+          geo_knots[i + 1] << T(geo_pos.x), T(geo_pos.y), T(geo_pos.z);
+        }
+        break;
+      }
+
+      // Always choose the first lane in the set as the new lane.
+      ld.lane = lane_end_set->get(0).lane;
+      ld.with_s = (lane_end_set->get(0).end == LaneEnd::kStart) ? true : false;
+
+      // Correct for the distance overshoot.
+      s_lane =
+          cond(ld.with_s, out_distance, T(ld.lane->length()) - out_distance);
+    }
+  }
+  // Resize the vector of knot points, if necessary.
+  geo_knots.resize(s_breaks.size());
+
+  // Create the resulting piecewise polynomial.
+  return PiecewisePolynomial<T>::Cubic(s_breaks, geo_knots);
+}
+
+template class RoadPath<double>;
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/road_path.h

@@ -0,0 +1,72 @@
+#pragma once
+
+#include "drake/automotive/lane_direction.h"
+#include "drake/automotive/maliput/api/road_geometry.h"
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+#include "drake/common/trajectories/piecewise_polynomial.h"
+
+namespace drake {
+namespace automotive {
+
+/// RoadPath converts a sequence of Maliput Lanes into a PiecewisePolynomial for
+/// the purpose of generating a path for a car to follow.  The path is created
+/// from the start of a user-specified initial lane and direction of travel, and
+/// proceeds in that direction until either the end of the road is reached
+/// (there exist no ongoing lanes), or the given number of specified breaks have
+/// been traversed.  The resulting path is a cubic spline that matches the `r =
+/// 0` coordinate of the lanes at each specified break point.  The resulting
+/// piecewise curve is C2-continuous throughout with zero first and second
+/// derivatives at the start and end of the path.
+///
+/// This class is explicitly instantiated for the following scalar types. No
+/// other scalar types are supported.
+/// - double
+///
+/// @tparam T The vector element type, which must be a valid Eigen scalar.
+///           Only double is supported.
+template <typename T>
+class RoadPath {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RoadPath)
+
+  /// Constructs a single RoadPath from a sequence of Maliput lanes based on the
+  /// following parameters:
+  /// @param initial_lane_direction contains the initial LaneDirection.  This
+  /// must be a valid @p road Lane.
+  /// @param step_size is the size of each step (in the `s`-direction) between
+  /// knot points.
+  /// @param num_breaks are the number of breaks at which the knot points will
+  /// be evaluated.
+  RoadPath(const LaneDirection& initial_lane_direction, const T& step_size,
+           int num_breaks);
+  ~RoadPath();
+
+  const PiecewisePolynomial<T>& get_path() const;
+
+  /// Computes the closest `s`-position on the path to an arbitrary point in
+  /// the world frame of the provided Maliput Lanes.  (Not yet implemented)
+  // TODO(jadecastro): Implement this.
+  const T GetClosestPathPosition(const Vector3<T>& geo_position,
+                                 const T& s_guess) const;
+
+ private:
+  // Traverse the road, starting from an initial LaneDirection, and build a
+  // cubic spline PiecewisePolynomial until 1) a given a number of segments has
+  // been traversed, or 2) the end of the road has been reached.
+  //
+  // If a BranchPoint is encountered in which there is more than one ongoing
+  // lane, the zero-index lane is always selected.
+  // TODO(jadecastro): Use Lane::GetDefaultBranch() to decide the ongoing Lane.
+  const PiecewisePolynomial<T> MakePiecewisePolynomial(
+      const LaneDirection& initial_lane_direction, const T& step_size,
+      int num_breaks) const;
+
+  PiecewisePolynomial<T> path_;  // The path representing the mid-curve of the
+                                 // road.
+  PiecewisePolynomial<T> path_prime_;         // First derivative of path_.
+  PiecewisePolynomial<T> path_double_prime_;  // Second derivative of path_.
+};
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/test/CMakeLists.txt

@@ -37,6 +37,9 @@ target_link_libraries(pose_selector_test drakeAutomotive)
 drake_add_cc_test(idm_controller_test)
 target_link_libraries(idm_controller_test drakeAutomotive)
 
+drake_add_cc_test(road_path_test)
+target_link_libraries(road_path_test drakeAutomotive)
+
 if(lcm_FOUND)
   drake_add_cc_test(automotive_simulator_test)
   target_link_libraries(automotive_simulator_test drakeAutomotiveLcm)