Drake
lane.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 
13 #include "drake/common/unused.h"
14 
15 namespace drake {
16 namespace maliput {
17 namespace multilane {
18 
20 
21 typedef Vector2<double> V2;
23 
24 /// Base class for the multilane implementation of api::Lane.
25 class Lane : public api::Lane {
26  public:
28 
29  /// Constructs a Lane.
30  ///
31  /// @param id the ID
32  /// @param segment the Segment to which this Lane will belong, which must
33  /// remain valid for the lifetime of this class
34  /// @param index Lane's index to identify it when querying parent @p segment.
35  /// It must be positive.
36  /// @param lane_bounds nominal bounds of the lane, uniform along the entire
37  /// reference path, which must be a subset of @p driveable_bounds
38  /// @param driveable_bounds driveable bounds of the lane, uniform along the
39  /// entire reference path
40  /// @param elevation_bounds elevation bounds of the lane, uniform along the
41  /// entire driveable surface
42  /// @param road_curve The trajectory of the Lane over parent @p segment's
43  /// surface.
44  /// @param r0 The lateral displacement with respect to the @p road_curve's
45  /// reference curve.
46  ///
47  /// N.B. The override Lane::ToLanePosition() is currently restricted to lanes
48  /// in which superelevation and elevation change are both zero.
49  Lane(const api::LaneId& id, const api::Segment* segment,
50  int index,
51  const api::RBounds& lane_bounds,
52  const api::RBounds& driveable_bounds,
53  const api::HBounds& elevation_bounds,
54  const RoadCurve* road_curve,
55  double r0)
56  : id_(id), segment_(segment),
57  index_(index),
58  lane_bounds_(lane_bounds),
59  driveable_bounds_(driveable_bounds),
60  elevation_bounds_(elevation_bounds),
61  road_curve_(road_curve),
62  r0_(r0) {
63  DRAKE_DEMAND(index_ >= 0);
64  DRAKE_DEMAND(lane_bounds_.min() >= driveable_bounds_.min());
65  DRAKE_DEMAND(lane_bounds_.max() <= driveable_bounds_.max());
66  DRAKE_DEMAND(road_curve != nullptr);
67  }
68 
69  // TODO(maddog-tri) Allow superelevation to have a center-of-rotation
70  // which is different from r = 0.
71 
72  const CubicPolynomial& elevation() const {
73  return road_curve_->elevation();
74  }
75 
77  return road_curve_->superelevation();
78  }
79 
80  double r0() const { return r0_; }
81 
82  void SetStartBp(BranchPoint* bp) { start_bp_ = bp; }
83  void SetEndBp(BranchPoint* bp) { end_bp_ = bp; }
84 
85  BranchPoint* start_bp() { return start_bp_; }
86 
87  BranchPoint* end_bp() { return end_bp_; }
88 
89  ~Lane() override = default;
90 
91  private:
92  const api::LaneId do_id() const override { return id_; }
93 
94  const api::Segment* do_segment() const override;
95 
96  int do_index() const override { return index_; }
97 
98  const api::Lane* do_to_left() const override {
99  if (index_ == (segment_->num_lanes() - 1)) {
100  return nullptr;
101  } else {
102  return segment_->lane(index_ + 1);
103  }
104  }
105 
106  const api::Lane* do_to_right() const override {
107  if (index_ == 0) {
108  return nullptr;
109  } else {
110  return segment_->lane(index_ - 1);
111  }
112  }
113 
114  const api::BranchPoint* DoGetBranchPoint(
115  const api::LaneEnd::Which which_end) const override;
116 
117  const api::LaneEndSet* DoGetConfluentBranches(
118  const api::LaneEnd::Which which_end) const override;
119 
120  const api::LaneEndSet* DoGetOngoingBranches(
121  const api::LaneEnd::Which which_end) const override;
122 
123  optional<api::LaneEnd> DoGetDefaultBranch(
124  const api::LaneEnd::Which which_end) const override;
125 
126  double do_length() const override;
127 
128  api::RBounds do_lane_bounds(double) const override { return lane_bounds_; }
129 
130  api::RBounds do_driveable_bounds(double) const override {
131  return driveable_bounds_;
132  }
133 
134  api::HBounds do_elevation_bounds(double, double) const override {
135  return elevation_bounds_;
136  }
137 
138  api::GeoPosition DoToGeoPosition(
139  const api::LanePosition& lane_pos) const override;
140 
141  api::Rotation DoGetOrientation(
142  const api::LanePosition& lane_pos) const override;
143 
144  api::LanePosition DoEvalMotionDerivatives(
146  const api::IsoLaneVelocity& velocity) const override;
147 
148  api::LanePosition DoToLanePosition(
149  const api::GeoPosition& geo_position,
150  api::GeoPosition* nearest_position,
151  double* distance) const override;
152 
153  const api::LaneId id_;
154  const api::Segment* segment_{};
155  const int index_{};
156  BranchPoint* start_bp_{};
157  BranchPoint* end_bp_{};
158 
159  const api::RBounds lane_bounds_;
160  const api::RBounds driveable_bounds_;
161  const api::HBounds elevation_bounds_;
162  const RoadCurve* road_curve_{};
163  const double r0_;
164 };
165 
166 
167 } // namespace multilane
168 } // namespace maliput
169 } // namespace drake
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
A BranchPoint is a node in the network of a RoadGeometry at which Lanes connect to one another...
Definition: branch_point.h:67
Definition: automotive_demo.cc:88
const double position
Definition: robot_plan_interpolator_test.cc:65
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
void SetEndBp(BranchPoint *bp)
Definition: lane.h:83
const CubicPolynomial & elevation() const
Definition: road_curve.h:98
BranchPoint * end_bp()
Definition: lane.h:87
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:92
An implementation of api::BranchPoint.
Definition: branch_point.h:42
Base class for the multilane implementation of api::Lane.
Definition: lane.h:25
int num_lanes() const
Returns the number of Lanes contained in this Segment.
Definition: segment.h:42
A set of LaneEnds.
Definition: branch_point.h:23
A Lane represents a lane of travel in a road network.
Definition: lane.h:34
stx::optional< T > optional
Definition: drake_optional.h:14
const CubicPolynomial & superelevation() const
Definition: lane.h:76
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:266
Definition: arc_road_curve.cc:11
double max() const
Gets maximum bound.
Definition: lane_data.h:324
int index() const
Returns the index of this Lane within the Segment which owns it.
Definition: lane.h:47
TypeSpecificIdentifier<T> represents an identifier specifically identifying an entity of type T...
Definition: type_specific_identifier.h:38
HBounds elevation_bounds(double s, double r) const
Returns the elevation (h) bounds of the lane as a function of (s, r).
Definition: lane.h:89
TypeSpecificIdentifier< class Lane > LaneId
Persistent identifier for a Lane element.
Definition: lane.h:22
#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
const Segment * segment() const
Returns the Segment to which this Lane belongs.
Definition: lane.h:44
GeoPositionT< double > GeoPosition
Definition: lane_data.h:178
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
An api::Segment implementation.
Definition: segment.h:24
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:298
A 3-dimensional rotation.
Definition: lane_data.h:52
BranchPoint * start_bp()
Definition: lane.h:85
double min() const
Gets minimum bound.
Definition: lane_data.h:316
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
A Segment represents a bundle of adjacent Lanes which share a continuously traversable road surface...
Definition: segment.h:27
const Lane * lane(int index) const
Returns the Lane indexed by index.
Definition: segment.h:51
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:24
double r0() const
Definition: lane.h:80
const CubicPolynomial & superelevation() const
Definition: road_curve.h:100
const CubicPolynomial & elevation() const
Definition: lane.h:72
Vector3< double > V3
Definition: lane.h:22
RBounds driveable_bounds(double s) const
Returns the driveable lateral (r) bounds of the lane as a function of s.
Definition: lane.h:83
RBounds lane_bounds(double s) const
Returns the nominal lateral (r) bounds for the lane as a function of s.
Definition: lane.h:75
void SetStartBp(BranchPoint *bp)
Definition: lane.h:82
#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
Vector6< double > velocity
Definition: pose_smoother.cc:29
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
Vector2< double > V2
Definition: lane.h:19