Added MoveToHelper test

Signed-off-by: ahcorde <ahcorde@gmail.com>

src/MoveToHelper_TEST.cc

@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include <gtest/gtest.h>
+
+#include <chrono>
+
+#include <ignition/rendering/MoveToHelper.hh>
+#include <ignition/common/Console.hh>
+
+#include "test_config.h"  // NOLINT(build/include)
+
+#include "ignition/rendering/RenderEngine.hh"
+#include "ignition/rendering/RenderingIface.hh"
+#include "ignition/rendering/Scene.hh"
+
+using namespace ignition;
+using namespace rendering;
+
+class MoveToHelperTest : public testing::Test,
+                 public testing::WithParamInterface<const char *>
+{
+  public: void MoveTo(const std::string &_renderEngine);
+
+  public: void OnMoveToComplete();
+
+  public: void checkIsCompleted();
+
+  public: MoveToHelper moveToHelper;
+
+  public: bool isMoveCompleted = false;
+};
+
+void MoveToHelperTest::OnMoveToComplete()
+{
+  isMoveCompleted = true;
+}
+
+void MoveToHelperTest::checkIsCompleted()
+{
+  isMoveCompleted = false;
+
+  std::chrono::time_point<std::chrono::system_clock> startTime;
+  std::chrono::time_point<std::chrono::system_clock> endTime;
+
+  startTime = std::chrono::system_clock::now();
+
+  while (!isMoveCompleted)
+  {
+    moveToHelper.AddTime(0.01);
+    endTime = std::chrono::system_clock::now();
+
+    auto seconds =
+      std::chrono::duration_cast<std::chrono::milliseconds>(
+        endTime - startTime).count() / 1000.0;
+    if (seconds > 0.5)
+      break;
+  }
+}
+
+void MoveToHelperTest::MoveTo(const std::string &_renderEngine)
+{
+  // create and populate scene
+  RenderEngine *engine = rendering::engine(_renderEngine);
+  if (!engine)
+  {
+    igndbg << "Engine '" << _renderEngine
+              << "' is not supported" << std::endl;
+    return;
+  }
+  ScenePtr scene = engine->CreateScene("scene");
+  ASSERT_NE(nullptr, scene);
+
+  VisualPtr root = scene->RootVisual();
+  ASSERT_NE(nullptr, root);
+
+  // create center visual
+  VisualPtr center = scene->CreateVisual("center");
+  ASSERT_NE(nullptr, center);
+  center->AddGeometry(scene->CreateSphere());
+  center->SetLocalPosition(30, 0, 0);
+  center->SetLocalScale(0.1, 0.1, 0.1);
+  root->AddChild(center);
+
+  CameraPtr camera(scene->CreateCamera());
+  ASSERT_NE(nullptr, camera);
+
+  camera->SetLocalPosition(0.0, 0.0, 0.0);
+  camera->SetLocalRotation(0.0, 0.0, 0.0);
+  camera->SetImageWidth(800);
+  camera->SetImageHeight(600);
+  camera->SetAntiAliasing(2);
+  camera->SetAspectRatio(1.333);
+  camera->SetHFOV(IGN_PI / 2);
+  root->AddChild(camera);
+
+  ASSERT_NE(nullptr, camera);
+
+  rendering::NodePtr target = scene->NodeByName("center");
+
+  moveToHelper.MoveTo(camera, target, 0.5,
+    std::bind(&MoveToHelperTest::OnMoveToComplete, this));
+  EXPECT_FALSE(moveToHelper.Idle());
+  checkIsCompleted();
+  EXPECT_TRUE(moveToHelper.Idle());
+  EXPECT_EQ(math::Vector3d(28.5, 0.0, 0.0), camera->LocalPosition());
+
+  moveToHelper.MoveTo(camera, math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), 0.5,
+    std::bind(&MoveToHelperTest::OnMoveToComplete, this));
+  EXPECT_FALSE(moveToHelper.Idle());
+  checkIsCompleted();
+  EXPECT_EQ(math::Vector3d(0.0, 0.0, 0.0), camera->LocalPosition());
+  EXPECT_TRUE(moveToHelper.Idle());
+
+  math::Vector3d lookAt(0, 0, 1);
+  math::Vector3d viewAngleDirection(0, 1, 1);
+  moveToHelper.LookDirection(camera,
+      viewAngleDirection, lookAt,
+      0.5, std::bind(&MoveToHelperTest::OnMoveToComplete, this));
+  EXPECT_FALSE(moveToHelper.Idle());
+  checkIsCompleted();
+  EXPECT_TRUE(moveToHelper.Idle());
+  EXPECT_EQ(math::Vector3d(0.0, -1, 0.0), camera->LocalPosition());
+  EXPECT_EQ(math::Quaterniond(0.0, -0.785398, 1.5708), camera->LocalRotation());
+
+  engine->DestroyScene(scene);
+  rendering::unloadEngine(engine->Name());
+}
+
+/////////////////////////////////////////////////
+TEST_P(MoveToHelperTest, MoveToHelper)
+{
+  MoveTo(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(MoveToHelper, MoveToHelperTest,
+    RENDER_ENGINE_VALUES,
+    ignition::rendering::PrintToStringParam());
+
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}