Skip to content

Commit 1233f20

Browse files
iche033iche033
authored
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]>
1 parent bf3bbb0 commit 1233f20

File tree

3 files changed

+157
-0
lines changed

3 files changed

+157
-0
lines changed

‎include/ignition/rendering/base/BaseNode.hh

+8
Original file line numberDiff line numberDiff line change
@@ -312,6 +312,14 @@ namespace ignition
312312
{
313313
math::Pose3d pose = _pose;
314314
pose.Pos() = pose.Pos() - pose.Rot() * this->origin;
315+
316+
if (!pose.IsFinite())
317+
{
318+
ignerr << "Unable to set pose of a node: "
319+
<< "non-finite (nan, inf) values detected." << std::endl;
320+
return;
321+
}
322+
315323
this->SetRawLocalPose(pose);
316324
}
317325

‎include/ignition/rendering/base/BaseVisual.hh

+8
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,14 @@ namespace ignition
138138
math::Pose3d rawPose = _pose;
139139
math::Vector3d scale = this->LocalScale();
140140
rawPose.Pos() -= rawPose.Rot() * (scale * this->origin);
141+
142+
if (!rawPose.IsFinite())
143+
{
144+
ignerr << "Unable to set pose of a node: "
145+
<< "non-finite (nan, inf) values detected." << std::endl;
146+
return;
147+
}
148+
141149
this->SetRawLocalPose(rawPose);
142150
}
143151

‎src/Node_TEST.cc

+141
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
/*
2+
* Copyright (C) 2020 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include <gtest/gtest.h>
19+
#include <string>
20+
21+
#include <ignition/common/Console.hh>
22+
23+
#include "test_config.h" // NOLINT(build/include)
24+
25+
#include "ignition/rendering/Node.hh"
26+
#include "ignition/rendering/RenderEngine.hh"
27+
#include "ignition/rendering/RenderingIface.hh"
28+
#include "ignition/rendering/Scene.hh"
29+
#include "ignition/rendering/Visual.hh"
30+
31+
using namespace ignition;
32+
using namespace rendering;
33+
34+
class NodeTest : public testing::Test,
35+
public testing::WithParamInterface<const char *>
36+
{
37+
/// \brief Test visual material
38+
public: void Pose(const std::string &_renderEngine);
39+
};
40+
41+
/////////////////////////////////////////////////
42+
void NodeTest::Pose(const std::string &_renderEngine)
43+
{
44+
RenderEngine *engine = rendering::engine(_renderEngine);
45+
if (!engine)
46+
{
47+
igndbg << "Engine '" << _renderEngine
48+
<< "' is not supported" << std::endl;
49+
return;
50+
}
51+
52+
ScenePtr scene = engine->CreateScene("scene");
53+
54+
// create visual
55+
NodePtr node = scene->CreateVisual();
56+
ASSERT_NE(nullptr, node);
57+
58+
// check initial pose
59+
EXPECT_EQ(math::Pose3d(), node->LocalPose());
60+
EXPECT_EQ(math::Vector3d(), node->LocalPosition());
61+
EXPECT_EQ(math::Quaterniond(), node->LocalRotation());
62+
63+
EXPECT_EQ(math::Pose3d(), node->WorldPose());
64+
EXPECT_EQ(math::Vector3d(), node->WorldPosition());
65+
EXPECT_EQ(math::Quaterniond(), node->WorldRotation());
66+
67+
// set node pose, position, and quaternion
68+
node->SetLocalPose(math::Pose3d(1, 2, 3, 0, 1.57, 1.57));
69+
EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 1.57, 1.57), node->LocalPose());
70+
EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 1.57, 1.57), node->WorldPose());
71+
72+
node->SetLocalPosition(math::Vector3d(3, 4, 5));
73+
EXPECT_EQ(math::Vector3d(3, 4, 5), node->LocalPosition());
74+
EXPECT_EQ(math::Vector3d(3, 4, 5), node->WorldPosition());
75+
76+
node->SetLocalRotation(math::Quaterniond(math::Vector3d(0.3, 0.1, 0.2)));
77+
EXPECT_EQ(math::Quaterniond(math::Vector3d(0.3, 0.1, 0.2)),
78+
node->LocalRotation());
79+
EXPECT_EQ(math::Quaterniond(math::Vector3d(0.3, 0.1, 0.2)),
80+
node->WorldRotation());
81+
82+
node->SetWorldPose(math::Pose3d(-1, -2, -3, 0, -1.57, -1.57));
83+
EXPECT_EQ(math::Pose3d(-1, -2, -3, 0, -1.57, -1.57), node->WorldPose());
84+
EXPECT_EQ(math::Pose3d(-1, -2, -3, 0, -1.57, -1.57), node->LocalPose());
85+
86+
node->SetWorldPosition(math::Vector3d(-3, -4, -5));
87+
EXPECT_EQ(math::Vector3d(-3, -4, -5), node->WorldPosition());
88+
EXPECT_EQ(math::Vector3d(-3, -4, -5), node->LocalPosition());
89+
90+
node->SetWorldRotation(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)));
91+
EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
92+
node->WorldRotation());
93+
EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
94+
node->LocalRotation());
95+
96+
// set NAN and inf values. verify they are not set
97+
node->SetLocalPose(math::Pose3d(1, NAN, 3, 0, math::INF_D, 1.57));
98+
EXPECT_EQ(math::Pose3d(-3, -4, -5, -0.3, -0.1, -0.2), node->LocalPose());
99+
EXPECT_EQ(math::Pose3d(-3, -4, -5, -0.3, -0.1, -0.2), node->WorldPose());
100+
node->SetWorldPose(math::Pose3d(1, NAN, 3, 0, math::INF_D, 1.57));
101+
EXPECT_EQ(math::Pose3d(-3, -4, -5, -0.3, -0.1, -0.2), node->LocalPose());
102+
EXPECT_EQ(math::Pose3d(-3, -4, -5, -0.3, -0.1, -0.2), node->WorldPose());
103+
104+
node->SetLocalPosition(math::Vector3d(NAN, 4, 5));
105+
EXPECT_EQ(math::Vector3d(-3, -4, -5), node->WorldPosition());
106+
EXPECT_EQ(math::Vector3d(-3, -4, -5), node->LocalPosition());
107+
node->SetWorldPosition(math::Vector3d(NAN, 4, 5));
108+
EXPECT_EQ(math::Vector3d(-3, -4, -5), node->WorldPosition());
109+
EXPECT_EQ(math::Vector3d(-3, -4, -5), node->LocalPosition());
110+
111+
node->SetLocalRotation(math::Quaterniond(math::Vector3d(NAN, 0.4, 1.5)));
112+
EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
113+
node->LocalRotation());
114+
EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
115+
node->WorldRotation());
116+
node->SetWorldRotation(math::Quaterniond(math::Vector3d(NAN, 0.4, 1.5)));
117+
EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
118+
node->WorldRotation());
119+
EXPECT_EQ(math::Quaterniond(math::Vector3d(-0.3, -0.1, -0.2)),
120+
node->LocalRotation());
121+
122+
// Clean up
123+
engine->DestroyScene(scene);
124+
rendering::unloadEngine(engine->Name());
125+
}
126+
127+
/////////////////////////////////////////////////
128+
TEST_P(NodeTest, Pose)
129+
{
130+
Pose(GetParam());
131+
}
132+
133+
INSTANTIATE_TEST_CASE_P(Node, NodeTest,
134+
RENDER_ENGINE_VALUES,
135+
ignition::rendering::PrintToStringParam());
136+
137+
int main(int argc, char **argv)
138+
{
139+
::testing::InitGoogleTest(&argc, argv);
140+
return RUN_ALL_TESTS();
141+
}

0 commit comments

Comments
 (0)