From ef1012d9170f962756d8dfa7668d6a43a099eea9 Mon Sep 17 00:00:00 2001
From: Steven Peters <scpeters@openrobotics.org>
Date: Tue, 13 Jul 2021 22:25:20 -0700
Subject: [PATCH] LinkFeatures_TEST: check GetGravity in LoadWorld

Requires moving definition of AssertVectorApprox

Signed-off-by: Steven Peters <scpeters@openrobotics.org>
---
 dartsim/src/LinkFeatures_TEST.cc | 45 ++++++++++++++++++--------------
 1 file changed, 25 insertions(+), 20 deletions(-)

diff --git a/dartsim/src/LinkFeatures_TEST.cc b/dartsim/src/LinkFeatures_TEST.cc
index b2f0a3516..31df9a83d 100644
--- a/dartsim/src/LinkFeatures_TEST.cc
+++ b/dartsim/src/LinkFeatures_TEST.cc
@@ -74,26 +74,6 @@ class LinkFeaturesFixture : public ::testing::Test
   protected: TestEnginePtr engine;
 };
 
-TestWorldPtr LoadWorld(
-    const TestEnginePtr &_engine,
-    const std::string &_sdfFile,
-    const Eigen::Vector3d &_gravity = Eigen::Vector3d{0, 0, -9.8})
-{
-  sdf::Root root;
-  const sdf::Errors errors = root.Load(_sdfFile);
-  EXPECT_TRUE(errors.empty()) << errors;
-  const sdf::World *sdfWorld = root.WorldByIndex(0);
-  EXPECT_NE(nullptr, sdfWorld);
-
-  auto graphErrors = sdfWorld->ValidateGraphs();
-  EXPECT_EQ(0u, graphErrors.size()) << graphErrors;
-
-  TestWorldPtr world = _engine->ConstructWorld(*sdfWorld);
-  EXPECT_NE(nullptr, world);
-  world->SetGravity(_gravity);
-  return world;
-}
-
 // A predicate-formatter for asserting that two vectors are approximately equal.
 class AssertVectorApprox
 {
@@ -117,6 +97,31 @@ class AssertVectorApprox
   private: double tol;
 };
 
+TestWorldPtr LoadWorld(
+    const TestEnginePtr &_engine,
+    const std::string &_sdfFile,
+    const Eigen::Vector3d &_gravity = Eigen::Vector3d{0, 0, -9.8})
+{
+  sdf::Root root;
+  const sdf::Errors errors = root.Load(_sdfFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+  const sdf::World *sdfWorld = root.WorldByIndex(0);
+  EXPECT_NE(nullptr, sdfWorld);
+
+  auto graphErrors = sdfWorld->ValidateGraphs();
+  EXPECT_EQ(0u, graphErrors.size()) << graphErrors;
+
+  TestWorldPtr world = _engine->ConstructWorld(*sdfWorld);
+  EXPECT_NE(nullptr, world);
+  world->SetGravity(_gravity);
+
+  AssertVectorApprox vectorPredicate(1e-10);
+  EXPECT_PRED_FORMAT2(vectorPredicate, _gravity,
+                      world->GetGravity());
+
+  return world;
+}
+
 // Test setting force and torque.
 TEST_F(LinkFeaturesFixture, LinkForceTorque)
 {