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  s_from_p_at_r0_ = road_curve_->OptimizeCalcSFromP(r0_);
68  p_from_s_at_r0_ = road_curve_->OptimizeCalcPFromS(r0_);
69  lane_length_ = s_from_p_at_r0_(1.0);
70  }
71 
72  // TODO(maddog-tri) Allow superelevation to have a center-of-rotation
73  // which is different from r = 0.
74 
75  const CubicPolynomial& elevation() const {
76  return road_curve_->elevation();
77  }
78 
80  return road_curve_->superelevation();
81  }
82 
83  double r0() const { return r0_; }
84 
85  void SetStartBp(BranchPoint* bp) { start_bp_ = bp; }
86  void SetEndBp(BranchPoint* bp) { end_bp_ = bp; }
87 
88  BranchPoint* start_bp() { return start_bp_; }
89 
90  BranchPoint* end_bp() { return end_bp_; }
91 
92  ~Lane() override = default;
93 
94  private:
95  const api::LaneId do_id() const override { return id_; }
96 
97  const api::Segment* do_segment() const override;
98 
99  int do_index() const override { return index_; }
100 
101  const api::Lane* do_to_left() const override {
102  if (index_ == (segment_->num_lanes() - 1)) {
103  return nullptr;
104  } else {
105  return segment_->lane(index_ + 1);
106  }
107  }
108 
109  const api::Lane* do_to_right() const override {
110  if (index_ == 0) {
111  return nullptr;
112  } else {
113  return segment_->lane(index_ - 1);
114  }
115  }
116 
117  const api::BranchPoint* DoGetBranchPoint(
118  const api::LaneEnd::Which which_end) const override;
119 
120  const api::LaneEndSet* DoGetConfluentBranches(
121  const api::LaneEnd::Which which_end) const override;
122 
123  const api::LaneEndSet* DoGetOngoingBranches(
124  const api::LaneEnd::Which which_end) const override;
125 
126  optional<api::LaneEnd> DoGetDefaultBranch(
127  const api::LaneEnd::Which which_end) const override;
128 
129  double do_length() const override { return lane_length_; }
130 
131  api::RBounds do_lane_bounds(double) const override { return lane_bounds_; }
132 
133  api::RBounds do_driveable_bounds(double) const override {
134  return driveable_bounds_;
135  }
136 
137  api::HBounds do_elevation_bounds(double, double) const override {
138  return elevation_bounds_;
139  }
140 
141  api::GeoPosition DoToGeoPosition(
142  const api::LanePosition& lane_pos) const override;
143 
144  api::Rotation DoGetOrientation(
145  const api::LanePosition& lane_pos) const override;
146 
147  api::LanePosition DoEvalMotionDerivatives(
149  const api::IsoLaneVelocity& velocity) const override;
150 
151  api::LanePosition DoToLanePosition(
152  const api::GeoPosition& geo_position,
153  api::GeoPosition* nearest_position,
154  double* distance) const override;
155 
156  const api::LaneId id_;
157  const api::Segment* segment_{};
158  const int index_{};
159  BranchPoint* start_bp_{};
160  BranchPoint* end_bp_{};
161 
162  const api::RBounds lane_bounds_;
163  const api::RBounds driveable_bounds_;
164  const api::HBounds elevation_bounds_;
165  const RoadCurve* road_curve_{};
166  const double r0_;
167 
168  std::function<double(double)> s_from_p_at_r0_;
169  std::function<double(double)> p_from_s_at_r0_;
170  double lane_length_;
171 };
172 
173 
174 } // namespace multilane
175 } // namespace maliput
176 } // 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: bullet_model.cc:22
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:86
const CubicPolynomial & elevation() const
Definition: road_curve.h:115
BranchPoint * end_bp()
Definition: lane.h:90
std::function< double(double)> OptimizeCalcPFromS(double r) const
Optimizes the computation of the parametric position p along the reference curve from the longitudina...
Definition: road_curve.cc:200
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:109
An implementation of api::BranchPoint.
Definition: branch_point.h:42
std::function< double(double)> OptimizeCalcSFromP(double r) const
Optimizes the computation of path length integral in the interval of the parameter [0; p] and along a...
Definition: road_curve.cc:164
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:35
stx::optional< T > optional
Definition: drake_optional.h:22
const CubicPolynomial & superelevation() const
Definition: lane.h:79
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:283
Definition: arc_road_curve.cc:11
double max() const
Gets maximum bound.
Definition: lane_data.h:341
int index() const
Returns the index of this Lane within the Segment which owns it.
Definition: lane.h:48
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:90
TypeSpecificIdentifier< class Lane > LaneId
Persistent identifier for a Lane element.
Definition: lane.h:23
#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:45
GeoPositionT< double > GeoPosition
Definition: lane_data.h:186
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
An api::Segment implementation.
Definition: segment.h:26
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:315
A 3-dimensional rotation.
Definition: lane_data.h:56
BranchPoint * start_bp()
Definition: lane.h:88
double min() const
Gets minimum bound.
Definition: lane_data.h:333
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
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:27
double r0() const
Definition: lane.h:83
const CubicPolynomial & superelevation() const
Definition: road_curve.h:117
const CubicPolynomial & elevation() const
Definition: lane.h:75
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:84
RBounds lane_bounds(double s) const
Returns the nominal lateral (r) bounds for the lane as a function of s.
Definition: lane.h:76
void SetStartBp(BranchPoint *bp)
Definition: lane.h:85
#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