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); } diff --git a/src/Node_TEST.cc b/src/Node_TEST.cc new file mode 100644 index 000000000..f8f913b82 --- /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/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 +{ + /// \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(); +}