Drake
line_road_curve.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <limits>
5 #include <utility>
6 
13 #include "drake/common/unused.h"
14 
15 namespace drake {
16 namespace maliput {
17 namespace multilane {
18 
19 /// RoadCurve specification for a reference curve that describes a line.
20 class LineRoadCurve : public RoadCurve {
21  public:
23 
24  /// Constructor. Computes a line from @p xy0 as the initial point of the line
25  /// and @p dxy as the difference vector that connects the @p xy0 with the end
26  /// point of the reference curve.
27  /// @param xy0 A 2D vector that represents the first point of the lane.
28  /// @param dxy A 2D difference vector between the last point and @p xy0.
29  /// @param elevation CubicPolynomial object that represents the elevation
30  /// polynomial. See RoadCurve class constructor for more details.
31  /// @param superelevation CubicPolynomial object that represents the
32  /// superelevation polynomial. See RoadCurve class constructor for more
33  /// details.
34  /// @param linear_tolerance The linear tolerance, in meters, for all
35  /// computations. See RoadCurve class constructor for more details.
36  /// @param scale_length The minimum spatial period of variation in the curve,
37  /// in meters. See RoadCurve class constructor for more details.
38  /// @param computation_policy Policy to guide all computations. If geared
39  /// towards speed, computations will make use of analytical expressions even
40  /// if not actually correct for the curve as specified.
41  explicit LineRoadCurve(
42  const Vector2<double>& xy0, const Vector2<double>& dxy,
45  double linear_tolerance = 0.01, double scale_length = 1.0,
48  : RoadCurve(linear_tolerance, scale_length,
49  elevation, superelevation,
51  p0_(xy0), dp_(dxy), heading_(std::atan2(dxy.y(), dxy.x())) {
52  // TODO(hidmic): Remove default values in trailing arguments, which were
53  // added in the first place to defer the need to propagate changes upwards
54  // in the class hierarchy.
55  DRAKE_DEMAND(dxy.norm() > kMinimumNorm);
56  }
57 
58  ~LineRoadCurve() override = default;
59 
60  Vector2<double> xy_of_p(double p) const override { return p0_ + p * dp_; }
61 
62  Vector2<double> xy_dot_of_p(double p) const override {
63  unused(p);
64  return dp_;
65  }
66 
67  double heading_of_p(double p) const override {
68  unused(p);
69  return heading_;
70  }
71 
72  double heading_dot_of_p(double p) const override {
73  unused(p);
74  return 0.;
75  }
76 
77  double p_scale() const override { return dp_.norm(); }
78 
80  const Vector3<double>& geo_coordinate,
81  double r_min, double r_max,
82  const api::HBounds& height_bounds) const override;
83 
84  bool IsValid(
85  double r_min, double r_max,
86  const api::HBounds& height_bounds) const override {
87  unused(r_min);
88  unused(r_max);
89  unused(height_bounds);
90  return true;
91  }
92 
93  private:
94  double FastCalcPFromS(double s, double r) const override;
95 
96  double FastCalcSFromP(double p, double r) const override;
97 
98  double CalcMinimumRadiusAtOffset(double r) const override {
99  unused(r);
100  return std::numeric_limits<double>::infinity();
101  }
102 
103  // The first point in world coordinates over the z=0 plane of the reference
104  // curve.
105  const Vector2<double> p0_{};
106  // The difference vector that joins the end point of the reference curve with
107  // the first one, p0_.
108  const Vector2<double> dp_{};
109  // The constant angle deviation of dp_ with respect to the x axis of the world
110  // frame.
111  const double heading_{};
112  // The minimum dp_ norm to avoid having issues when computing heading_.
113  static const double kMinimumNorm;
114 };
115 
116 } // namespace multilane
117 } // namespace maliput
118 } // namespace drake
Vector2< double > xy_dot_of_p(double p) const override
Computes the first derivative of the reference curve.
Definition: line_road_curve.h:62
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
std::vector< Number > x
Definition: ipopt_solver.cc:150
Provides a convenient wrapper to throw an exception when a condition is unmet.
const double & scale_length() const
Definition: road_curve.h:118
Definition: automotive_demo.cc:90
RoadCurve specification for a reference curve that describes a line.
Definition: line_road_curve.h:20
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:666
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
STL namespace.
const CubicPolynomial & elevation() const
Definition: road_curve.h:112
double heading_dot_of_p(double p) const override
Computes the first derivative heading of the reference curve.
Definition: line_road_curve.h:72
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:106
const double & linear_tolerance() const
Definition: road_curve.h:116
double heading_of_p(double p) const override
Computes the heading of the reference curve.
Definition: line_road_curve.h:67
const api::HBounds height_bounds
Definition: multilane_arc_road_curve_test.cc:26
Always prefer accurate results, even if at a slower pace (e.g.
double y
Definition: vtk_util_test.cc:26
Definition: arc_road_curve.cc:11
double p_scale() const override
Computes the path length integral of the reference curve for the interval [0;1] of p...
Definition: line_road_curve.h:77
const ComputationPolicy & computation_policy() const
Definition: road_curve.h:120
Vector3< double > ToCurveFrame(const Vector3< double > &geo_coordinate, double r_min, double r_max, const api::HBounds &height_bounds) const override
Converts a geo_coordinate in the world frame to the composed curve frame, i.e., the superposition of ...
Definition: line_road_curve.cc:22
Provides Drake&#39;s assertion implementation.
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
Vector2< double > xy_of_p(double p) const override
Computes the reference curve.
Definition: line_road_curve.h:60
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: cubic_polynomial.h:13
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:360
bool IsValid(double r_min, double r_max, const api::HBounds &height_bounds) const override
Checks that there are no self-intersections (singularities) in the volume created by applying the con...
Definition: line_road_curve.h:84
ComputationPolicy
A policy to guide all computations in a RoadCurve, in terms of speed and accuracy.
Definition: road_curve.h:47
const CubicPolynomial & superelevation() const
Definition: road_curve.h:114
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:51
Provides careful macros to selectively enable or disable the special member functions for copy-constr...