Drake
arc_road_curve.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 
9 
10 namespace drake {
11 namespace maliput {
12 namespace multilane {
13 
14 /// RoadCurve specification for a reference curve that describes a piece
15 /// of an arc.
16 class ArcRoadCurve : public RoadCurve {
17  public:
19 
20  /// Constructor. The reference curve is created from the circle `center`, the
21  /// `radius`, initial angular position `theta0` and the angle span
22  /// `d_theta`. `elevation` and `superelevation` polynomials will be used to
23  /// feed RoadGeometry parent class.
24  /// @param center Center of the reference arc.
25  /// @param radius Radius of the reference arc (must be positive).
26  /// @param theta0 Angle of the start point of the reference arc with respect
27  /// to `center` (0 == parallel to x-axis).
28  /// @param d_theta Central angle of the arc, i.e., angular displacement
29  /// from start to end. d_theta > 0 is counter-clockwise.
30  /// @param elevation CubicPolynomial object that represents the elevation
31  /// polynomial. See RoadCurve class constructor for more details.
32  /// @param superelevation CubicPolynomial object that represents the
33  /// superelevation polynomial. See RoadCurve class constructor for more
34  /// details.
35  /// @param linear_tolerance The linear tolerance, in meters, for all
36  /// computations. See RoadCurve class constructor for more details.
37  /// @param scale_length The minimum spatial period of variation in the curve,
38  /// in meters. See RoadCurve class constructor for more details.
39  /// @param computation_policy Policy to guide all computations. If geared
40  /// towards speed, computations will make use of analytical expressions even
41  /// if not actually correct for the curve as specified.
42  /// @throw std::runtime_error if @p radius is not a positive number.
43  explicit ArcRoadCurve(
44  const Vector2<double>& center, double radius,
45  double theta0, double d_theta,
48  double linear_tolerance = 0.01, double scale_length = 1.0,
51  : RoadCurve(linear_tolerance, scale_length,
52  elevation, superelevation,
54  center_(center), radius_(radius), theta0_(theta0), d_theta_(d_theta) {
55  // TODO(hidmic): Remove default values in trailing arguments, which were
56  // added in the first place to defer the need to propagate changes upwards
57  // in the class hierarchy.
58  DRAKE_THROW_UNLESS(radius > 0.0);
59  }
60 
61  ~ArcRoadCurve() override = default;
62 
63  Vector2<double> xy_of_p(double p) const override {
64  // The result will be computed with the following function:
65  // [x;y] = center + [radius * cos(θ); radius * sin(θ)]
66  // and:
67  // θ = θ₀ + (p * Δθ)
68  const double theta = theta_of_p(p);
69  return center_ + Vector2<double>(radius_ * std::cos(theta),
70  radius_ * std::sin(theta));
71  }
72 
73  Vector2<double> xy_dot_of_p(double p) const override {
74  // Given:
75  // [x;y] = center + [radius * cos(θ); radius * sin(θ)]
76  // and:
77  // θ = θ₀ + (p * Δθ)
78  // dθ/dp = Δθ
79  // then:
80  // [dx/dp; dy/dp] = [-radius * sin(θ) * dθ/dp; radius * cos(θ) * dθ/dp]
81  // = [-radius * sin(θ) * Δθ; radius * cos(θ) * Δθ]
82  const double theta = theta_of_p(p);
83  return Vector2<double>(-radius_ * std::sin(theta) * d_theta_,
84  radius_ * std::cos(theta) * d_theta_);
85  }
86 
87  double heading_of_p(double p) const override {
88  // Given:
89  // θ = θ_0 + (p * Δθ)
90  // then:
91  // heading = θ + sign(Δθ) * π / 2.0
92  // sign(Δθ) is used to express the increasing-p direction of the tangent.
93  const double theta = theta_of_p(p);
94  return theta + std::copysign(M_PI / 2.0, d_theta_);
95  }
96 
97  double heading_dot_of_p(double p) const override {
98  // Given:
99  // heading = θ + sign(Δθ) * π / 2.0
100  // and:
101  // θ = θ₀ + (p * Δθ)
102  // then:
103  // dheading/dp = dθ/dp = Δθ
104  unused(p);
105  return d_theta_;
106  }
107 
108  double p_scale() const override { return radius_ * std::abs(d_theta_); }
109 
111  const Vector3<double>& geo_coordinate,
112  double r_min, double r_max,
113  const api::HBounds& height_bounds) const override;
114 
115  bool IsValid(double r_min, double r_max,
116  const api::HBounds& height_bounds) const override;
117 
118  private:
119  double FastCalcPFromS(double s, double r) const override;
120 
121  double FastCalcSFromP(double p, double r) const override;
122 
123  double CalcMinimumRadiusAtOffset(double r) const override {
124  return offset_radius(r);
125  }
126 
127  // Computes the radius of the reference arc offset at a distance @p r.
128  // Uses d_theta_'s sign (see ArcRoadCurve() for more details) to add or
129  // or subtract @p r distance.
130  // @param r Lateral offset of the reference curve over the z=0 plane.
131  // @return The reference arc offset radius.
132  double offset_radius(double r) const {
133  return radius_ - std::copysign(1., d_theta_) * r;
134  }
135 
136  // Computes the absolute position along reference arc as an angle in
137  // range [theta0_, (theta0 + d_theta_)],
138  // as a function of parameter @p p (in domain [0, 1]).
139  double theta_of_p(double p) const { return theta0_ + (p * d_theta_); }
140 
141  // Center of rotation in z=0 plane, world coordinates, for the arc reference
142  // curve.
143  const Vector2<double> center_;
144  // The length of the radius of the arc.
145  const double radius_{};
146  // The angular position at which the piece of arc starts.
147  const double theta0_{};
148  // The aperture angle of the arc. Positive values are counter-clockwise.
149  const double d_theta_{};
150 };
151 
152 } // namespace multilane
153 } // namespace maliput
154 } // namespace drake
double p_scale() const override
Computes the path length integral of the reference curve for the interval [0;1] of p...
Definition: arc_road_curve.h:108
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
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
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:613
Expression abs(const Expression &e)
Definition: symbolic_expression.cc:551
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: arc_road_curve.cc:82
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
const CubicPolynomial & elevation() const
Definition: road_curve.h:112
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:106
Vector2< double > xy_of_p(double p) const override
Computes the reference curve.
Definition: arc_road_curve.h:63
const double & linear_tolerance() const
Definition: road_curve.h:116
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: arc_road_curve.cc:126
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
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.
Definition: arc_road_curve.cc:11
double heading_of_p(double p) const override
Computes the heading of the reference curve.
Definition: arc_road_curve.h:87
const ComputationPolicy & computation_policy() const
Definition: road_curve.h:120
Eigen::AutoDiffScalar< DerType > copysign(const Eigen::AutoDiffScalar< DerType > &x, const T &y)
Overloads copysign from <cmath>.
Definition: autodiff_overloads.h:73
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
double heading_dot_of_p(double p) const override
Computes the first derivative heading of the reference curve.
Definition: arc_road_curve.h:97
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
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:621
RoadCurve specification for a reference curve that describes a piece of an arc.
Definition: arc_road_curve.h:16
#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
Vector2< double > xy_dot_of_p(double p) const override
Computes the first derivative of the reference curve.
Definition: arc_road_curve.h:73
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...