Fix crash due to NaN pose values (#169)

* check nan pose Signed-off-by: Ian Chen <[email protected]> * add test Signed-off-by: Ian Chen <[email protected]> * rearrage include Signed-off-by: Ian Chen <[email protected]>
gazebosim · Oct 27, 2020 · 1233f20 · 1233f20
1 parent bf3bbb0
commit 1233f20
Show file tree

Hide file tree

Showing 3 changed files with 157 additions and 0 deletions.
diff --git a/include/ignition/rendering/base/BaseNode.hh b/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);
     }
 

diff --git a/include/ignition/rendering/base/BaseVisual.hh b/include/ignition/rendering/base/BaseVisual.hh
@@ -138,6 +138,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);
     }
 

diff --git a/src/Node_TEST.cc b/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();
+}