Add RoadPath that converts a sequence of Lane segments into a PiecewisePolynomial

[jadecastro]

This change is 

[jadecastro]

+@rpoyner-tri for feature review, please.
/CC @liangfok - since this extracts LaneDirection from MaliputRailcar.

[rpoyner-tri]

Reviewed 9 of 9 files at r1.
Review status: all files reviewed at latest revision, 2 unresolved discussions, some commit checks failed.

---

drake/automotive/road_path.h, line 3 at r1 (raw file):

#pragma once

#include <vector>

unused.

---

drake/automotive/road_path.h, line 5 at r1 (raw file):

#include <vector>

#include <Eigen/Core>

unused.

---

Comments from Reviewable

[liangfok]

+@liangfok as a voluntary / secondary feature reviewer.

---

Reviewed 9 of 9 files at r1.
Review status: all files reviewed at latest revision, 16 unresolved discussions, some commit checks failed.

---

drake/automotive/BUILD, line 270 at r1 (raw file):

    deps = [
        "//drake/automotive/maliput/api",
        "//drake/automotive:lane_direction",

BTW, this can be ":lane_direction".

---

drake/automotive/BUILD, line 271 at r1 (raw file):

        "//drake/automotive/maliput/api",
        "//drake/automotive:lane_direction",
        "//drake/automotive:monolane_onramp_merge",

BTW, this can be ":monolane_onramp_merge".

---

drake/automotive/BUILD, line 748 at r1 (raw file):

    name = "road_path_test",
    deps = [
        "//drake/automotive:road_path",

BTW, this can be ":road_path".

---

drake/automotive/road_path.cc, line 20 at r1 (raw file):

template <typename T>
RoadPath<T>::RoadPath(const RoadGeometry& road,

Unused parameter.

---

drake/automotive/road_path.cc, line 22 at r1 (raw file):
There shouldn't be a const in front of double.

http://drake.mit.edu/code_style_guide.html#clarifications

You must not use const in a function declaration where it adds no meaning. That occurs in pass-by-value parameter declarations, where const int i and int i mean the same thing, and in return-by-value declarations, where int f() and const int f() are also synonymous. You may add const to such parameter declarations in the function definition, where it does indicate that the implementation will not modify its own copy of the parameter value. The C++ standard explicitly states that the signatures are identical with or without the const in these cases, see Overloadable declarations. (This applies to volatile also.)

---

drake/automotive/road_path.cc, line 38 at r1 (raw file):

template <typename T>
const T RoadPath<T>::GetClosestPathPosition(const Vector3<T>& geo_pos,
                                            const double s_guess) const {

There shouldn't be a const in front of double.

http://drake.mit.edu/code_style_guide.html#clarifications

---

drake/automotive/road_path.cc, line 43 at r1 (raw file):

template <typename T>
const PiecewisePolynomial<T> RoadPath<T>::MakePiecewisePolynomial(

This method does not appear to modify any member variables. Consider making it const or a stand-alone method in an anonymous namespace.

---

drake/automotive/road_path.cc, line 44 at r1 (raw file):

template <typename T>
const PiecewisePolynomial<T> RoadPath<T>::MakePiecewisePolynomial(
    const LaneDirection& initial_lane_direction, const double step_size,

There shouldn't be a const in front of double.

http://drake.mit.edu/code_style_guide.html#clarifications

---

drake/automotive/road_path.cc, line 59 at r1 (raw file):

    GeoPosition geo_pos =
        ld.lane->ToGeoPosition({s_lane /* s */, 0. /* r */, 0. /* h */});

BTW, will non-zero r eventually be supported?

---

drake/automotive/road_path.h, line 20 at r1 (raw file):

/// 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

BTW, upon first read, I did not understand what a "break" was. Consider using "branches" since that's what they're called in Maliput.

http://drake.mit.edu/doxygen_cxx/classdrake_1_1maliput_1_1api_1_1_lane.html#a526f27f32359b74308337f498f7dac21

---

drake/automotive/road_path.h, line 22 at r1 (raw file):

/// (there exist no ongoing lanes), or the given number of specified breaks have
/// been traversed.  The resulting path is a cubic spline that exactly matches
/// the `s = 0` coordinate of the lanes at each specified break point.

Should r = 0 too? Are there also intermediate points between those at branch points?

---

drake/automotive/road_path.h, line 50 at r1 (raw file):

  const PiecewisePolynomial<T>& get_path() const;

  /// Computes the closest path position of an arbitrary point in Cartesian

Is "path position" defined?

---

drake/automotive/road_path.h, line 63 at r1 (raw file):

  // If a BranchPoint is encountered in which there is more than one ongoing
  // lane, the zero-index lane is always selected.
  // TODO(jadecastro): Use DefaultBranch to decide the ongoing Lane.

BTW, "DefaultBranch" should be "Lane::GetDefaultBranch()":

http://drake.mit.edu/doxygen_cxx/classdrake_1_1maliput_1_1api_1_1_lane.html#a7a8c9b11086e5f381cb00fe7b5c64056

---

drake/automotive/test/road_path_test.cc, line 111 at r1 (raw file):

  expected_value << 0., 0., 0.;
  actual_value = path.get_path().value(0.);
  EXPECT_TRUE(CompareMatrices(expected_value, actual_value, 1e-2));

BTW, wow, that's a large tolerance of 1cm. How was this tolerance selected?

---

Comments from Reviewable

[jadecastro]

As per a 1-on-1 conversation with Rico, I have reduced the number of breaks from 1000 to 200 in the unit test - this effectively decreased the unit test's run time from 61 sec down to 1 sec at little loss in precision!

---

Review status: all files reviewed at latest revision, 16 unresolved discussions, some commit checks failed.

---

drake/automotive/road_path.h, line 3 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

unused.

Done.

---

drake/automotive/road_path.h, line 5 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

unused.

Done.

---

drake/automotive/road_path.h, line 22 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Should r = 0 too? Are there also intermediate points between those at branch points?

Yes. Fixed the comment. There are no intermediate points between those at branch points. There's smarter ways to discretize the lane, and this is meant only as a first-pass.

---

drake/automotive/test/road_path_test.cc, line 111 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

BTW, wow, that's a large tolerance of 1cm. How was this tolerance selected?

There's inevitably interpolation error with a curve fit; that's why 1 cm was used. I've since reduced it to 0.1 cm since that appears to work.

---

Comments from Reviewable

[liangfok]

Reviewed 2 of 2 files at r2.
Review status: all files reviewed at latest revision, 14 unresolved discussions, some commit checks failed.

---

Comments from Reviewable

[rpoyner-tri]

---

Review status: all files reviewed at latest revision, 12 unresolved discussions, some commit checks failed.

---

Comments from Reviewable

[jadecastro]

Review status: all files reviewed at latest revision, 12 unresolved discussions, some commit checks failed.

---

drake/automotive/BUILD, line 270 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

BTW, this can be ":lane_direction".

Done.

---

drake/automotive/BUILD, line 271 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

BTW, this can be ":monolane_onramp_merge".

Done.

---

drake/automotive/BUILD, line 748 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

BTW, this can be ":road_path".

Done.

---

drake/automotive/road_path.cc, line 20 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Unused parameter.

Done.

---

drake/automotive/road_path.cc, line 22 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

There shouldn't be a const in front of double.

http://drake.mit.edu/code_style_guide.html#clarifications

You must not use const in a function declaration where it adds no meaning. That occurs in pass-by-value parameter declarations, where const int i and int i mean the same thing, and in return-by-value declarations, where int f() and const int f() are also synonymous. You may add const to such parameter declarations in the function definition, where it does indicate that the implementation will not modify its own copy of the parameter value. The C++ standard explicitly states that the signatures are identical with or without the const in these cases, see Overloadable declarations. (This applies to volatile also.)

Done.

---

drake/automotive/road_path.cc, line 38 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

There shouldn't be a const in front of double.

http://drake.mit.edu/code_style_guide.html#clarifications

Done.

---

drake/automotive/road_path.cc, line 43 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

This method does not appear to modify any member variables. Consider making it const or a stand-alone method in an anonymous namespace.

Done.

---

drake/automotive/road_path.cc, line 44 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

There shouldn't be a const in front of double.

http://drake.mit.edu/code_style_guide.html#clarifications

Done.

---

drake/automotive/road_path.cc, line 59 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

BTW, will non-zero r eventually be supported?

It's meant to find a path corresponding to the center of a sequence of lanes.

---

drake/automotive/road_path.h, line 20 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

BTW, upon first read, I did not understand what a "break" was. Consider using "branches" since that's what they're called in Maliput.

http://drake.mit.edu/doxygen_cxx/classdrake_1_1maliput_1_1api_1_1_lane.html#a526f27f32359b74308337f498f7dac21

But the knot points are not generated at the branch points currently, so this term wouldn't be the correct one to use.
Also, "breaks" are terms used in cubic spline curve fitting (see https://github.com/RobotLocomotion/drake/blob/master/drake/common/trajectories/piecewise_polynomial.h#L101 as one example.)

---

drake/automotive/road_path.h, line 50 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Is "path position" defined?

Done.

---

drake/automotive/road_path.h, line 63 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

BTW, "DefaultBranch" should be "Lane::GetDefaultBranch()":

http://drake.mit.edu/doxygen_cxx/classdrake_1_1maliput_1_1api_1_1_lane.html#a7a8c9b11086e5f381cb00fe7b5c64056

Done.

---

Comments from Reviewable

[liangfok]

---

Reviewed 4 of 4 files at r3, 1 of 1 files at r4.
Review status: all files reviewed at latest revision, all discussions resolved.

---

Comments from Reviewable

[jadecastro]

+@sherm1 for platform review, please.
+@jwnimmer-tri for car review, please

[jwnimmer-tri]

🚗 vroom

---

Reviewed 2 of 9 files at r1, 1 of 4 files at r3.
Review status: all files reviewed at latest revision, all discussions resolved.

---

Comments from Reviewable

[sherm1]

Checkpoint.

---

Reviewed 5 of 9 files at r1, 3 of 4 files at r3.
Review status: all files reviewed at latest revision, 8 unresolved discussions.

---

drake/automotive/road_path.h, line 18 at r4 (raw file):

/// (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.

BTW is that a sufficient specification of the path? I think there are additional dofs left unspecified for smoothness and treatment of slope and curvature at the end points. Please consider fully specifying the path here.

---

drake/automotive/road_path.h, line 31 at r4 (raw file):

  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RoadPath)

  /// Constructor.

BTW should have a "brief" sentence here -- just "constructor" isn't helpful. (Doxygen shows the first sentence in the method list for the class.) Something like "The constructor performs the translation from Maliput lanes to a smooth path."

---

drake/automotive/road_path.h, line 46 at r4 (raw file):

  /// Computes the closest `s`-position on the path to an arbitrary point in
  /// Cartesian space.

BTW this should either be (a) removed, (b) left out of doxygen (can use @cond/@endcond), or (c) noted in the doxygen that the method hasn't been implemented yet. (I mildly prefer (c) so the API intent is visible.)

---

drake/automotive/road_path.h, line 46 at r4 (raw file):

  /// Computes the closest `s`-position on the path to an arbitrary point in
  /// Cartesian space.

BTW "Cartesian space" seems ambiguous here. Is there a World frame used by Maliput from which this position is measured and in which the vector is expressed?

---

drake/automotive/road_path.h, line 66 at r4 (raw file):

                                   // road.
  PiecewisePolynomial<T> path_prime_{};         // First derivative of path_.
  PiecewisePolynomial<T> path_double_prime_{};  // Second derivative of path_.

BTW, mustaches not necessary here since these are class objects with default constructors.

---

drake/automotive/test/road_path_test.cc, line 36 at r4 (raw file):

const double kCurvedRoadRadius{10};
const double kCurvedRoadTheta{M_PI_2};
const double kCurvedRoadLength{kCurvedRoadRadius * M_PI / 2.};

BTW M_PI_2 since you used it on the previous line.

---

drake/automotive/test/road_path_test.cc, line 105 at r4 (raw file):

                       200);             /* num_breaks */
  ASSERT_LE(kTotalRoadLength,
            path.get_path().getEndTime() - path.get_path().getStartTime());

BTW getEndTime() and getStartTime() aren't styleguide compliant. Is that 3rd party code?

---

drake/automotive/test/road_path_test.cc, line 112 at r4 (raw file):

  expected_value << 0., 0., 0.;
  actual_value = path.get_path().value(0.);
  EXPECT_TRUE(CompareMatrices(expected_value, actual_value, 1e-3));

BTW can you explain in a comment why 1e-3 is the right tolerance?

---

Comments from Reviewable

[sherm1]

Platform with a few BTW suggestions to consider first.

---

Reviewed 1 of 1 files at r4.
Review status: all files reviewed at latest revision, 11 unresolved discussions.

---

drake/automotive/road_path.cc, line 27 at r4 (raw file):

                                    num_breaks)),
      path_prime_(path_.derivative()),
      path_double_prime_(path_.derivative(2)) {}

BTW, derivative(1) and derivative(2) would be slightly clearer rather than depending on the default.

---

drake/automotive/road_path.cc, line 48 at r4 (raw file):

    int num_breaks) const {
  std::vector<T> s_breaks{};
  std::vector<MatrixX<T>> geo_knots(num_breaks, MatrixX<T>::Zero(3, 1));

BTW, shouldn't this be an std::vector<Vector3<T>>? As it is each element is heap allocated and variable size.

---

drake/automotive/road_path.cc, line 100 at r4 (raw file):

  // Resize the data structure with the knot points, if necessary.
  for (int i = 0; i < num_breaks - static_cast<int>(s_breaks.size()); ++i) {
    geo_knots.pop_back();

BTW, if you can use fixed-size 3-vector elements, then consider using std::vector::reserve() initially and then push_back or emplace_back so that the vector ends up the right size at the end. You could even arrange to give back the extra heap space (if you care) using shrink_to_fit().

---

Comments from Reviewable

[jadecastro]

Review status: 6 of 9 files reviewed at latest revision, 11 unresolved discussions.

---

drake/automotive/road_path.cc, line 27 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW, derivative(1) and derivative(2) would be slightly clearer rather than depending on the default.

Done.

---

drake/automotive/road_path.cc, line 48 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW, shouldn't this be an std::vector<Vector3<T>>? As it is each element is heap allocated and variable size.

Unfortunately, the arguments to PiecewisePolynomial<T>::Cubic() that consumes this vector do not support fixed-size vectors, preventing us from having std::vector<Vector3<T>> here.

---

drake/automotive/road_path.cc, line 100 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW, if you can use fixed-size 3-vector elements, then consider using std::vector::reserve() initially and then push_back or emplace_back so that the vector ends up the right size at the end. You could even arrange to give back the extra heap space (if you care) using shrink_to_fit().

Didn't fix this one, it looks as though we're bound to dynamically-sized elements. However, I did clean up the resize section below.

---

drake/automotive/road_path.h, line 18 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW is that a sufficient specification of the path? I think there are additional dofs left unspecified for smoothness and treatment of slope and curvature at the end points. Please consider fully specifying the path here.

Done.

---

drake/automotive/road_path.h, line 31 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW should have a "brief" sentence here -- just "constructor" isn't helpful. (Doxygen shows the first sentence in the method list for the class.) Something like "The constructor performs the translation from Maliput lanes to a smooth path."

Done.

---

drake/automotive/road_path.h, line 46 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this should either be (a) removed, (b) left out of doxygen (can use @cond/@endcond), or (c) noted in the doxygen that the method hasn't been implemented yet. (I mildly prefer (c) so the API intent is visible.)

Done.

---

drake/automotive/road_path.h, line 46 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW "Cartesian space" seems ambiguous here. Is there a World frame used by Maliput from which this position is measured and in which the vector is expressed?

Done.

---

drake/automotive/road_path.h, line 66 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW, mustaches not necessary here since these are class objects with default constructors.

Done.

---

drake/automotive/test/road_path_test.cc, line 36 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW M_PI_2 since you used it on the previous line.

Done.

---

drake/automotive/test/road_path_test.cc, line 105 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW getEndTime() and getStartTime() aren't styleguide compliant. Is that 3rd party code?

This is from PiecewisePolynomial within Drake (see https://github.com/RobotLocomotion/drake/blob/master/drake/common/trajectories/piecewise_polynomial.h)

It looks like other files within the trajectory folder are not compliant either. On a quick search, I didn't come across any open issues addressing a fix to this.

---

drake/automotive/test/road_path_test.cc, line 112 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW can you explain in a comment why 1e-3 is the right tolerance?

Done.

---

Comments from Reviewable

[rpoyner-tri]

Reviewed 3 of 3 files at r5.
Review status: all files reviewed at latest revision, 3 unresolved discussions.

---

Comments from Reviewable

[sherm1]

All good -- thanks!

---

Reviewed 3 of 3 files at r5.
Review status: all files reviewed at latest revision, 1 unresolved discussion, some commit checks failed.

---

drake/automotive/road_path.cc, line 98 at r5 (raw file):

    }
  }
  // Resize the vecotor of knot points, if necessary.

BTW, typo vecotor

---

Comments from Reviewable

[liangfok]

Reviewed 3 of 3 files at r5.
Review status: all files reviewed at latest revision, 2 unresolved discussions, some commit checks failed.

---

drake/automotive/road_path.h, line 35 at r5 (raw file):

  /// Constructs a single RoadPath from a sequence of Maliput lanes based on the
  /// following parameters:
  /// @param road is the RoadGeometry from which a path is generated.

This parameter no longer exists.

---

Comments from Reviewable

[jadecastro]

Review status: 6 of 9 files reviewed at latest revision, 2 unresolved discussions.

---

drake/automotive/road_path.cc, line 98 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW, typo vecotor

Done.

---

drake/automotive/road_path.h, line 35 at r5 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

This parameter no longer exists.

Done.

---

Comments from Reviewable

[liangfok]

Reviewed 3 of 3 files at r6.
Review status: all files reviewed at latest revision, all discussions resolved, some commit checks pending.

---

Comments from Reviewable