|
26 | 26 | #include "transmission_interface/four_bar_linkage_transmission_loader.hpp" |
27 | 27 | #include "transmission_interface/transmission_loader.hpp" |
28 | 28 |
|
| 29 | +using testing::DoubleNear; |
29 | 30 | using testing::SizeIs; |
30 | 31 |
|
| 32 | +// Floating-point value comparison threshold |
| 33 | +const double EPS = 1e-5; |
| 34 | + |
31 | 35 | class TransmissionPluginLoader |
32 | 36 | { |
33 | 37 | public: |
@@ -113,17 +117,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) |
113 | 117 |
|
114 | 118 | const std::vector<double> & actuator_reduction = |
115 | 119 | four_bar_linkage_transmission->get_actuator_reduction(); |
116 | | - EXPECT_EQ(50.0, actuator_reduction[0]); |
117 | | - EXPECT_EQ(-50.0, actuator_reduction[1]); |
| 120 | + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); |
| 121 | + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); |
118 | 122 |
|
119 | 123 | const std::vector<double> & joint_reduction = |
120 | 124 | four_bar_linkage_transmission->get_joint_reduction(); |
121 | | - EXPECT_EQ(2.0, joint_reduction[0]); |
122 | | - EXPECT_EQ(-2.0, joint_reduction[1]); |
| 125 | + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); |
| 126 | + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); |
123 | 127 |
|
124 | 128 | const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(); |
125 | | - EXPECT_EQ(0.5, joint_offset[0]); |
126 | | - EXPECT_EQ(-0.5, joint_offset[1]); |
| 129 | + EXPECT_THAT(0.5, DoubleNear(joint_offset[0], EPS)); |
| 130 | + EXPECT_THAT(-0.5, DoubleNear(joint_offset[1], EPS)); |
127 | 131 | } |
128 | 132 |
|
129 | 133 | TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) |
@@ -185,17 +189,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) |
185 | 189 |
|
186 | 190 | const std::vector<double> & actuator_reduction = |
187 | 191 | four_bar_linkage_transmission->get_actuator_reduction(); |
188 | | - EXPECT_EQ(50.0, actuator_reduction[0]); |
189 | | - EXPECT_EQ(-50.0, actuator_reduction[1]); |
| 192 | + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); |
| 193 | + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); |
190 | 194 |
|
191 | 195 | const std::vector<double> & joint_reduction = |
192 | 196 | four_bar_linkage_transmission->get_joint_reduction(); |
193 | | - EXPECT_EQ(1.0, joint_reduction[0]); |
194 | | - EXPECT_EQ(1.0, joint_reduction[1]); |
| 197 | + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); |
| 198 | + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); |
195 | 199 |
|
196 | 200 | const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(); |
197 | | - EXPECT_EQ(0.0, joint_offset[0]); |
198 | | - EXPECT_EQ(0.0, joint_offset[1]); |
| 201 | + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); |
| 202 | + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); |
199 | 203 | } |
200 | 204 |
|
201 | 205 | TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) |
@@ -249,17 +253,17 @@ TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) |
249 | 253 |
|
250 | 254 | const std::vector<double> & actuator_reduction = |
251 | 255 | four_bar_linkage_transmission->get_actuator_reduction(); |
252 | | - EXPECT_EQ(1.0, actuator_reduction[0]); |
253 | | - EXPECT_EQ(1.0, actuator_reduction[1]); |
| 256 | + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); |
| 257 | + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); |
254 | 258 |
|
255 | 259 | const std::vector<double> & joint_reduction = |
256 | 260 | four_bar_linkage_transmission->get_joint_reduction(); |
257 | | - EXPECT_EQ(1.0, joint_reduction[0]); |
258 | | - EXPECT_EQ(1.0, joint_reduction[1]); |
| 261 | + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); |
| 262 | + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); |
259 | 263 |
|
260 | 264 | const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(); |
261 | | - EXPECT_EQ(0.0, joint_offset[0]); |
262 | | - EXPECT_EQ(0.0, joint_offset[1]); |
| 265 | + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); |
| 266 | + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); |
263 | 267 | } |
264 | 268 |
|
265 | 269 | TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) |
@@ -322,17 +326,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) |
322 | 326 | // default kicks in for ill-defined values |
323 | 327 | const std::vector<double> & actuator_reduction = |
324 | 328 | four_bar_linkage_transmission->get_actuator_reduction(); |
325 | | - EXPECT_EQ(1.0, actuator_reduction[0]); |
326 | | - EXPECT_EQ(1.0, actuator_reduction[1]); |
| 329 | + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); |
| 330 | + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); |
327 | 331 |
|
328 | 332 | const std::vector<double> & joint_reduction = |
329 | 333 | four_bar_linkage_transmission->get_joint_reduction(); |
330 | | - EXPECT_EQ(1.0, joint_reduction[0]); |
331 | | - EXPECT_EQ(1.0, joint_reduction[1]); |
| 334 | + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); |
| 335 | + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); |
332 | 336 |
|
333 | 337 | const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(); |
334 | | - EXPECT_EQ(0.0, joint_offset[0]); |
335 | | - EXPECT_EQ(0.0, joint_offset[1]); |
| 338 | + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); |
| 339 | + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); |
336 | 340 | } |
337 | 341 |
|
338 | 342 | TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) |
@@ -397,18 +401,18 @@ TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) |
397 | 401 | // default kicks in for ill-defined values |
398 | 402 | const std::vector<double> & actuator_reduction = |
399 | 403 | four_bar_linkage_transmission->get_actuator_reduction(); |
400 | | - EXPECT_EQ(50.0, actuator_reduction[0]); |
401 | | - EXPECT_EQ(-50.0, actuator_reduction[1]); |
| 404 | + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); |
| 405 | + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); |
402 | 406 |
|
403 | 407 | const std::vector<double> & joint_reduction = |
404 | 408 | four_bar_linkage_transmission->get_joint_reduction(); |
405 | | - EXPECT_EQ(2.0, joint_reduction[0]); |
406 | | - EXPECT_EQ(-2.0, joint_reduction[1]); |
| 409 | + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); |
| 410 | + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); |
407 | 411 |
|
408 | 412 | // default kicks in for ill-defined values |
409 | 413 | const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(); |
410 | | - EXPECT_EQ(0.0, joint_offset[0]); |
411 | | - EXPECT_EQ(0.0, joint_offset[1]); |
| 414 | + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); |
| 415 | + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); |
412 | 416 | } |
413 | 417 |
|
414 | 418 | TEST(FourBarLinkageTransmissionLoaderTest, mech_red_invalid_value) |
|
0 commit comments