Drake
builder.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <map>
5 #include <memory>
6 #include <string>
7 #include <tuple>
8 #include <unordered_set>
9 #include <vector>
10 
15 
16 namespace drake {
17 namespace maliput {
18 
19 namespace multilane {
20 class RoadGeometry;
21 
47 
48 
56 class EndpointXy {
57  public:
59 
60  // Constructs an EndpointXy with all zero parameters.
61  EndpointXy() = default;
62 
63  EndpointXy(double x, double y, double heading)
64  : x_(x), y_(y), heading_(heading) {}
65 
67  EndpointXy reverse() const {
68  return EndpointXy(x_, y_,
69  std::atan2(-std::sin(heading_), -std::cos(heading_)));
70  }
71 
72  double x() const { return x_; }
73 
74  double y() const { return y_; }
75 
76  double heading() const { return heading_; }
77 
78  private:
79  double x_{};
80  double y_{};
81  double heading_{};
82 };
83 
84 
96 class EndpointZ {
97  public:
99 
100  // Constructs an EndpointZ with all zero parameters.
101  EndpointZ() = default;
102 
103  EndpointZ(double z, double z_dot, double theta, double theta_dot)
104  : z_(z), z_dot_(z_dot), theta_(theta), theta_dot_(theta_dot) {}
105 
107  EndpointZ reverse() const {
108  return EndpointZ(z_, -z_dot_, -theta_, -theta_dot_);
109  }
110 
111  double z() const { return z_; }
112 
113  double z_dot() const { return z_dot_; }
114 
115  double theta() const { return theta_; }
116 
117  double theta_dot() const { return theta_dot_; }
118 
119  private:
120  double z_{};
121  double z_dot_{};
122 
123  double theta_{};
124  double theta_dot_{};
125 };
126 
127 
132 class Endpoint {
133  public:
135 
136  // Constructs an Endpoint with all zero parameters.
137  Endpoint() = default;
138 
139  Endpoint(const EndpointXy& xy, const EndpointZ& z) : xy_(xy), z_(z) {}
140 
142  Endpoint reverse() const {
143  return Endpoint(xy_.reverse(), z_.reverse());
144  }
145 
147  const EndpointXy& xy() const { return xy_; }
148 
151  const EndpointZ& z() const { return z_; }
152 
153  private:
154  EndpointXy xy_;
155  EndpointZ z_;
156 };
157 
158 
164 class ArcOffset {
165  public:
167 
168 
169  ArcOffset() = default;
170 
171  ArcOffset(double radius, double d_theta)
172  : radius_(radius), d_theta_(d_theta) {
173  DRAKE_DEMAND(radius_ > 0.);
174  }
175 
176  double radius() const { return radius_; }
177 
178  double d_theta() const { return d_theta_; }
179 
180  private:
181  double radius_{};
182  double d_theta_{};
183 };
184 
185 
198 class Connection {
199  public:
201 
202 
203  enum Type { kLine, kArc };
204 
206  Connection(const std::string& id,
207  const Endpoint& start, const Endpoint& end)
208  : type_(kLine), id_(id), start_(start), end_(end) {}
209 
217  Connection(const std::string& id,
218  const Endpoint& start, const Endpoint& end,
219  double cx, double cy, double radius, double d_theta)
220  : type_(kArc), id_(id), start_(start), end_(end),
221  cx_(cx), cy_(cy), radius_(radius), d_theta_(d_theta) {}
222 
224  Type type() const { return type_; }
225 
227  const std::string& id() const { return id_; }
228 
230  const Endpoint& start() const { return start_; }
231 
233  const Endpoint& end() const { return end_; }
234 
236  double cx() const {
237  DRAKE_DEMAND(type_ == kArc);
238  return cx_;
239  }
240 
242  double cy() const {
243  DRAKE_DEMAND(type_ == kArc);
244  return cy_;
245  }
246 
248  double radius() const {
249  DRAKE_DEMAND(type_ == kArc);
250  return radius_;
251  }
252 
254  double d_theta() const {
255  DRAKE_DEMAND(type_ == kArc);
256  return d_theta_;
257  }
258 
259  private:
260  Type type_{};
261  std::string id_;
262  Endpoint start_;
263  Endpoint end_;
264 
265  // Bits specific to type_ == kArc:
266  double cx_{};
267  double cy_{};
268  double radius_{};
269  double d_theta_{};
270 };
271 
272 
277 class Group {
278  public:
280 
281 
282  explicit Group(const std::string& id) : id_(id) {}
283 
285  Group(const std::string& id,
286  const std::vector<const Connection*>& connections)
287  : id_(id) {
288  for (const Connection* connection : connections) {
289  Add(connection);
290  }
291  }
292 
294  void Add(const Connection* connection) {
295  auto result = connection_set_.insert(connection);
296  DRAKE_DEMAND(result.second);
297  connection_vector_.push_back(connection);
298  }
299 
301  const std::string& id() const { return id_; }
302 
304  const std::vector<const Connection*>& connections() const {
305  return connection_vector_;
306  }
307 
308  private:
309  std::string id_;
310  std::unordered_set<const Connection*> connection_set_;
311  std::vector<const Connection*> connection_vector_;
312 };
313 
314 
315 // N.B. The Builder class overview documentation lives at the top of this file.
316 class Builder {
317  public:
319 
320 
321  Builder(const api::RBounds& lane_bounds,
329  const api::RBounds& driveable_bounds,
330  const api::HBounds& elevation_bounds,
331  const double linear_tolerance,
332  const double angular_tolerance);
333 
338  const Connection* Connect(
339  const std::string& id,
340  const Endpoint& start,
341  const double length,
342  const EndpointZ& z_end);
343 
347  const Connection* Connect(
348  const std::string& id,
349  const Endpoint& start,
350  const ArcOffset& arc,
351  const EndpointZ& z_end);
352 
360  void SetDefaultBranch(
361  const Connection* in, const api::LaneEnd::Which in_end,
362  const Connection* out, const api::LaneEnd::Which out_end);
363 
365  Group* MakeGroup(const std::string& id);
366 
369  Group* MakeGroup(const std::string& id,
370  const std::vector<const Connection*>& connections);
371 
373  std::unique_ptr<const api::RoadGeometry> Build(
374  const api::RoadGeometryId& id) const;
375 
376  private:
377  // EndpointFuzzyOrder is an arbitrary strict complete ordering of Endpoints
378  // useful for, e.g., std::map. It provides a comparison operation that
379  // treats two Endpoints within @p linear_tolerance of one another as
380  // equivalent.
381  //
382  // This is used to match up the endpoints of Connections, to determine
383  // how Connections are linked to one another. Exact numeric equality
384  // would not be robust given the use of floating-point values in Endpoints.
385  class EndpointFuzzyOrder {
386  public:
387  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(EndpointFuzzyOrder)
388 
389  explicit EndpointFuzzyOrder(const double linear_tolerance)
390  : lin_tol_(linear_tolerance) {}
391 
392  bool operator()(const Endpoint& lhs, const Endpoint& rhs) const {
393  switch (fuzzy_compare(rhs.xy().x(), lhs.xy().x())) {
394  case -1: { return true; }
395  case 1: { return false; }
396  case 0: {
397  switch (fuzzy_compare(rhs.xy().y(), lhs.xy().y())) {
398  case -1: { return true; }
399  case 1: { return false; }
400  case 0: {
401  switch (fuzzy_compare(rhs.z().z(), lhs.z().z())) {
402  case -1: { return true; }
403  case 1: { return false; }
404  case 0: { return false; }
405  default: { DRAKE_ABORT(); }
406  }
407  }
408  default: { DRAKE_ABORT(); }
409  }
410  }
411  default: { DRAKE_ABORT(); }
412  }
413  }
414 
415  private:
416  int fuzzy_compare(const double a, const double b) const {
417  if (a < (b - lin_tol_)) {
418  return -1;
419  } else if (a > (b + lin_tol_)) {
420  return 1;
421  } else {
422  return 0;
423  }
424  }
425 
426  double lin_tol_{};
427  };
428 
429  struct DefaultBranch {
430  DefaultBranch() = default;
431 
432  DefaultBranch(
433  const Connection* ain, const api::LaneEnd::Which ain_end,
434  const Connection* aout, const api::LaneEnd::Which aout_end)
435  : in(ain), in_end(ain_end), out(aout), out_end(aout_end) {}
436 
437  const Connection* in{};
438  api::LaneEnd::Which in_end{};
439  const Connection* out{};
440  api::LaneEnd::Which out_end{};
441  };
442 
443  Lane* BuildConnection(
444  const Connection* const cnx,
445  Junction* const junction,
446  RoadGeometry* const rg,
447  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
448 
449  BranchPoint* FindOrCreateBranchPoint(
450  const Endpoint& point,
451  RoadGeometry* rg,
452  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
453 
454  void AttachBranchPoint(
455  const Endpoint& point, Lane* const lane, const api::LaneEnd::Which end,
456  RoadGeometry* rg,
457  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* bp_map) const;
458 
459  api::RBounds lane_bounds_;
460  api::RBounds driveable_bounds_;
461  api::HBounds elevation_bounds_;
462  double linear_tolerance_{};
463  double angular_tolerance_{};
464  std::vector<std::unique_ptr<Connection>> connections_;
465  std::vector<DefaultBranch> default_branches_;
466  std::vector<std::unique_ptr<Group>> groups_;
467 };
468 
469 } // namespace multilane
470 } // namespace maliput
471 } // namespace drake
const Expression xy_
Definition: symbolic_polynomial_test.cc:43
XY-plane-only parameters for an endpoint of a connection, specified in the world frame.
Definition: builder.h:56
const std::vector< const Connection * > & connections() const
Returns the grouped Connections.
Definition: builder.h:304
Type type() const
Returns the geometric type of the path.
Definition: builder.h:224
EndpointXy(double x, double y, double heading)
Definition: builder.h:63
EndpointZ reverse() const
Returns an EndpointZ with reversed direction.
Definition: builder.h:107
Definition: automotive_demo.cc:88
Complete set of parameters for an endpoint of a connection, specified in the world frame...
Definition: builder.h:132
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
Convenient builder class which makes it easy to construct a multilane road network.
Definition: builder.h:316
A group of Connections.
Definition: builder.h:277
ArcOffset(double radius, double d_theta)
Definition: builder.h:171
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:652
STL namespace.
Connection(const std::string &id, const Endpoint &start, const Endpoint &end)
Constructs a line-segment connection joining start to end.
Definition: builder.h:206
EndpointXy reverse() const
Returns an EndpointXy with reversed direction.
Definition: builder.h:67
Connection(const std::string &id, const Endpoint &start, const Endpoint &end, double cx, double cy, double radius, double d_theta)
Constructs an arc-segment connection joining start to end.
Definition: builder.h:217
An implementation of api::BranchPoint.
Definition: branch_point.h:41
double y() const
Definition: builder.h:74
Base class for the multilane implementation of api::Lane.
Definition: lane.h:51
double theta() const
Definition: builder.h:115
int b
Definition: rgbd_camera.cc:91
double d_theta() const
Returns the angle of the arc (for arc connections only).
Definition: builder.h:254
double z() const
Definition: builder.h:111
double theta_dot() const
Definition: builder.h:117
std::vector< Number > result
Definition: ipopt_solver.cc:154
Definition: arc_road_curve.cc:11
void Add(const Connection *connection)
Adds a Connection.
Definition: builder.h:294
double x() const
Definition: builder.h:72
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:59
Out-of-plane parameters for an endpoint of a connection, specified in the world frame.
Definition: builder.h:96
Group(const std::string &id, const std::vector< const Connection * > &connections)
Constructs a Group with id, populated by connections.
Definition: builder.h:285
double heading() const
Definition: builder.h:76
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:47
Type
Possible connection geometries: line- or arc-segment.
Definition: builder.h:203
const std::string & id() const
Returns the ID string.
Definition: builder.h:301
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:274
const Endpoint & end() const
Returns the parameters of the endpoint.
Definition: builder.h:233
double radius() const
Returns the radius of the arc (for arc connections only).
Definition: builder.h:248
TypeSpecificIdentifier< class RoadGeometry > RoadGeometryId
Persistent identifier for a RoadGeometry element.
Definition: road_geometry.h:19
double cx() const
Returns the x-component of the arc center (for arc connections only).
Definition: builder.h:236
double z_dot() const
Definition: builder.h:113
const Expression z_
Definition: symbolic_expansion_test.cc:31
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:309
const EndpointZ & z() const
Returns the subset of parameters pertaining to out-of-ground-plane aspects.
Definition: builder.h:151
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:25
double z
Definition: vtk_util_test.cc:27
double cy() const
Returns the y-component of the arc center (for arc connections only).
Definition: builder.h:242
double radius() const
Definition: builder.h:176
EndpointZ(double z, double z_dot, double theta, double theta_dot)
Definition: builder.h:103
Representation of a reference path connecting two endpoints.
Definition: builder.h:198
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:607
Specification for path offset along a circular arc.
Definition: builder.h:164
Endpoint reverse() const
Returns an Endpoint with reversed direction.
Definition: builder.h:142
#define DRAKE_ABORT()
Aborts the program (via ::abort) with a message showing at least the function name, file, and line.
Definition: drake_assert.h:50
double d_theta() const
Definition: builder.h:178
const EndpointXy & xy() const
Returns the subset of parameters pertaining to the xy ground-plane.
Definition: builder.h:147
A simple api::RoadGeometry implementation that only supports a single lane per segment.
Definition: road_geometry.h:20
An api::Junction implementation.
Definition: junction.h:18
Endpoint(const EndpointXy &xy, const EndpointZ &z)
Definition: builder.h:139
const Endpoint & start() const
Returns the parameters of the start point.
Definition: builder.h:230
#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
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
const std::string & id() const
Returns the ID string.
Definition: builder.h:227