3 to 4

Signed-off-by: Ashton Larkin <[email protected]>

include/ignition/rendering/base/BaseNode.hh

@@ -312,6 +312,14 @@ namespace ignition
     {
       math::Pose3d pose = _pose;
       pose.Pos() = pose.Pos() - pose.Rot() * this->origin;
+
+      if (!pose.IsFinite())
+      {
+        ignerr << "Unable to set pose of a node: "
+               << "non-finite (nan, inf) values detected." << std::endl;
+        return;
+      }
+
       this->SetRawLocalPose(pose);
     }
 

include/ignition/rendering/base/BaseVisual.hh

@@ -167,6 +167,14 @@ namespace ignition
       math::Pose3d rawPose = _pose;
       math::Vector3d scale = this->LocalScale();
       rawPose.Pos() -= rawPose.Rot() * (scale * this->origin);
+
+      if (!rawPose.IsFinite())
+      {
+        ignerr << "Unable to set pose of a node: "
+               << "non-finite (nan, inf) values detected." << std::endl;
+        return;
+      }
+
       this->SetRawLocalPose(rawPose);
     }
 

ogre/src/OgreRenderEngine.cc

@@ -497,8 +497,11 @@ void OgreRenderEngine::CreateRenderSystem()
     renderSys = rsList->at(c);
     c++;
   }
-  while (renderSys &&
-         renderSys->getName().compare("OpenGL Rendering Subsystem") != 0);
+  // cpplint has a false positive when extending a while call to multiple lines
+  // (it thinks the while loop is empty), so we must put the whole while
+  // statement on one line and add NOLINT at the end so that cpplint doesn't
+  // complain about the line being too long
+  while (renderSys && renderSys->getName().compare("OpenGL Rendering Subsystem") != 0); // NOLINT
 
   if (renderSys == nullptr)
   {

ogre2/src/Ogre2RenderEngine.cc

@@ -480,8 +480,11 @@ void Ogre2RenderEngine::CreateRenderSystem()
     renderSys = rsList->at(c);
     c++;
   }
-  while (renderSys &&
-         renderSys->getName().compare("OpenGL 3+ Rendering Subsystem") != 0);
+  // cpplint has a false positive when extending a while call to multiple lines
+  // (it thinks the while loop is empty), so we must put the whole while
+  // statement on one line and add NOLINT at the end so that cpplint doesn't
+  // complain about the line being too long
+  while (renderSys && renderSys->getName().compare("OpenGL 3+ Rendering Subsystem") != 0); // NOLINT
 
   if (renderSys == nullptr)
   {

src/Node_TEST.cc

@@ -0,0 +1,141 @@
+/*
+ * Copyright (C) 2020 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 <string>
+
+#include <ignition/common/Console.hh>
+
+#include "test_config.h"  // NOLINT(build/include)
+
+#include "ignition/rendering/Node.hh"
+#include "ignition/rendering/RenderEngine.hh"
+#include "ignition/rendering/RenderingIface.hh"
+#include "ignition/rendering/Scene.hh"
+#include "ignition/rendering/Visual.hh"
+
+using namespace ignition;
+using namespace rendering;
+
+class NodeTest : public testing::Test,
+                 public testing::WithParamInterface<const char *>
+{
+  /// \brief Test visual material
+  public: void Pose(const std::string &_renderEngine);
+};
+
+/////////////////////////////////////////////////
+void NodeTest::Pose(const std::string &_renderEngine)
+{
+  RenderEngine *engine = rendering::engine(_renderEngine);
+  if (!engine)
+  {
+    igndbg << "Engine '" << _renderEngine
+              << "' is not supported" << std::endl;
+    return;
+  }
+
+  ScenePtr scene = engine->CreateScene("scene");
+
+  // create visual
+  NodePtr node = scene->CreateVisual();
+  ASSERT_NE(nullptr, node);
+
+  // check initial pose
+  EXPECT_EQ(math::Pose3d(), node->LocalPose());
+  EXPECT_EQ(math::Vector3d(), node->LocalPosition());
+  EXPECT_EQ(math::Quaterniond(), node->LocalRotation());
+
+  EXPECT_EQ(math::Pose3d(), node->WorldPose());
+  EXPECT_EQ(math::Vector3d(), node->WorldPosition());
+  EXPECT_EQ(math::Quaterniond(), node->WorldRotation());
+
+  // set node pose, position, and quaternion
+  node->SetLocalPose(math::Pose3d(1, 2, 3, 0, 1.57, 1.57));
+  EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 1.57, 1.57), node->LocalPose());
+  EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 1.57, 1.57), node->WorldPose());
+
+  node->SetLocalPosition(math::Vector3d(3, 4, 5));
+  EXPECT_EQ(math::Vector3d(3, 4, 5), node->LocalPosition());
+  EXPECT_EQ(math::Vector3d(3, 4, 5), node->WorldPosition());
+
+  node->SetLocalRotation(math::Quaterniond(math::Vector3d(0.3, 0.1, 0.2)));
+  EXPECT_EQ(math::Quaterniond(math::Vector3d(0.3, 0.1, 0.2)),
+      node->LocalRotation());
+  EXPECT_EQ(math::Quaterniond(math::Vector3d(0.3, 0.1, 0.2)),
+      node->WorldRotation());
+
+  node->SetWorldPose(math::Pose3d(-1, -2, -3, 0, -1.57, -1.57));
+  EXPECT_EQ(math::Pose3d(-1, -2, -3, 0, -1.57, -1.57), node->WorldPose());
+  EXPECT_EQ(math::Pose3d(-1, -2, -3, 0, -1.57, -1.57), node->LocalPose());
+
+  node->SetWorldPosition(math::Vector3d(-3, -4, -5));
+  EXPECT_EQ(math::Vector3d(-3, -4, -5), node->WorldPosition());
+  EXPECT_EQ(math::Vector3d(-3, -4, -5), node->LocalPosition());
+
+  node->SetWorldRotation(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)));
+  EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
+      node->WorldRotation());
+  EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
+      node->LocalRotation());
+
+  // set NAN and inf values. verify they are not set
+  node->SetLocalPose(math::Pose3d(1, NAN, 3, 0, math::INF_D, 1.57));
+  EXPECT_EQ(math::Pose3d(-3, -4, -5, -0.3, -0.1, -0.2), node->LocalPose());
+  EXPECT_EQ(math::Pose3d(-3, -4, -5, -0.3, -0.1, -0.2), node->WorldPose());
+  node->SetWorldPose(math::Pose3d(1, NAN, 3, 0, math::INF_D, 1.57));
+  EXPECT_EQ(math::Pose3d(-3, -4, -5, -0.3, -0.1, -0.2), node->LocalPose());
+  EXPECT_EQ(math::Pose3d(-3, -4, -5, -0.3, -0.1, -0.2), node->WorldPose());
+
+  node->SetLocalPosition(math::Vector3d(NAN, 4, 5));
+  EXPECT_EQ(math::Vector3d(-3, -4, -5), node->WorldPosition());
+  EXPECT_EQ(math::Vector3d(-3, -4, -5), node->LocalPosition());
+  node->SetWorldPosition(math::Vector3d(NAN, 4, 5));
+  EXPECT_EQ(math::Vector3d(-3, -4, -5), node->WorldPosition());
+  EXPECT_EQ(math::Vector3d(-3, -4, -5), node->LocalPosition());
+
+  node->SetLocalRotation(math::Quaterniond(math::Vector3d(NAN, 0.4, 1.5)));
+  EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
+      node->LocalRotation());
+  EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
+      node->WorldRotation());
+  node->SetWorldRotation(math::Quaterniond(math::Vector3d(NAN, 0.4, 1.5)));
+  EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
+      node->WorldRotation());
+  EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
+      node->LocalRotation());
+
+  // Clean up
+  engine->DestroyScene(scene);
+  rendering::unloadEngine(engine->Name());
+}
+
+/////////////////////////////////////////////////
+TEST_P(NodeTest, Pose)
+{
+  Pose(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(Node, NodeTest,
+    RENDER_ENGINE_VALUES,
+    ignition::rendering::PrintToStringParam());
+
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/integration/camera.cc

@@ -230,30 +230,44 @@ void CameraTest::VisualAt(const std::string &_renderEngine)
   camera->SetHFOV(IGN_PI / 2);
   root->AddChild(camera);
 
-  // render a frame
-  camera->Update();
+  // render a few frames
+  for (auto i = 0; i < 30; ++i)
+  {
+    camera->Update();
+  }
+
+  EXPECT_EQ(800u, camera->ImageWidth());
+  EXPECT_EQ(600u, camera->ImageHeight());
 
   for (auto x = 0u; x < camera->ImageWidth(); x = x + 100)
   {
     auto vis = camera->VisualAt(math::Vector2i(x, camera->ImageHeight() / 2));
 
     if (x <= 100)
     {
-     EXPECT_EQ(nullptr, vis);
+      EXPECT_EQ(nullptr, vis);
     }
     else if (x > 100 && x <= 300)
     {
-      ASSERT_NE(nullptr, vis);
-      EXPECT_EQ("sphere", vis->Name());
+      // Don't end test here on failure, this condition is flaky
+      EXPECT_NE(nullptr, vis) << x;
+      if (vis)
+      {
+        EXPECT_EQ("sphere", vis->Name());
+      }
     }
     else if (x > 300 && x <= 400)
     {
       EXPECT_EQ(nullptr, vis);
     }
     else if (x > 400 && x <= 700)
     {
-      ASSERT_NE(nullptr, vis);
-      EXPECT_EQ("box", vis->Name());
+      // Don't end test here on failure, this condition is flaky
+      EXPECT_NE(nullptr, vis) << x;
+      if (vis)
+      {
+        EXPECT_EQ("box", vis->Name());
+      }
     }
     else
     {