From 30764772bc19732907555a059fd2d7cd4e98f1c9 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 26 Oct 2020 19:03:00 -0700 Subject: [PATCH 1/3] check nan pose Signed-off-by: Ian Chen --- include/ignition/rendering/base/BaseNode.hh | 8 ++++++++ include/ignition/rendering/base/BaseVisual.hh | 8 ++++++++ 2 files changed, 16 insertions(+) diff --git a/include/ignition/rendering/base/BaseNode.hh b/include/ignition/rendering/base/BaseNode.hh index dbf8b591e..eb76ba456 100644 --- 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 index 1d6c0392a..c16f09db2 100644 --- 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); } From b138b87be481b779b4a676e30e6cb082ee1b962e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 26 Oct 2020 19:05:04 -0700 Subject: [PATCH 2/3] add test Signed-off-by: Ian Chen --- src/Node_TEST.cc | 141 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 141 insertions(+) create mode 100644 src/Node_TEST.cc diff --git a/src/Node_TEST.cc b/src/Node_TEST.cc new file mode 100644 index 000000000..a3d18ac38 --- /dev/null +++ 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 +#include + +#include + +#include "test_config.h" // NOLINT(build/include) + +#include "ignition/rendering/RenderEngine.hh" +#include "ignition/rendering/RenderingIface.hh" +#include "ignition/rendering/Scene.hh" +#include "ignition/rendering/Node.hh" +#include "ignition/rendering/Visual.hh" + +using namespace ignition; +using namespace rendering; + +class NodeTest : public testing::Test, + public testing::WithParamInterface +{ + /// \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(); +} From 026b4f1f9339a00100d2e9e1c1e5fcae9dc23be5 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 26 Oct 2020 19:05:55 -0700 Subject: [PATCH 3/3] rearrage include Signed-off-by: Ian Chen --- src/Node_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Node_TEST.cc b/src/Node_TEST.cc index a3d18ac38..f8f913b82 100644 --- a/src/Node_TEST.cc +++ b/src/Node_TEST.cc @@ -22,10 +22,10 @@ #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/Node.hh" #include "ignition/rendering/Visual.hh" using namespace ignition;