Drake
multilane_types_compare.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <gmock/gmock.h>
4 #include <gtest/gtest.h>
5 
9 
10 namespace drake {
11 namespace maliput {
12 namespace multilane {
13 namespace test {
14 
15 using ::testing::Matcher;
16 using ::testing::MatcherInterface;
17 using ::testing::MatchResultListener;
18 
19 // Compares equality within @p tolerance deviation of two EndpointXy objects.
20 // @param xy1 An EndpointXy object to compare.
21 // @param xy2 An EndpointXy object to compare.
22 // @param linear_tolerance An allowable absolute deviation for each EndpointXy's
23 // x and y position coordinates, in meters.
24 // @param angular_tolerance An allowable absolute deviation for EndpointXy's
25 // heading angle, in radians.
26 // @return ::testing::AssertionFailure() When EndpointXy objects are different.
27 // @return ::testing::AssertionSuccess() When EndpointXy objects' quantities
28 // are within @p linear_tolerance or
29 // @p angular_tolerance, as appropriate.
30 ::testing::AssertionResult IsEndpointXyClose(const EndpointXy& xy1,
31  const EndpointXy& xy2,
32  double linear_tolerance,
33  double angular_tolerance);
34 
35 // Compares equality within @p tolerance deviation of two EndpointZ objects.
36 // @param z1 An EndpointZ object to compare.
37 // @param z2 An EndpointZ object to compare.
38 // @param linear_tolerance An allowable absolute deviation, in meters, for
39 // EndpointZ's z position coordinate, as well as
40 // elevation derivative ż through the linear deviation
41 // that would result of moving 1 m along the reference
42 // curve path for which the derivative is defined.
43 // @param angular_tolerance An allowable absolute deviation, in radians, for
44 // each EndpointZ's heading angle and superelevation
45 // angle θ, as well as superelevation derivative θ_dot
46 // through the angular deviation that would result of
47 // moving 1 m along the reference curve path for which
48 // the derivative is defined.
49 // @return ::testing::AssertionFailure() When EndpointZ objects are different.
50 // @return ::testing::AssertionSuccess() When EndpointZ objects' quantities
51 // are within @p linear_tolerance or
52 // @p angular_tolerance, as appropriate.
53 // TODO(hidmic): Review tolerances definition, units and usage for elevation
54 // and superelevation derivatives in EndpointZ instances.
55 ::testing::AssertionResult IsEndpointZClose(const EndpointZ& z1,
56  const EndpointZ& z2,
57  double linear_tolerance,
58  double angular_tolerance);
59 
60 // Compares equality within @p tolerance deviation of two Endpoint objects.
61 // @param pos1 An Endpoint object to compare.
62 // @param pos2 An Endpoint object to compare.
63 // @param linear_tolerance An allowable absolute deviation, in meters, for each
64 // Endpoint's x, y and z position coordinates, as well
65 // as elevation derivative ż through the linear
66 // deviation that would result of moving 1 m along the
67 // reference curve path for which the derivative is
68 // defined.
69 // @param angular_tolerance An allowable absolute deviation, in radians, for
70 // each Endpoint's heading angle and superelevation
71 // angle θ, as well as superelevation derivative θ_dot
72 // through the angular deviation that would result of
73 // moving 1 m along the reference curve path for which
74 // the derivative is defined.
75 // @return ::testing::AssertionFailure() When Endpoint objects are different.
76 // @return ::testing::AssertionSuccess() When Endpoint objects' quantities
77 // are within @p linear_tolerance or
78 // @p angular_tolerance, as appropriate.
79 ::testing::AssertionResult IsEndpointClose(const Endpoint& pos1,
80  const Endpoint& pos2,
81  double linear_tolerance,
82  double angular_tolerance);
83 
84 // Compares equality within @p linear_tolerance and @p angular_tolerance
85 // deviations of two ArcOffset objects.
86 // @param arc_offset1 An ArcOffset object to compare.
87 // @param arc_offset2 An ArcOffset object to compare.
88 // @param linear_tolerance An allowable absolute linear deviation for
89 // ArcOffset's radius.
90 // @param angular_tolerance An allowable absolute angle deviation for
91 // ArcOffset's d_theta.
92 // @return ::testing::AssertionFailure() When ArcOffset objects are different.
93 // @return ::testing::AssertionSuccess() When ArcOffset objects are within
94 // @p linear_tolerance and @p angular_tolerance deviations.
95 ::testing::AssertionResult IsArcOffsetClose(const ArcOffset& arc_offset1,
96  const ArcOffset& arc_offset2,
97  double linear_tolerance,
98  double angular_tolerance);
99 
100 // Compares equality within @p tolerance of @p cubic1 and @p cubic2
101 // coefficients.
102 // @param cubic1 A CubicPolynomial object to compare.
103 // @param cubic2 A CubicPolynomial object to compare.
104 // @param tolerance An allowable absolute linear deviation for each coefficient.
105 // @return ::testing::AssertionFailure() When any coefficient of
106 // CubicPolynomial objects are different.
107 // @return ::testing::AssertionSuccess() When all coefficients of
108 // CubicPolynomial objects are equal.
109 ::testing::AssertionResult IsCubicPolynomialClose(const CubicPolynomial& cubic1,
110  const CubicPolynomial& cubic2,
111  double tolerance);
112 
113 /// Wraps api::HBounds comparison into a MatcherInterface.
114 class HBoundsMatcher : public MatcherInterface<const api::HBounds&> {
115  public:
117  : elevation_bounds_(elevation_bounds), tolerance_(tolerance) {}
118 
119  bool MatchAndExplain(const api::HBounds& other,
120  MatchResultListener*) const override {
121  return api::test::IsHBoundsClose(elevation_bounds_, other, tolerance_);
122  }
123 
124  void DescribeTo(std::ostream* os) const override {
125  *os << "is within tolerance [" << tolerance_ << "] of elevation_bounds: ["
126  << elevation_bounds_.min() << ", " << elevation_bounds_.max() << "].";
127  }
128 
129  private:
130  const api::HBounds elevation_bounds_;
131  const double tolerance_{};
132 };
133 
134 /// @return A Matcher<const api::HBounds&> of type HBoundsMatcher.
135 Matcher<const api::HBounds&> Matches(const api::HBounds& elevation_bounds,
136  double tolerance);
137 
138 /// Wraps an ArcOffset comparison into a MatcherInterface.
139 class ArcOffsetMatcher : public MatcherInterface<const ArcOffset&> {
140  public:
141  ArcOffsetMatcher(const ArcOffset& arc_offset, double linear_tolerance,
142  double angular_tolerance)
143  : arc_offset_(arc_offset),
144  linear_tolerance_(linear_tolerance),
145  angular_tolerance_(angular_tolerance) {}
146 
147  bool MatchAndExplain(const ArcOffset& other,
148  MatchResultListener*) const override {
149  return IsArcOffsetClose(arc_offset_, other, linear_tolerance_,
150  angular_tolerance_);
151  }
152 
153  void DescribeTo(std::ostream* os) const override {
154  *os << "is within linear and angular tolerance: [" << linear_tolerance_
155  << ", " << angular_tolerance_ << "] of arc_offset: ["
156  << arc_offset_.radius() << ", " << arc_offset_.d_theta() << "].";
157  }
158 
159  private:
160  const ArcOffset arc_offset_;
161  const double linear_tolerance_{};
162  const double angular_tolerance_{};
163 };
164 
165 /// @return A Matcher<const ArcOffset&> of type ArcOffsetMatcher.
166 Matcher<const ArcOffset&> Matches(const ArcOffset& arc_offset,
167  double linear_tolerance,
168  double angular_tolerance);
169 
170 /// Wraps a LineOffset comparison into a MatcherInterface.
171 class LineOffsetMatcher : public MatcherInterface<const LineOffset&> {
172  public:
173  LineOffsetMatcher(const LineOffset& line_offset, double tolerance)
174  : line_offset_(line_offset), tolerance_(tolerance) {}
175 
176  bool MatchAndExplain(const LineOffset& other,
177  MatchResultListener*) const override {
178  const double delta = std::abs(line_offset_.length() - other.length());
179  return delta <= tolerance_;
180  }
181 
182  void DescribeTo(std::ostream* os) const override {
183  *os << "is within tolerance: [" << tolerance_ << "] of line_offset: ["
184  << line_offset_ << "].";
185  }
186 
187  private:
188  const LineOffset line_offset_;
189  const double tolerance_{};
190 };
191 
192 /// @return A Matcher<const LineOffset&> of type LineOffsetMatcher.
193 Matcher<const LineOffset&> Matches(const LineOffset& line_offset,
194  double tolerance);
195 
196 /// Wraps a LineOffset comparison into a MatcherInterface.
197 class LaneLayoutMatcher : public MatcherInterface<const LaneLayout&> {
198  public:
199  LaneLayoutMatcher(const LaneLayout& lane_layout, double tolerance)
200  : lane_layout_(lane_layout), tolerance_(tolerance) {}
201 
202  bool MatchAndExplain(const LaneLayout& other,
203  MatchResultListener*) const override {
204  double delta{};
205 
206  delta = std::abs(lane_layout_.left_shoulder() - other.left_shoulder());
207  if (delta > tolerance_) return false;
208 
209  delta = std::abs(lane_layout_.right_shoulder() - other.right_shoulder());
210  if (delta > tolerance_) return false;
211 
212  if (lane_layout_.num_lanes() != other.num_lanes()) return false;
213 
214  if (lane_layout_.ref_lane() != other.ref_lane()) return false;
215 
216  delta = std::abs(lane_layout_.ref_r0() - other.ref_r0());
217  if (delta > tolerance_) return false;
218 
219  return true;
220  }
221 
222  void DescribeTo(std::ostream* os) const override {
223  *os << "is within tolerance: [" << tolerance_ << "] of lane_layout: ["
224  << lane_layout_ << "].";
225  }
226 
227  private:
228  const LaneLayout lane_layout_;
229  const double tolerance_{};
230 };
231 
232 /// @return A Matcher<const LaneLayout&> of type LaneLayoutMatcher.
233 Matcher<const LaneLayout&> Matches(const LaneLayout& lane_layout,
234  double tolerance);
235 
236 /// Wraps a StartReference::Spec comparison into a MatcherInterface.
238  : public MatcherInterface<const StartReference::Spec&> {
239  public:
241  double linear_tolerance, double angular_tolerance)
242  : start_reference_(start_reference),
243  linear_tolerance_(linear_tolerance),
244  angular_tolerance_(angular_tolerance) {}
245 
247  MatchResultListener*) const override {
248  return IsEndpointClose(start_reference_.endpoint(), other.endpoint(),
249  linear_tolerance_, angular_tolerance_);
250  }
251 
252  void DescribeTo(std::ostream* os) const override {
253  *os << "is within linear tolerance: [" << linear_tolerance_
254  << "] and angular tolerance: [" << angular_tolerance_
255  << "] of start_reference: [" << start_reference_ << "].";
256  }
257 
258  private:
259  const StartReference::Spec start_reference_;
260  const double linear_tolerance_{};
261  const double angular_tolerance_{};
262 };
263 
264 /// @return A Matcher<const StartReference::Spec&> of type
265 /// StartReferenceSpecMatcher.
266 Matcher<const StartReference::Spec&> Matches(
267  const StartReference::Spec& start_reference,
268  double linear_tolerance, double angular_tolerance);
269 
270 /// Wraps a EndReference::Spec comparison into a MatcherInterface.
272  : public MatcherInterface<const EndReference::Spec&> {
273  public:
275  double linear_tolerance, double angular_tolerance)
276  : end_reference_(end_reference),
277  linear_tolerance_(linear_tolerance),
278  angular_tolerance_(angular_tolerance) {}
279 
281  MatchResultListener*) const override {
282  return IsEndpointZClose(end_reference_.endpoint_z(), other.endpoint_z(),
283  linear_tolerance_, angular_tolerance_);
284  }
285 
286  void DescribeTo(std::ostream* os) const override {
287  *os << "is within linear tolerance: [" << linear_tolerance_
288  << "] and angular tolerance: [" << angular_tolerance_
289  << "] of end_reference: [" << end_reference_ << "].";
290  }
291 
292  private:
293  const EndReference::Spec end_reference_;
294  const double linear_tolerance_{};
295  const double angular_tolerance_{};
296 };
297 
298 /// Wraps a StartLane::Spec comparison into a MatcherInterface.
299 class StartLaneSpecMatcher : public MatcherInterface<const StartLane::Spec&> {
300  public:
302  double linear_tolerance, double angular_tolerance)
303  : start_lane_(start_lane),
304  linear_tolerance_(linear_tolerance),
305  angular_tolerance_(angular_tolerance) {}
306 
307  bool MatchAndExplain(const StartLane::Spec& other,
308  MatchResultListener*) const override {
309  return (IsEndpointClose(start_lane_.endpoint(), other.endpoint(),
310  linear_tolerance_, angular_tolerance_)
311  && start_lane_.lane_id() == other.lane_id());
312  }
313 
314  void DescribeTo(std::ostream* os) const override {
315  *os << "is within linear tolerance: [" << linear_tolerance_
316  << "] and angular tolerance: [" << angular_tolerance_
317  << "] and lane ID is equal to start_lane: ["
318  << start_lane_ << "].";
319  }
320 
321  private:
322  const StartLane::Spec start_lane_;
323  const double linear_tolerance_{};
324  const double angular_tolerance_{};
325 };
326 
327 /// @return A Matcher<const StartLane::Spec&> of type StartLaneSpecMatcher.
328 Matcher<const StartLane::Spec&> Matches(
329  const StartLane::Spec& start_reference,
330  double linear_tolerance, double angular_tolerance);
331 
332 /// @return A Matcher<const EndReference::Spec&> of type
333 /// EndReferenceSpecMatcher.
334 Matcher<const EndReference::Spec&> Matches(
335  const EndReference::Spec& end_reference,
336  double linear_tolerance, double angular_tolerance);
337 
338 /// Wraps a EndLane::Spec comparison into a MatcherInterface.
339 class EndLaneSpecMatcher : public MatcherInterface<const EndLane::Spec&> {
340  public:
341  EndLaneSpecMatcher(const EndLane::Spec& end_lane, double linear_tolerance,
342  double angular_tolerance)
343  : end_lane_(end_lane),
344  linear_tolerance_(linear_tolerance),
345  angular_tolerance_(angular_tolerance) {}
346 
347  bool MatchAndExplain(const EndLane::Spec& other,
348  MatchResultListener*) const override {
349  return IsEndpointZClose(end_lane_.endpoint_z(), other.endpoint_z(),
350  linear_tolerance_, angular_tolerance_);
351  }
352 
353  void DescribeTo(std::ostream* os) const override {
354  *os << "is within linear tolerance: [" << linear_tolerance_
355  << "] and angular tolerance: [" << angular_tolerance_
356  << "] and lane ID is equal to end_lane: ["
357  << end_lane_ << "].";
358  }
359 
360  private:
361  const EndLane::Spec end_lane_;
362  const double linear_tolerance_{};
363  const double angular_tolerance_{};
364 };
365 
366 /// @return A Matcher<const EndLane::Spec&> of type EndLaneSpecMatcher.
367 Matcher<const EndLane::Spec&> Matches(
368  const EndLane::Spec& end_lane,
369  double linear_tolerance, double angular_tolerance);
370 
371 } // namespace test
372 } // namespace multilane
373 } // namespace maliput
374 } // namespace drake
::testing::AssertionResult IsHBoundsClose(const HBounds &hbounds1, const HBounds &hbounds2, double tolerance)
Definition: maliput_types_compare.cc:184
void DescribeTo(std::ostream *os) const override
Definition: multilane_types_compare.h:314
double ref_r0() const
Definition: builder.h:338
void DescribeTo(std::ostream *os) const override
Definition: multilane_types_compare.h:124
bool MatchAndExplain(const api::HBounds &other, MatchResultListener *) const override
Definition: multilane_types_compare.h:119
StartReferenceSpecMatcher(const StartReference::Spec &start_reference, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.h:240
double right_shoulder() const
Definition: builder.h:332
Definition: bullet_model.cc:22
::testing::AssertionResult IsEndpointZClose(const EndpointZ &z1, const EndpointZ &z2, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.cc:55
bool MatchAndExplain(const StartReference::Spec &other, MatchResultListener *) const override
Definition: multilane_types_compare.h:246
Expression abs(const Expression &e)
Definition: symbolic_expression.cc:550
Defines how a Connection&#39;s reference curve starts.
Definition: builder.h:32
Wraps a EndLane::Spec comparison into a MatcherInterface.
Definition: multilane_types_compare.h:339
Wraps api::HBounds comparison into a MatcherInterface.
Definition: multilane_types_compare.h:114
LineOffsetMatcher(const LineOffset &line_offset, double tolerance)
Definition: multilane_types_compare.h:173
::testing::AssertionResult IsEndpointClose(const Endpoint &p1, const Endpoint &p2, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.cc:116
bool MatchAndExplain(const LineOffset &other, MatchResultListener *) const override
Definition: multilane_types_compare.h:176
void DescribeTo(std::ostream *os) const override
Definition: multilane_types_compare.h:252
void DescribeTo(std::ostream *os) const override
Definition: multilane_types_compare.h:182
double length() const
Definition: connection.h:162
Wraps a EndReference::Spec comparison into a MatcherInterface.
Definition: multilane_types_compare.h:271
EndLaneSpecMatcher(const EndLane::Spec &end_lane, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.h:341
Wraps all the lane-related specifications in a Connection.
Definition: builder.h:301
double left_shoulder() const
Definition: builder.h:330
double max() const
Gets maximum bound.
Definition: lane_data.h:386
bool MatchAndExplain(const ArcOffset &other, MatchResultListener *) const override
Definition: multilane_types_compare.h:147
void DescribeTo(std::ostream *os) const override
Definition: multilane_types_compare.h:286
Defines how a Connection&#39;s lane curve ends.
Definition: builder.h:225
Definition: arc_road_curve.cc:11
Wraps a StartLane::Spec comparison into a MatcherInterface.
Definition: multilane_types_compare.h:299
double min() const
Gets minimum bound.
Definition: lane_data.h:378
Wraps a LineOffset comparison into a MatcherInterface.
Definition: multilane_types_compare.h:197
int num_lanes() const
Definition: builder.h:334
Specification for path offset along a line.
Definition: connection.h:152
const api::HBounds elevation_bounds
Definition: multilane_line_road_curve_test.cc:34
void DescribeTo(std::ostream *os) const override
Definition: multilane_types_compare.h:153
int lane_id() const
Definition: builder.h:93
Wraps a LineOffset comparison into a MatcherInterface.
Definition: multilane_types_compare.h:171
::testing::AssertionResult IsEndpointXyClose(const EndpointXy &xy1, const EndpointXy &xy2, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.cc:15
EndReferenceSpecMatcher(const EndReference::Spec &end_reference, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.h:274
::testing::AssertionResult IsArcOffsetClose(const ArcOffset &arc_offset1, const ArcOffset &arc_offset2, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.cc:148
HBoundsMatcher(const api::HBounds &elevation_bounds, double tolerance)
Definition: multilane_types_compare.h:116
const EndpointZ & endpoint_z() const
Definition: builder.h:229
bool MatchAndExplain(const StartLane::Spec &other, MatchResultListener *) const override
Definition: multilane_types_compare.h:307
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:360
double tolerance_
Definition: model_test.cc:276
::testing::AssertionResult IsCubicPolynomialClose(const CubicPolynomial &cubic1, const CubicPolynomial &cubic2, double tolerance)
Definition: multilane_types_compare.cc:180
const Endpoint & endpoint() const
Definition: builder.h:91
int ref_lane() const
Definition: builder.h:336
Wraps a StartReference::Spec comparison into a MatcherInterface.
Definition: multilane_types_compare.h:237
Defines how a Connection&#39;s reference curve ends.
Definition: builder.h:169
void DescribeTo(std::ostream *os) const override
Definition: multilane_types_compare.h:222
Specification for path offset along a circular arc.
Definition: connection.h:178
void DescribeTo(std::ostream *os) const override
Definition: multilane_types_compare.h:353
Wraps an ArcOffset comparison into a MatcherInterface.
Definition: multilane_types_compare.h:139
Matcher< const api::HBounds & > Matches(const api::HBounds &elevation_bounds, double tolerance)
Definition: multilane_types_compare.cc:212
Defines how a Connection&#39;s lane curve starts.
Definition: builder.h:87
ArcOffsetMatcher(const ArcOffset &arc_offset, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.h:141
const EndpointZ & endpoint_z() const
Definition: builder.h:173
bool MatchAndExplain(const EndLane::Spec &other, MatchResultListener *) const override
Definition: multilane_types_compare.h:347
StartLaneSpecMatcher(const StartLane::Spec &start_lane, double linear_tolerance, double angular_tolerance)
Definition: multilane_types_compare.h:301
LaneLayoutMatcher(const LaneLayout &lane_layout, double tolerance)
Definition: multilane_types_compare.h:199
bool MatchAndExplain(const EndReference::Spec &other, MatchResultListener *) const override
Definition: multilane_types_compare.h:280
const double tolerance
Definition: dynamic_bicycle_car_test.cc:42
bool MatchAndExplain(const LaneLayout &other, MatchResultListener *) const override
Definition: multilane_types_compare.h:202
const Endpoint & endpoint() const
Definition: builder.h:36