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  /// @throw std::runtime_error if @p linear_tolerance is not a positive number.
42  /// @throw std::runtime_error if @p scale_length is not a positive number.
43  explicit LineRoadCurve(
44  const Vector2<double>& xy0, const Vector2<double>& dxy,
47  double linear_tolerance, double scale_length,
49  : RoadCurve(linear_tolerance, scale_length,
50  elevation, superelevation,
52  p0_(xy0), dp_(dxy), heading_(std::atan2(dxy.y(), dxy.x())) {
53  DRAKE_DEMAND(dxy.norm() > kMinimumNorm);
54  }
55 
56  ~LineRoadCurve() override = default;
57 
58  Vector2<double> xy_of_p(double p) const override { return p0_ + p * dp_; }
59 
60  Vector2<double> xy_dot_of_p(double p) const override {
61  unused(p);
62  return dp_;
63  }
64 
65  double heading_of_p(double p) const override {
66  unused(p);
67  return heading_;
68  }
69 
70  double heading_dot_of_p(double p) const override {
71  unused(p);
72  return 0.;
73  }
74 
75  double l_max() const override { return dp_.norm(); }
76 
78  const Vector3<double>& geo_coordinate,
79  double r_min, double r_max,
80  const api::HBounds& height_bounds) const override;
81 
82  bool IsValid(
83  double r_min, double r_max,
84  const api::HBounds& height_bounds) const override {
85  unused(r_min);
86  unused(r_max);
87  unused(height_bounds);
88  return true;
89  }
90 
91  private:
92  double FastCalcPFromS(double s, double r) const override;
93 
94  double FastCalcSFromP(double p, double r) const override;
95 
96  double CalcMinimumRadiusAtOffset(double r) const override {
97  unused(r);
98  return std::numeric_limits<double>::infinity();
99  }
100 
101  // The first point in world coordinates over the z=0 plane of the reference
102  // curve.
103  const Vector2<double> p0_{};
104  // The difference vector that joins the end point of the reference curve with
105  // the first one, p0_.
106  const Vector2<double> dp_{};
107  // The constant angle deviation of dp_ with respect to the x axis of the world
108  // frame.
109  const double heading_{};
110  // The minimum dp_ norm to avoid having issues when computing heading_.
111  static const double kMinimumNorm;
112 };
113 
114 } // namespace multilane
115 } // namespace maliput
116 } // namespace drake
double l_max() const override
Computes the path length integral of the reference curve for the whole [0; 1] interval of p...
Definition: line_road_curve.h:75
Vector2< double > xy_dot_of_p(double p) const override
Computes the first derivative of the reference curve.
Definition: line_road_curve.h:60
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:121
Definition: bullet_model.cc:22
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:665
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:115
double heading_dot_of_p(double p) const override
Computes the first derivative heading of the reference curve.
Definition: line_road_curve.h:70
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:109
const double & linear_tolerance() const
Definition: road_curve.h:119
double heading_of_p(double p) const override
Computes the heading of the reference curve.
Definition: line_road_curve.h:65
const api::HBounds height_bounds
Definition: multilane_arc_road_curve_test.cc:32
double y
Definition: vtk_util_test.cc:26
Definition: arc_road_curve.cc:11
const ComputationPolicy & computation_policy() const
Definition: road_curve.h:123
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:58
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: cubic_polynomial.h:14
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:82
ComputationPolicy
A policy to guide all computations in a RoadCurve, in terms of speed and accuracy.
Definition: road_curve.h:50
const CubicPolynomial & superelevation() const
Definition: road_curve.h:117
#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...