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  /// @throw std::runtime_error if @p linear_tolerance is not a positive number.
44  /// @throw std::runtime_error if @p scale_length is not a positive number.
45  explicit ArcRoadCurve(
46  const Vector2<double>& center, double radius,
47  double theta0, double d_theta,
50  double linear_tolerance, double scale_length,
52  : RoadCurve(linear_tolerance, scale_length,
53  elevation, superelevation,
55  center_(center), radius_(radius), theta0_(theta0), d_theta_(d_theta) {
56  DRAKE_THROW_UNLESS(radius > 0.0);
57  }
58 
59  ~ArcRoadCurve() override = default;
60 
61  Vector2<double> xy_of_p(double p) const override {
62  // The result will be computed with the following function:
63  // [x;y] = center + [radius * cos(θ); radius * sin(θ)]
64  // and:
65  // θ = θ₀ + (p * Δθ)
66  const double theta = theta_of_p(p);
67  return center_ + Vector2<double>(radius_ * std::cos(theta),
68  radius_ * std::sin(theta));
69  }
70 
71  Vector2<double> xy_dot_of_p(double p) const override {
72  // Given:
73  // [x;y] = center + [radius * cos(θ); radius * sin(θ)]
74  // and:
75  // θ = θ₀ + (p * Δθ)
76  // dθ/dp = Δθ
77  // then:
78  // [dx/dp; dy/dp] = [-radius * sin(θ) * dθ/dp; radius * cos(θ) * dθ/dp]
79  // = [-radius * sin(θ) * Δθ; radius * cos(θ) * Δθ]
80  const double theta = theta_of_p(p);
81  return Vector2<double>(-radius_ * std::sin(theta) * d_theta_,
82  radius_ * std::cos(theta) * d_theta_);
83  }
84 
85  double heading_of_p(double p) const override {
86  // Given:
87  // θ = θ_0 + (p * Δθ)
88  // then:
89  // heading = θ + sign(Δθ) * π / 2.0
90  // sign(Δθ) is used to express the increasing-p direction of the tangent.
91  const double theta = theta_of_p(p);
92  return theta + std::copysign(M_PI / 2.0, d_theta_);
93  }
94 
95  double heading_dot_of_p(double p) const override {
96  // Given:
97  // heading = θ + sign(Δθ) * π / 2.0
98  // and:
99  // θ = θ₀ + (p * Δθ)
100  // then:
101  // dheading/dp = dθ/dp = Δθ
102  unused(p);
103  return d_theta_;
104  }
105 
106  double p_scale() const override { return radius_ * std::abs(d_theta_); }
107 
109  const Vector3<double>& geo_coordinate,
110  double r_min, double r_max,
111  const api::HBounds& height_bounds) const override;
112 
113  bool IsValid(double r_min, double r_max,
114  const api::HBounds& height_bounds) const override;
115 
116  private:
117  double FastCalcPFromS(double s, double r) const override;
118 
119  double FastCalcSFromP(double p, double r) const override;
120 
121  double CalcMinimumRadiusAtOffset(double r) const override {
122  return offset_radius(r);
123  }
124 
125  // Computes the radius of the reference arc offset at a distance @p r.
126  // Uses d_theta_'s sign (see ArcRoadCurve() for more details) to add or
127  // or subtract @p r distance.
128  // @param r Lateral offset of the reference curve over the z=0 plane.
129  // @return The reference arc offset radius.
130  double offset_radius(double r) const {
131  return radius_ - std::copysign(1., d_theta_) * r;
132  }
133 
134  // Computes the absolute position along reference arc as an angle in
135  // range [theta0_, (theta0 + d_theta_)],
136  // as a function of parameter @p p (in domain [0, 1]).
137  double theta_of_p(double p) const { return theta0_ + (p * d_theta_); }
138 
139  // Center of rotation in z=0 plane, world coordinates, for the arc reference
140  // curve.
141  const Vector2<double> center_;
142  // The length of the radius of the arc.
143  const double radius_{};
144  // The angular position at which the piece of arc starts.
145  const double theta0_{};
146  // The aperture angle of the arc. Positive values are counter-clockwise.
147  const double d_theta_{};
148 };
149 
150 } // namespace multilane
151 } // namespace maliput
152 } // 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:106
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:120
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:114
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:108
Vector2< double > xy_of_p(double p) const override
Computes the reference curve.
Definition: arc_road_curve.h:61
const double & linear_tolerance() const
Definition: road_curve.h:118
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:32
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:85
const ComputationPolicy & computation_policy() const
Definition: road_curve.h:122
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:14
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:95
ComputationPolicy
A policy to guide all computations in a RoadCurve, in terms of speed and accuracy.
Definition: road_curve.h:49
const CubicPolynomial & superelevation() const
Definition: road_curve.h:116
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:71
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...