Drake
line_road_curve.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <utility>
5 
12 #include "drake/common/unused.h"
13 
14 namespace drake {
15 namespace maliput {
16 namespace multilane {
17 
18 /// RoadCurve specification for a reference curve that describes a line.
19 class LineRoadCurve : public RoadCurve {
20  public:
22 
23  /// Constructor. Computes a line from @p xy0 as the initial point of the line
24  /// and @p dxy as the difference vector that connects the @p xy0 with the end
25  /// point of the reference curve.
26  /// @param xy0 A 2D vector that represents the first point of the lane.
27  /// @param dxy A 2D difference vector between the last point and @p xy0.
28  /// @param elevation CubicPolynomial object that represents the elevation
29  /// polynomial. See RoadCurve class constructor for more details.
30  /// @param superelevation CubicPolynomial object that represents the
31  /// superelevation polynomial. See RoadCurve class constructor for more
32  /// details.
33  explicit LineRoadCurve(const Vector2<double>& xy0, const Vector2<double>& dxy,
36  : RoadCurve(elevation, superelevation),
37  p0_(xy0),
38  dp_(dxy),
39  heading_(std::atan2(dxy.y(), dxy.x())) {
40  DRAKE_DEMAND(dxy.norm() > kMinimumNorm);
41  }
42 
43  ~LineRoadCurve() override = default;
44 
45  double p_from_s(double s, double r) const override;
46 
47  double s_from_p(double p, double r) const override;
48 
49  Vector2<double> xy_of_p(double p) const override { return p0_ + p * dp_; }
50 
51  Vector2<double> xy_dot_of_p(double p) const override {
52  unused(p);
53  return dp_;
54  }
55 
56  double heading_of_p(double p) const override {
57  unused(p);
58  return heading_;
59  }
60 
61  double heading_dot_of_p(double p) const override {
62  unused(p);
63  return 0.;
64  }
65 
66  double p_scale() const override { return dp_.norm(); }
67 
69  const Vector3<double>& geo_coordinate,
70  double r_min, double r_max,
71  const api::HBounds& height_bounds) const override;
72 
73  bool IsValid(
74  double r_min, double r_max,
75  const api::HBounds& height_bounds) const override {
76  unused(r_min);
77  unused(r_max);
78  unused(height_bounds);
79  return true;
80  }
81 
82  private:
83  // The first point in world coordinates over the z=0 plane of the reference
84  // curve.
85  const Vector2<double> p0_{};
86  // The difference vector that joins the end point of the reference curve with
87  // the first one, p0_.
88  const Vector2<double> dp_{};
89  // The constant angle deviation of dp_ with respect to the x axis of the world
90  // frame.
91  const double heading_{};
92  // The minimum dp_ norm to avoid having issues when computing heading_.
93  static const double kMinimumNorm;
94 };
95 
96 } // namespace multilane
97 } // namespace maliput
98 } // namespace drake
double s_from_p(double p, double r) const override
Computes the path length integral in the interval of the parameter [0; p] and along a parallel curve ...
Definition: line_road_curve.cc:21
Vector2< double > xy_dot_of_p(double p) const override
Computes the first derivative of the reference curve.
Definition: line_road_curve.h:51
int y
Definition: rgbd_renderer_test.cc:33
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.
Definition: automotive_demo.cc:88
RoadCurve specification for a reference curve that describes a line.
Definition: line_road_curve.h:19
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:652
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:98
double heading_dot_of_p(double p) const override
Computes the first derivative heading of the reference curve.
Definition: line_road_curve.h:61
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:92
double heading_of_p(double p) const override
Computes the heading of the reference curve.
Definition: line_road_curve.h:56
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:66
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:31
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:49
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:343
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:73
const CubicPolynomial & superelevation() const
Definition: road_curve.h:100
double p_from_s(double s, double r) const override
Computes the parametric position p along the reference curve corresponding to longitudinal position (...
Definition: line_road_curve.cc:11
#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...