diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp
index e7cbdf18..97e7643c 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("%s", 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,6 +265,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 positive 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..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,55 @@ 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);
+ EXPECT_TRUE(link->visual_array.empty());
+}
+
+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());
+}
- 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);
+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)
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)
{