Skip to content

Commit 345f551

Browse files
saikishorscpeters
saikishor
and
scpeters
authored
Add invalid data checks to the Geometry data (#242)
* Add invalid data checks * Extend invalid data checks to other geometry entities * Add conditioning based on the URDF version Signed-off-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com> Co-authored-by: Steve Peters <scpeters@openrobotics.org>
1 parent 7eba5cc commit 345f551

3 files changed

Lines changed: 445 additions & 15 deletions

File tree

‎urdf_parser/src/link.cpp‎

Lines changed: 41 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -107,7 +107,8 @@ bool parseMaterial(Material &material, tinyxml2::XMLElement *config, bool only_n
107107
}
108108

109109

110-
bool parseSphere(Sphere &s, tinyxml2::XMLElement *c)
110+
bool parseSphere(Sphere &s, tinyxml2::XMLElement *c,
111+
const urdf_export_helpers::URDFVersion version)
111112
{
112113
s.clear();
113114

@@ -127,10 +128,17 @@ bool parseSphere(Sphere &s, tinyxml2::XMLElement *c)
127128
return false;
128129
}
129130

131+
if (version.at_least(1, 2) && (!std::isfinite(s.radius) || s.radius <= 0))
132+
{
133+
CONSOLE_BRIDGE_logError("Sphere radius must be a positive finite value");
134+
return false;
135+
}
136+
130137
return true;
131138
}
132139

133-
bool parseBox(Box &b, tinyxml2::XMLElement *c)
140+
bool parseBox(Box &b, tinyxml2::XMLElement *c,
141+
const urdf_export_helpers::URDFVersion version)
134142
{
135143
b.clear();
136144

@@ -150,10 +158,20 @@ bool parseBox(Box &b, tinyxml2::XMLElement *c)
150158
CONSOLE_BRIDGE_logError("%s", e.what());
151159
return false;
152160
}
161+
162+
const bool are_dim_finite = std::isfinite(b.dim.x) && std::isfinite(b.dim.y) && std::isfinite(b.dim.z);
163+
const bool are_dim_positive = b.dim.x > 0 && b.dim.y > 0 && b.dim.z > 0;
164+
if (version.at_least(1, 2) && (!are_dim_finite || !are_dim_positive))
165+
{
166+
CONSOLE_BRIDGE_logError("Box size must be positive finite values");
167+
return false;
168+
}
169+
153170
return true;
154171
}
155172

156-
bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c)
173+
bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c,
174+
const urdf_export_helpers::URDFVersion version)
157175
{
158176
y.clear();
159177

@@ -183,6 +201,13 @@ bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c)
183201
return false;
184202
}
185203

204+
if (version.at_least(1, 2) &&
205+
(!std::isfinite(y.length) || !std::isfinite(y.radius) || y.length <= 0 || y.radius <= 0))
206+
{
207+
CONSOLE_BRIDGE_logError("Cylinder length and radius must be positive finite values");
208+
return false;
209+
}
210+
186211
return true;
187212
}
188213

@@ -216,7 +241,8 @@ bool parseMesh(Mesh &m, tinyxml2::XMLElement *c)
216241
return true;
217242
}
218243

219-
bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem)
244+
bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem,
245+
const urdf_export_helpers::URDFVersion version)
220246
{
221247
c.clear();
222248

@@ -246,6 +272,13 @@ bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem)
246272
return false;
247273
}
248274

275+
if (version.at_least(1, 2) &&
276+
(!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length <= 0 || c.radius <= 0))
277+
{
278+
CONSOLE_BRIDGE_logError("Capsule length and radius must be positive finite values");
279+
return false;
280+
}
281+
249282
return true;
250283
}
251284

@@ -267,21 +300,21 @@ GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g,
267300
{
268301
Sphere *s = new Sphere();
269302
geom.reset(s);
270-
if (parseSphere(*s, shape))
303+
if (parseSphere(*s, shape, version))
271304
return geom;
272305
}
273306
else if (type_name == "box")
274307
{
275308
Box *b = new Box();
276309
geom.reset(b);
277-
if (parseBox(*b, shape))
310+
if (parseBox(*b, shape, version))
278311
return geom;
279312
}
280313
else if (type_name == "cylinder")
281314
{
282315
Cylinder *c = new Cylinder();
283316
geom.reset(c);
284-
if (parseCylinder(*c, shape))
317+
if (parseCylinder(*c, shape, version))
285318
return geom;
286319
}
287320
else if (type_name == "mesh")
@@ -300,7 +333,7 @@ GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g,
300333
else {
301334
Capsule *c = new Capsule();
302335
geom.reset(c);
303-
if (parseCapsule(*c, shape))
336+
if (parseCapsule(*c, shape, version))
304337
return geom;
305338
}
306339
}

‎urdf_parser/test/urdf_schema_capsule_geometry_test.cpp‎

Lines changed: 112 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -97,7 +97,29 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_ignored_version_1_0)
9797
EXPECT_TRUE(link->visual_array.empty());
9898
}
9999

100-
TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values)
100+
TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values_fails_v1_2)
101+
{
102+
std::string urdf_str = R"urdf(
103+
<robot name="capsule_zero_test" version="1.2">
104+
<link name="link1">
105+
<visual>
106+
<geometry>
107+
<capsule radius="0.0" length="0.0"/>
108+
</geometry>
109+
</visual>
110+
</link>
111+
</robot>
112+
)urdf";
113+
114+
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
115+
// Invalid radius/length causes geometry parsing to fail for version >= 1.2
116+
ASSERT_NE(nullptr, urdf);
117+
urdf::LinkConstSharedPtr link = urdf->getLink("link1");
118+
ASSERT_NE(nullptr, link);
119+
EXPECT_TRUE(link->visual_array.empty());
120+
}
121+
122+
TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values_allowed_pre_v1_2)
101123
{
102124
std::string urdf_str = R"urdf(
103125
<robot name="capsule_zero_test" version="1.1">
@@ -112,16 +134,99 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values)
112134
)urdf";
113135

114136
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
137+
// Zero radius/length is allowed for version < 1.2
138+
ASSERT_NE(nullptr, urdf);
139+
urdf::LinkConstSharedPtr link = urdf->getLink("link1");
140+
ASSERT_NE(nullptr, link);
141+
EXPECT_FALSE(link->visual_array.empty());
142+
}
143+
144+
TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_fails_v1_2)
145+
{
146+
std::string urdf_str = R"urdf(
147+
<robot name="capsule_negative_test" version="1.2">
148+
<link name="link1">
149+
<visual>
150+
<geometry>
151+
<capsule radius="-0.05" length="0.5"/>
152+
</geometry>
153+
</visual>
154+
</link>
155+
</robot>
156+
)urdf";
115157

158+
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
159+
// Negative radius causes geometry parsing to fail for version >= 1.2
116160
ASSERT_NE(nullptr, urdf);
117161
urdf::LinkConstSharedPtr link = urdf->getLink("link1");
118162
ASSERT_NE(nullptr, link);
163+
EXPECT_TRUE(link->visual_array.empty());
164+
}
119165

120-
std::shared_ptr<urdf::Capsule> capsule =
121-
std::dynamic_pointer_cast<urdf::Capsule>(link->visual_array[0]->geometry);
122-
ASSERT_NE(nullptr, capsule);
123-
EXPECT_DOUBLE_EQ(0.0, capsule->radius);
124-
EXPECT_DOUBLE_EQ(0.0, capsule->length);
166+
TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_allowed_pre_v1_2)
167+
{
168+
std::string urdf_str = R"urdf(
169+
<robot name="capsule_negative_test" version="1.1">
170+
<link name="link1">
171+
<visual>
172+
<geometry>
173+
<capsule radius="-0.05" length="0.5"/>
174+
</geometry>
175+
</visual>
176+
</link>
177+
</robot>
178+
)urdf";
179+
180+
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
181+
// Negative radius is allowed for version < 1.2
182+
ASSERT_NE(nullptr, urdf);
183+
urdf::LinkConstSharedPtr link = urdf->getLink("link1");
184+
ASSERT_NE(nullptr, link);
185+
EXPECT_FALSE(link->visual_array.empty());
186+
}
187+
188+
TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_length_fails_v1_2)
189+
{
190+
std::string urdf_str = R"urdf(
191+
<robot name="capsule_negative_length_test" version="1.2">
192+
<link name="link1">
193+
<visual>
194+
<geometry>
195+
<capsule radius="0.05" length="-0.5"/>
196+
</geometry>
197+
</visual>
198+
</link>
199+
</robot>
200+
)urdf";
201+
202+
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
203+
// Negative length causes geometry parsing to fail for version >= 1.2
204+
ASSERT_NE(nullptr, urdf);
205+
urdf::LinkConstSharedPtr link = urdf->getLink("link1");
206+
ASSERT_NE(nullptr, link);
207+
EXPECT_TRUE(link->visual_array.empty());
208+
}
209+
210+
TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_length_allowed_pre_v1_2)
211+
{
212+
std::string urdf_str = R"urdf(
213+
<robot name="capsule_negative_length_test" version="1.1">
214+
<link name="link1">
215+
<visual>
216+
<geometry>
217+
<capsule radius="0.05" length="-0.5"/>
218+
</geometry>
219+
</visual>
220+
</link>
221+
</robot>
222+
)urdf";
223+
224+
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
225+
// Negative length is allowed for version < 1.2
226+
ASSERT_NE(nullptr, urdf);
227+
urdf::LinkConstSharedPtr link = urdf->getLink("link1");
228+
ASSERT_NE(nullptr, link);
229+
EXPECT_FALSE(link->visual_array.empty());
125230
}
126231

127232
TEST(URDF_UNIT_TEST, parse_capsule_geometry_missing_radius_fails)
@@ -183,7 +288,7 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_inf_value_fails)
183288
)urdf";
184289

185290
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
186-
// Infinity value causes geometry parsing to fail, visual is empty
291+
// "inf" is not parseable by strToDouble, so geometry parsing always fails
187292
ASSERT_NE(nullptr, urdf);
188293
urdf::LinkConstSharedPtr link = urdf->getLink("link1");
189294
ASSERT_NE(nullptr, link);

0 commit comments

Comments
 (0)