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 
16 class ArcRoadCurve : public RoadCurve {
17  public:
19 
20 
21  explicit ArcRoadCurve(const Vector2<double>& center, double radius,
37  double theta0, double d_theta,
40  : RoadCurve(elevation, superelevation),
41  center_(center),
42  radius_(radius),
43  theta0_(theta0),
44  d_theta_(d_theta) {
45  DRAKE_THROW_UNLESS(radius > 0.0);
46  }
47 
48  ~ArcRoadCurve() override = default;
49 
50  Vector2<double> xy_of_p(double p) const override {
51  // The result will be computed with the following function:
52  // [x;y] = center + [radius * cos(θ); radius * sin(θ)]
53  // and:
54  // θ = θ₀ + (p * Δθ)
55  const double theta = theta_of_p(p);
56  return center_ + Vector2<double>(radius_ * std::cos(theta),
57  radius_ * std::sin(theta));
58  }
59 
60  Vector2<double> xy_dot_of_p(double p) const override {
61  // Given:
62  // [x;y] = center + [radius * cos(θ); radius * sin(θ)]
63  // and:
64  // θ = θ₀ + (p * Δθ)
65  // dθ/dp = Δθ
66  // then:
67  // [dx/dp; dy/dp] = [-radius * sin(θ) * dθ/dp; radius * cos(θ) * dθ/dp]
68  // = [-radius * sin(θ) * Δθ; radius * cos(θ) * Δθ]
69  const double theta = theta_of_p(p);
70  return Vector2<double>(-radius_ * std::sin(theta) * d_theta_,
71  radius_ * std::cos(theta) * d_theta_);
72  }
73 
74  double heading_of_p(double p) const override {
75  // Given:
76  // θ = θ_0 + (p * Δθ)
77  // then:
78  // heading = θ + sign(Δθ) * π / 2.0
79  // sign(Δθ) is used to express the increasing-p direction of the tangent.
80  const double theta = theta_of_p(p);
81  return theta + std::copysign(M_PI / 2.0, d_theta_);
82  }
83 
84  double heading_dot_of_p(double p) const override {
85  // Given:
86  // heading = θ + sign(Δθ) * π / 2.0
87  // and:
88  // θ = θ₀ + (p * Δθ)
89  // then:
90  // dheading/dp = dθ/dp = Δθ
91  unused(p);
92  return d_theta_;
93  }
94 
95  double p_scale() const override { return radius_ * std::abs(d_theta_); }
96 
98  const Vector3<double>& geo_coordinate,
99  const api::RBounds& lateral_bounds,
100  const api::HBounds& height_bounds) const override;
101 
109  bool IsValid(
110  const api::RBounds& lateral_bounds,
111  const api::HBounds& height_bounds) const override;
112 
113  private:
114  // Computes the absolute position along reference arc as an angle in
115  // range [theta0_, (theta0 + d_theta_)],
116  // as a function of parameter @p p (in domain [0, 1]).
117  double theta_of_p(double p) const { return theta0_ + (p * d_theta_); }
118 
119  // Center of rotation in z=0 plane, world coordinates, for the arc reference
120  // curve.
121  const Vector2<double> center_;
122  // The length of the radius of the arc.
123  const double radius_{};
124  // The angular position at which the piece of arc starts.
125  const double theta0_{};
126  // The aperture angle of the arc. Positive values are counter-clockwise.
127  const double d_theta_{};
128 };
129 
130 } // namespace multilane
131 } // namespace maliput
132 } // 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:95
ArcRoadCurve(const ArcRoadCurve &)=delete
Vector3< double > ToCurveFrame(const Vector3< double > &geo_coordinate, const api::RBounds &lateral_bounds, 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:65
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.
Definition: automotive_demo.cc:88
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
Expression abs(const Expression &e)
Definition: symbolic_expression.cc:537
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:30
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:24
Vector2< double > xy_of_p(double p) const override
Computes the reference curve.
Definition: arc_road_curve.h:50
#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
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:74
bool IsValid(const api::RBounds &lateral_bounds, const api::HBounds &height_bounds) const override
Evaluates extrema in superelevation polynomial to verify that for the given lateral_bounds the surfac...
Definition: arc_road_curve.cc:110
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:274
Eigen::AutoDiffScalar< DerType > copysign(const Eigen::AutoDiffScalar< DerType > &x, const T &y)
Overloads copysign from <cmath>.
Definition: autodiff_overloads.h:69
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:309
double heading_dot_of_p(double p) const override
Computes the first derivative heading of the reference curve.
Definition: arc_road_curve.h:84
RoadCurve(const RoadCurve &)=delete
const CubicPolynomial & superelevation() const
Definition: road_curve.h:32
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:607
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:35
Vector2< double > xy_dot_of_p(double p) const override
Computes the first derivative of the reference curve.
Definition: arc_road_curve.h:60
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:53
Provides careful macros to selectively enable or disable the special member functions for copy-constr...