From f678dfcc3f60370073bb10590552867c151d9571 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Jan 2026 23:46:02 +0100 Subject: [PATCH 1/5] Add support for capsule geometry type Signed-off-by: Sai Kishor Kothakota --- urdf_parser/src/link.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index dab48c29..086783ca 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -246,6 +246,12 @@ bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem) return false; } + if (!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length < 0 || c.radius < 0) + { + CONSOLE_BRIDGE_logError("Capsule length and radius must be non-negative finite values"); + return false; + } + return true; } From e442dc3f5ccea088112e307f9ca868d9d9e8731b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 30 Jan 2026 09:39:22 +0100 Subject: [PATCH 2/5] Add tests for the capsule geometry Signed-off-by: Sai Kishor Kothakota --- .../urdf_schema_capsule_geometry_test.cpp | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp index b586a5d0..5de24a25 100644 --- a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp +++ b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp @@ -124,6 +124,50 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values) EXPECT_DOUBLE_EQ(0.0, capsule->length); } +TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative radius causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_length_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative length causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + TEST(URDF_UNIT_TEST, parse_capsule_geometry_missing_radius_fails) { std::string urdf_str = R"urdf( From a33290a906371fb424b7c5cd85c6350e72417c9e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Feb 2026 10:05:21 +0100 Subject: [PATCH 3/5] remove invalid number checks for consistency Signed-off-by: Sai Kishor Kothakota --- urdf_parser/src/link.cpp | 6 --- .../urdf_schema_capsule_geometry_test.cpp | 44 ------------------- 2 files changed, 50 deletions(-) diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 086783ca..dab48c29 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -246,12 +246,6 @@ bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem) return false; } - if (!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length < 0 || c.radius < 0) - { - CONSOLE_BRIDGE_logError("Capsule length and radius must be non-negative finite values"); - return false; - } - return true; } diff --git a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp index 5de24a25..b586a5d0 100644 --- a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp +++ b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp @@ -124,50 +124,6 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values) EXPECT_DOUBLE_EQ(0.0, capsule->length); } -TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_fails) -{ - std::string urdf_str = R"urdf( - - - - - - - - - - )urdf"; - - urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); - // Negative radius causes geometry parsing to fail, visual is empty - ASSERT_NE(nullptr, urdf); - urdf::LinkConstSharedPtr link = urdf->getLink("link1"); - ASSERT_NE(nullptr, link); - EXPECT_TRUE(link->visual_array.empty()); -} - -TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_length_fails) -{ - std::string urdf_str = R"urdf( - - - - - - - - - - )urdf"; - - urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); - // Negative length causes geometry parsing to fail, visual is empty - ASSERT_NE(nullptr, urdf); - urdf::LinkConstSharedPtr link = urdf->getLink("link1"); - ASSERT_NE(nullptr, link); - EXPECT_TRUE(link->visual_array.empty()); -} - TEST(URDF_UNIT_TEST, parse_capsule_geometry_missing_radius_fails) { std::string urdf_str = R"urdf( From fe043c91c4d72e9c5510f0e5b3e86d898905b2bf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Feb 2026 10:06:48 +0100 Subject: [PATCH 4/5] Add invalid data checks Signed-off-by: Sai Kishor Kothakota --- urdf_parser/src/link.cpp | 6 +++ .../urdf_schema_capsule_geometry_test.cpp | 44 +++++++++++++++++++ 2 files changed, 50 insertions(+) diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index dab48c29..086783ca 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -246,6 +246,12 @@ bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem) return false; } + if (!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length < 0 || c.radius < 0) + { + CONSOLE_BRIDGE_logError("Capsule length and radius must be non-negative finite values"); + return false; + } + return true; } diff --git a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp index b586a5d0..5de24a25 100644 --- a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp +++ b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp @@ -124,6 +124,50 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values) EXPECT_DOUBLE_EQ(0.0, capsule->length); } +TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative radius causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_length_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative length causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + TEST(URDF_UNIT_TEST, parse_capsule_geometry_missing_radius_fails) { std::string urdf_str = R"urdf( From 3f53ceda132233303aa3ca90321a2212581f5386 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Feb 2026 10:22:29 +0100 Subject: [PATCH 5/5] Extend invalid data checks to other geometry entities Signed-off-by: Sai Kishor Kothakota --- urdf_parser/src/link.cpp | 23 ++- .../urdf_schema_capsule_geometry_test.cpp | 9 +- urdf_parser/test/urdf_unit_test.cpp | 183 ++++++++++++++++++ 3 files changed, 206 insertions(+), 9 deletions(-) diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 086783ca..2894bc55 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -127,6 +127,12 @@ bool parseSphere(Sphere &s, tinyxml2::XMLElement *c) return false; } + if (!std::isfinite(s.radius) || s.radius <= 0) + { + CONSOLE_BRIDGE_logError("Sphere radius must be a positive finite value"); + return false; + } + return true; } @@ -150,6 +156,13 @@ bool parseBox(Box &b, tinyxml2::XMLElement *c) CONSOLE_BRIDGE_logError(e.what()); return false; } + + if (b.dim.x <= 0 || b.dim.y <= 0 || b.dim.z <= 0) + { + CONSOLE_BRIDGE_logError("Box size must be positive finite values"); + return false; + } + return true; } @@ -183,6 +196,12 @@ bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c) return false; } + if (!std::isfinite(y.length) || !std::isfinite(y.radius) || y.length <= 0 || y.radius <= 0) + { + CONSOLE_BRIDGE_logError("Cylinder length and radius must be positive finite values"); + return false; + } + return true; } @@ -246,9 +265,9 @@ bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem) return false; } - if (!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length < 0 || c.radius < 0) + if (!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length <= 0 || c.radius <= 0) { - CONSOLE_BRIDGE_logError("Capsule length and radius must be non-negative finite values"); + CONSOLE_BRIDGE_logError("Capsule length and radius must be positive finite values"); return false; } diff --git a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp index 5de24a25..31a0abce 100644 --- a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp +++ b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp @@ -112,16 +112,11 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values) )urdf"; urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); - + // Invalid radius/length causes geometry parsing to fail ASSERT_NE(nullptr, urdf); urdf::LinkConstSharedPtr link = urdf->getLink("link1"); ASSERT_NE(nullptr, link); - - std::shared_ptr capsule = - std::dynamic_pointer_cast(link->visual_array[0]->geometry); - ASSERT_NE(nullptr, capsule); - EXPECT_DOUBLE_EQ(0.0, capsule->radius); - EXPECT_DOUBLE_EQ(0.0, capsule->length); + EXPECT_TRUE(link->visual_array.empty()); } TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_fails) diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index 8349b1de..720f07ac 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -346,6 +346,189 @@ TEST(URDF_UNIT_TEST, parse_color_doubles) EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz); } +TEST(URDF_UNIT_TEST, negative_sphere_radius_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Invalid radius causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_as_sphere_radius_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Invalid radius causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_box_size_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Negative size causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_box_size_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Zero size causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_cylinder_radius_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Negative radius causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_cylinder_length_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Negative length causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_cylinder_radius_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Zero radius causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_cylinder_length_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Zero length causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} int main(int argc, char **argv) {