Drake
road_geometry.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <string>
4 #include <vector>
5 
13 
14 namespace drake {
15 namespace maliput {
16 namespace api {
17 
18 class BranchPoint;
19 class Junction;
20 
21 
22 /// Persistent identifier for a RoadGeometry element.
24 
25 
26 /// Abstract API for the geometry of a road network, including both
27 /// the network topology and the geometry of its embedding in 3-space.
28 // TODO(maddog@tri.global) This entire API should be templated on a
29 // scalar type T like everything else in drake.
30 class RoadGeometry {
31  public:
32  class IdIndex;
33 
35 
36  virtual ~RoadGeometry() = default;
37 
38  /// Returns the persistent identifier.
39  ///
40  // TODO(maddog@tri.global) Tie id into a tiling mechanism?
41  const RoadGeometryId id() const { return do_id(); }
42 
43  // TODO(maddog@tri.global) Proper iterators over junctions and branch-points?
44 
45  /// Returns the number of Junctions in the RoadGeometry.
46  ///
47  /// Return value is non-negative.
48  int num_junctions() const { return do_num_junctions(); }
49 
50  /// Returns the Junction indexed by @p index.
51  ///
52  /// @pre @p index must be >= 0 and < num_junctions().
53  const Junction* junction(int index) const { return do_junction(index); }
54 
55  /// Returns the number of BranchPoints in the RoadGeometry.
56  ///
57  /// Return value is non-negative.
58  int num_branch_points() const { return do_num_branch_points(); }
59 
60  /// Returns the BranchPoint indexed by @p index.
61  ///
62  /// @pre @p index must be >= 0 and < num_branch_points().
63  const BranchPoint* branch_point(int index) const {
64  return do_branch_point(index);
65  }
66 
67  /// Accesses the IdIndex interface, which allows getting elements of
68  /// the RoadGeometry's object graph by their unique id's.
69  const IdIndex& ById() const { return DoById(); }
70 
71  /// Determines the RoadPosition corresponding to GeoPosition @p geo_position.
72  ///
73  /// If @p hint is non-null, its value is used to help determine the result.
74  ///
75  /// The return value is the RoadPosition of the point in the RoadGeometry's
76  /// manifold which is, in the world frame, closest to @p geo_position. If
77  /// @p nearest_position is non-null, then it will be populated with the
78  /// GeoPosition of that nearest point. If @p distance is non-null, then it
79  /// will be populated with the Cartesian distance from @p geo_position to
80  /// that nearest point.
81  ///
82  /// This method guarantees that its result satisfies the condition that
83  /// `result.lane->ToGeoPosition(result.pos)` is within `linear_tolerance()`
84  /// of `*nearest_position`.
85  ///
86  /// The map from RoadGeometry to the world frame is not onto (as a bounded
87  /// RoadGeometry cannot completely cover the unbounded Cartesian universe).
88  /// If @p geo_position does represent a point contained within the volume
89  /// of the RoadGeometry, then result @p distance is guaranteed to be less
90  /// than or equal to `linear_tolerance()`.
91  ///
92  /// The map from RoadGeometry to world frame is not necessarily one-to-one.
93  /// Different `(s,r,h)` coordinates from different Lanes, potentially from
94  /// different Segments, may map to the same `(x,y,z)` world frame location.
95  ///
96  /// If @p geo_position is contained within the volumes of multiple Segments,
97  /// then ToRoadPosition() will choose a Segment which yields the minimum
98  /// height `h` value in the result. If the chosen Segment has multiple
99  /// Lanes, then ToRoadPosition() will choose a Lane which contains
100  /// @p geo_position within its `lane_bounds()` if possible, and if that is
101  /// still ambiguous, it will further select a Lane which minimizes the
102  /// absolute value of the lateral `r` coordinate in the result.
103  // TODO(maddog@tri.global) Establish what effect `hint` has on the outcome.
104  // Two notions:
105  // 1) the hint helps to bootstrap a search;
106  // 2) the hint helps to choose between multiple
107  // equally valid solutions.
108  // The general idea: if one knows the RoadPosition
109  // of an entity some small dT in the past, then one
110  // might expect an updated RoadPosition which is
111  // nearby (e.g., on the same Lane).
113  const RoadPosition* hint,
114  GeoPosition* nearest_position,
115  double* distance) const {
116  return DoToRoadPosition(geo_position, hint, nearest_position, distance);
117  }
118 
119  /// Returns the tolerance guaranteed for linear measurements (positions).
120  double linear_tolerance() const {
121  return do_linear_tolerance();
122  }
123 
124  /// Returns the tolerance guaranteed for angular measurements (orientations).
125  double angular_tolerance() const {
126  return do_angular_tolerance();
127  }
128 
129  /// Verifies certain invariants guaranteed by the API.
130  ///
131  /// Returns a vector of strings describing violations of invariants.
132  /// Return value with size() == 0 indicates success.
133  std::vector<std::string> CheckInvariants() const;
134 
135  protected:
136  RoadGeometry() = default;
137 
138  private:
139  /// @name NVI implementations of the public methods.
140  /// These must satisfy the constraints/invariants of the
141  /// corresponding public methods.
142  ///@{
143  virtual const RoadGeometryId do_id() const = 0;
144 
145  virtual int do_num_junctions() const = 0;
146 
147  virtual const Junction* do_junction(int index) const = 0;
148 
149  virtual int do_num_branch_points() const = 0;
150 
151  virtual const BranchPoint* do_branch_point(int index) const = 0;
152 
153  virtual const IdIndex& DoById() const = 0;
154 
155  virtual RoadPosition DoToRoadPosition(const GeoPosition& geo_pos,
156  const RoadPosition* hint,
157  GeoPosition* nearest_position,
158  double* distance) const = 0;
159 
160  virtual double do_linear_tolerance() const = 0;
161 
162  virtual double do_angular_tolerance() const = 0;
163  ///@}
164 };
165 
166 
167 /// Abstract interface for a collection of methods which allow accessing
168 /// objects in a RoadGeometry's object graph (Lanes, Segments, Junctions,
169 /// BranchPoints) by their unique id's.
171  public:
173  virtual ~IdIndex() = default;
174 
175  /// Returns the Lane identified by @p id, or `nullptr` if @p id is unknown.
176  const Lane* GetLane(const LaneId& id) const { return DoGetLane(id); }
177 
178  /// Returns the Segment identified by @p id, or `nullptr` if @p id is
179  /// unknown.
180  const Segment* GetSegment(const SegmentId& id) const {
181  return DoGetSegment(id);
182  }
183 
184  /// Returns the Junction identified by @p id, or `nullptr` if @p id is
185  /// unknown.
186  const Junction* GetJunction(const JunctionId& id) const {
187  return DoGetJunction(id);
188  }
189 
190  /// Returns the BranchPoint identified by @p id, or `nullptr` if @p id is
191  /// unknown.
192  const BranchPoint* GetBranchPoint(const BranchPointId& id) const {
193  return DoGetBranchPoint(id);
194  }
195 
196  protected:
197  IdIndex() = default;
198 
199  private:
200  virtual const Lane* DoGetLane(const LaneId& id) const = 0;
201  virtual const Segment* DoGetSegment(const SegmentId& id) const = 0;
202  virtual const Junction* DoGetJunction(const JunctionId& id) const = 0;
203  virtual const BranchPoint* DoGetBranchPoint(
204  const BranchPointId& id) const = 0;
205 };
206 
207 
208 } // namespace api
209 } // namespace maliput
210 } // namespace drake
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 Segment * GetSegment(const SegmentId &id) const
Returns the Segment identified by id, or nullptr if id is unknown.
Definition: road_geometry.h:180
std::vector< std::string > CheckInvariants() const
Verifies certain invariants guaranteed by the API.
Definition: road_geometry.cc:118
const IdIndex & ById() const
Accesses the IdIndex interface, which allows getting elements of the RoadGeometry&#39;s object graph by t...
Definition: road_geometry.h:69
Abstract API for the geometry of a road network, including both the network topology and the geometry...
Definition: road_geometry.h:30
Abstract interface for a collection of methods which allow accessing objects in a RoadGeometry&#39;s obje...
Definition: road_geometry.h:170
TypeSpecificIdentifier< class BranchPoint > BranchPointId
Definition: branch_point.h:19
const RoadGeometryId id() const
Returns the persistent identifier.
Definition: road_geometry.h:41
A Lane represents a lane of travel in a road network.
Definition: lane.h:35
RoadPosition ToRoadPosition(const GeoPosition &geo_position, const RoadPosition *hint, GeoPosition *nearest_position, double *distance) const
Determines the RoadPosition corresponding to GeoPosition geo_position.
Definition: road_geometry.h:112
double linear_tolerance() const
Returns the tolerance guaranteed for linear measurements (positions).
Definition: road_geometry.h:120
TypeSpecificIdentifier<T> represents an identifier specifically identifying an entity of type T...
Definition: type_specific_identifier.h:39
int num_junctions() const
Returns the number of Junctions in the RoadGeometry.
Definition: road_geometry.h:48
GeoPositionT< double > GeoPosition
Definition: lane_data.h:186
double angular_tolerance() const
Returns the tolerance guaranteed for angular measurements (orientations).
Definition: road_geometry.h:125
const BranchPoint * branch_point(int index) const
Returns the BranchPoint indexed by index.
Definition: road_geometry.h:63
TypeSpecificIdentifier< class Junction > JunctionId
Persistent identifier for a Junction element.
Definition: junction.h:17
TypeSpecificIdentifier< class RoadGeometry > RoadGeometryId
Persistent identifier for a RoadGeometry element.
Definition: road_geometry.h:23
int num_branch_points() const
Returns the number of BranchPoints in the RoadGeometry.
Definition: road_geometry.h:58
A Segment represents a bundle of adjacent Lanes which share a continuously traversable road surface...
Definition: segment.h:27
const BranchPoint * GetBranchPoint(const BranchPointId &id) const
Returns the BranchPoint identified by id, or nullptr if id is unknown.
Definition: road_geometry.h:192
const Junction * GetJunction(const JunctionId &id) const
Returns the Junction identified by id, or nullptr if id is unknown.
Definition: road_geometry.h:186
A Junction is a closed set of Segments which have physically coplanar road surfaces, in the sense that RoadPositions with the same h value (height above surface) in the domains of two Segments map to the same GeoPosition.
Definition: junction.h:27
const Lane * GetLane(const LaneId &id) const
Returns the Lane identified by id, or nullptr if id is unknown.
Definition: road_geometry.h:176
TypeSpecificIdentifier< class Segment > SegmentId
Persistent identifier for a Segment element.
Definition: segment.h:17
#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
A position in the road network, consisting of a pointer to a specific Lane and a Lane-frame position ...
Definition: lane_data.h:299
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
const Junction * junction(int index) const
Returns the Junction indexed by index.
Definition: road_geometry.h:53