From 716aed19411d939cc900393f21d8c8a30454e5d5 Mon Sep 17 00:00:00 2001 From: Kamal Karki Date: Sat, 26 Sep 2026 19:10:32 +0530 Subject: [PATCH] fix(urdf): report stage, element and line when a URDF/xacro fails to load Part of #673. Loading failures now raise URDFError (a ValueError) that names the failing stage (xml / xacro / urdf), the element chain, the line in a saved copy of the expanded URDF, and xacro's include/macro stack, instead of leaking a bare KeyError, ParseError or ValueError. Undefined link references, missing joint type, missing mass/inertia and duplicate names now say which joint or link is at fault. --- src/roboticstoolbox/models/URDF/URDFRobot.py | 192 ++++++++++++++- src/roboticstoolbox/tools/urdf/__init__.py | 2 + src/roboticstoolbox/tools/urdf/urdf.py | 235 ++++++++++++++++--- tests/test_urdf_errors.py | 164 +++++++++++++ 4 files changed, 546 insertions(+), 47 deletions(-) create mode 100644 tests/test_urdf_errors.py diff --git a/src/roboticstoolbox/models/URDF/URDFRobot.py b/src/roboticstoolbox/models/URDF/URDFRobot.py index d0da86df9..efcaa7359 100644 --- a/src/roboticstoolbox/models/URDF/URDFRobot.py +++ b/src/roboticstoolbox/models/URDF/URDFRobot.py @@ -11,8 +11,11 @@ from pathlib import Path import importlib +import re import sys +import tempfile import warnings +import xml.parsers.expat from typing import Callable, TextIO import numpy as np @@ -21,7 +24,23 @@ from xacrodoc import XacroDoc, packages -from roboticstoolbox.tools.urdf import URDF +try: + from xacro import XacroException +except ImportError: # pragma nocover + + class XacroException(Exception): + pass + + +try: + from xacrodoc.packages import PackageNotFoundError +except ImportError: # pragma nocover + + class PackageNotFoundError(Exception): + pass + + +from roboticstoolbox.tools.urdf import URDF, URDFError from roboticstoolbox.robot.Link import Link from roboticstoolbox.ets.ET import ET from roboticstoolbox.ets.ETS import ETS @@ -174,10 +193,139 @@ def _load_urdf_from_RD(robot_name: str) -> "tuple[Path, dict | None]": return urdf_path, getattr(module, "XACRO_ARGS", None) -def _parse_urdf(urdf_str: str): - """Parse a URDF string into (elinks, name).""" - urdf = URDF.loadstr(urdf_str, None) +def _xacro_location() -> list[str]: + """Describe where xacro was when it failed, like xacro's own print_location().""" + try: + import xacro + except ImportError: # pragma nocover + return [] + + lines = [] + msg = "when instantiating macro:" + for m in reversed(getattr(xacro, "macrostack", None) or []): + try: + name = m.body.getAttribute("name") + where = m.history[-1][-1] or "???" + except Exception: # pragma nocover + continue + lines.append(f"{msg} {name} ({where})") + msg = "instantiated from:" + + msg = "in file:" if lines else "when processing file:" + for f in reversed(getattr(xacro, "filestack", None) or []): + if f is None: + continue + lines.append(f"{msg} {f}") + msg = "included from:" + return lines + + +def _reset_xacro_stacks() -> None: + """Clear xacro's module-level include/macro stacks. + + xacro does not unwind them when it raises, and xacrodoc never resets + them, so without this a failure inside an include or macro would leave + stale "in file: / when instantiating macro:" lines that a later, + unrelated failure would then report. + """ + try: + import xacro + + xacro.init_stacks(None) + except (ImportError, AttributeError): # pragma nocover + pass + + +def _package_not_found(e: BaseException) -> "PackageNotFoundError | None": + """Return the PackageNotFoundError at the root of a xacro error chain, if any. + + xacro wraps the exceptions raised while evaluating ``$(find pkg)`` in one + or two layers of ``XacroException`` (each with the original in ``.exc``), + so the package failure has to be dug out to give the right hint. + """ + cause = e + while isinstance(cause, XacroException) and getattr(cause, "exc", None): + cause = cause.exc + return cause if isinstance(cause, PackageNotFoundError) else None + + +def _expand_xacro(source: str, expand: "Callable[[], XacroDoc]") -> XacroDoc: + """Run one of the XacroDoc constructors, converting its failures to URDFError.""" + _reset_xacro_stacks() + try: + return expand() + except xml.parsers.expat.ExpatError as e: + raise URDFError( + f"{source} is not well-formed XML: {e}", + stage="xml", + source=source, + line=getattr(e, "lineno", None), + column=getattr(e, "offset", None), + location=_xacro_location(), + ) from e + except (PackageNotFoundError, XacroException) as e: + missing = _package_not_found(e) + if missing is not None: + raise URDFError( + f"xacro could not find a package while processing {source}: " + f"{missing}. Register its directory with " + "xacrodoc.packages.update_package_cache() or pass it via " + "extra_packages=", + stage="xacro", + source=source, + location=_xacro_location(), + ) from e + raise URDFError( + f"xacro failed while processing {source}: {e}", + stage="xacro", + source=source, + location=_xacro_location(), + ) from e + + +def _attach_expanded(err: URDFError, urdf_str: str, source: str) -> None: + """Point a URDF parse error at a saved copy of the expanded text it came from. + + xacro output is what actually failed to parse, so line numbers only make + sense against it. Write it to a temp file, record the path on the error, + and when the error names an element (``name="..."``) but has no line yet, + look the element up in the text to give one. + """ + err.source = source + with tempfile.NamedTemporaryFile( + "w", suffix=".urdf", prefix="rtb-expanded-", delete=False, encoding="utf-8" + ) as f: + f.write(urdf_str) + err.expanded_file = f.name + + if err.line is None: + for desc in err.elements: + m = re.search(r'name="([^"]*)"', desc) + if m is None: + continue + idx = urdf_str.find(f'name="{m.group(1)}"') + if idx >= 0: + err.line = urdf_str.count("\n", 0, idx) + 1 + break + +def _parse_urdf(urdf_str: str, source: str = ""): + """Parse a URDF string into (elinks, name). + + :raises URDFError: if the text is not well-formed XML or is not a valid + robot description; the error names the element, the line in a saved + copy of ``urdf_str``, and that copy's path + """ + try: + urdf = URDF.loadstr(urdf_str, None) + return _links_from_urdf(urdf) + except URDFError as e: + _attach_expanded(e, urdf_str, source) + raise + + +def _links_from_urdf(urdf: URDF): + """Convert a parsed :class:`URDF` into (elinks, name).""" elinks = [] elinkdict = {} @@ -213,6 +361,14 @@ def _parse_urdf(urdf_str: str): elink.collision = shapes for joint in urdf._joints: + for role, link_name in (("parent", joint.parent), ("child", joint.child)): + if link_name not in elinkdict: + raise URDFError( + f'joint "{joint.name}" refers to {role} link "{link_name}", ' + f"which is not defined (defined links: {', '.join(elinkdict)})", + stage="urdf", + element=f'', + ) childlink = elinkdict[joint.child] parentlink = elinkdict[joint.parent] @@ -320,6 +476,14 @@ def URDF_file( package lookup only auto-discovers directories that are already known by name, it doesn't fall back to searching by content. See ``LBR.py`` for a real example. + + :raises FileNotFoundError: if ``file`` is a path that does not exist + :raises URDFError: if xacro cannot expand the file, or the result is not + well-formed XML or not a valid robot description. The error reports + the stage that failed, the element and line involved, xacro's + include/macro stack, and the path of a saved copy of the expanded + URDF that the line number refers to. That copy is left in the + system temp directory for inspection """ import rtbdata @@ -342,23 +506,33 @@ def URDF_file( if isinstance(file, Path): if not file.is_absolute(): file = xacro_root / file + if not file.is_file(): + raise FileNotFoundError(f"URDF/xacro file not found: {file}") resolved_path = file + source = str(file) if patch is not None: # mirrors XacroDoc.from_file()'s own package-discovery step, # since we bypass from_file() here to patch the text first packages.walk_up_from(file) - doc = XacroDoc.from_string( - patch(file.read_text()), rootdir=file.parent, subargs=xacro_args + text = patch(file.read_text()) + doc = _expand_xacro( + source, + lambda: XacroDoc.from_string( + text, rootdir=file.parent, subargs=xacro_args + ), ) else: - doc = XacroDoc.from_file(file, subargs=xacro_args) + doc = _expand_xacro( + source, lambda: XacroDoc.from_file(file, subargs=xacro_args) + ) else: + source = getattr(file, "name", None) or "" text = file.read() if patch is not None: text = patch(text) - doc = XacroDoc.from_string(text) + doc = _expand_xacro(source, lambda: XacroDoc.from_string(text)) - elinks, name = _parse_urdf(doc.to_urdf_string()) + elinks, name = _parse_urdf(doc.to_urdf_string(), source=source) return elinks, name, resolved_path diff --git a/src/roboticstoolbox/tools/urdf/__init__.py b/src/roboticstoolbox/tools/urdf/__init__.py index 2e3fe895f..264469169 100644 --- a/src/roboticstoolbox/tools/urdf/__init__.py +++ b/src/roboticstoolbox/tools/urdf/__init__.py @@ -19,6 +19,7 @@ Joint, Link, URDF, + URDFError, ) __all__ = [ @@ -42,4 +43,5 @@ "Joint", "Link", "URDF", + "URDFError", ] diff --git a/src/roboticstoolbox/tools/urdf/urdf.py b/src/roboticstoolbox/tools/urdf/urdf.py index 9fd26c5fe..68f3221b6 100644 --- a/src/roboticstoolbox/tools/urdf/urdf.py +++ b/src/roboticstoolbox/tools/urdf/urdf.py @@ -24,6 +24,109 @@ _base_path = None +class URDFError(ValueError): + """ + A URDF or xacro file could not be loaded. + + Raised by :meth:`URDF.loadstr` and by the model loaders in + :mod:`roboticstoolbox.models.URDF` instead of the bare ``ValueError``, + ``KeyError`` or XML parser errors they used to leak. It subclasses + :class:`ValueError`, so existing ``except ValueError`` handlers keep + working. ``str(err)`` is a multi-line report; the fields below let code + inspect the details. + + :param msg: what went wrong + :param stage: ``"xml"`` (the text is not well-formed XML), ``"xacro"`` + (xacro could not expand the file) or ``"urdf"`` (the XML is fine but + the robot description is not) + :param source: the file, or a description of the input, being loaded + :param line: 1-based line of the failure, when known + :param column: column of the failure, when known + :param element: the XML element being parsed when the failure happened; + enclosing elements are added as the error propagates outward, see + :meth:`add_context` + :param location: xacro's file and macro stack at the time of the failure + :param expanded_file: path of a saved copy of the xacro-expanded URDF, + which is what ``line`` refers to when it is set + """ + + def __init__( + self, + msg: str, + *, + stage: str = "urdf", + source: "str | os.PathLike | None" = None, + line: int | None = None, + column: int | None = None, + element: str | None = None, + location: list[str] | None = None, + expanded_file: "str | os.PathLike | None" = None, + ): + super().__init__(msg) + self.msg = msg + self.stage = stage + self.source = source + self.line = line + self.column = column + self.elements: list[str] = [element] if element else [] + self.location: list[str] = list(location) if location else [] + self.expanded_file = expanded_file + + @property + def element(self) -> str | None: + """The innermost XML element being parsed when the failure happened.""" + return self.elements[0] if self.elements else None + + def add_context(self, element: str) -> None: + """Record an enclosing element, called as the error propagates outward.""" + if not self.elements or self.elements[-1] != element: + self.elements.append(element) + + def __str__(self) -> str: + lines = [self.msg] + if self.elements: + lines.append(" in " + " inside ".join(self.elements)) + if self.line is not None: + where = f"line {self.line}" + if self.column is not None: + where += f", column {self.column}" + if self.expanded_file is not None: + lines.append(f" at {where} of the expanded URDF") + elif self.source is not None: + lines.append(f" at {where} of {self.source}") + else: + lines.append(f" at {where}") + lines.extend(f" {loc}" for loc in self.location) + if self.expanded_file is not None: + lines.append(f" expanded URDF written to {self.expanded_file}") + return "\n".join(lines) + + +def _describe(node) -> str: + """Short description of an XML element for error messages.""" + name = node.attrib.get("name") + if name is None: + return f"<{node.tag}>" + return f'<{node.tag} name="{name}">' + + +def _parse_child(cls, node, path): + """Call ``cls._from_xml(node, path)``, attaching element context to any error.""" + try: + return cls._from_xml(node, path) + except URDFError as e: + e.add_context(_describe(node)) + raise + except KeyError as e: + raise URDFError( + f"missing attribute {e.args[0]!r}", stage="urdf", element=_describe(node) + ) from e + except (ValueError, TypeError, AttributeError) as e: + raise URDFError( + str(e) or type(e).__name__, stage="urdf", element=_describe(node) + ) from e + + class URDFType(): """Abstract base class for all URDF types. This has useful class methods for automatic parsing/unparsing @@ -89,17 +192,32 @@ def _parse_simple_attribs(cls, node): for a in cls._ATTRIBS: t, r = cls._ATTRIBS[a] # t = type, r = required (bool) if r: + if a not in node.attrib: + raise URDFError( + f'missing required attribute "{a}"', + stage="urdf", + element=_describe(node), + ) try: v = cls._parse_attrib(t, node.attrib[a]) - except Exception: # pragma nocover - raise ValueError( - "Missing required attribute {} when parsing an object " - "of type {}".format(a, cls.__name__) - ) + except (ValueError, TypeError) as e: + raise URDFError( + f'invalid value {node.attrib[a]!r} for attribute "{a}": {e}', + stage="urdf", + element=_describe(node), + ) from e else: v = None if a in node.attrib: - v = cls._parse_attrib(t, node.attrib[a]) + try: + v = cls._parse_attrib(t, node.attrib[a]) + except (ValueError, TypeError) as e: + raise URDFError( + f'invalid value {node.attrib[a]!r} for attribute "{a}": ' + f"{e}", + stage="urdf", + element=_describe(node), + ) from e kwargs[a] = v return kwargs @@ -125,16 +243,23 @@ def _parse_simple_elements(cls, node, path): t, r, m = cls._ELEMENTS[a] if not m: v = node.find(t._TAG) - if r or v is not None: - v = t._from_xml(v, path) + if v is None and r: + raise URDFError( + f"missing required <{t._TAG}> element", + stage="urdf", + element=_describe(node), + ) + if v is not None: + v = _parse_child(t, v, path) else: vs = node.findall(t._TAG) - if len(vs) == 0 and r: # pragma nocover - raise ValueError( - "Missing required subelement(s) of type {} when " - "parsing an object of type {}".format(t.__name__, cls.__name__) + if len(vs) == 0 and r: + raise URDFError( + f"missing required <{t._TAG}> element(s)", + stage="urdf", + element=_describe(node), ) - v = [t._from_xml(n, path) for n in vs] + v = [_parse_child(t, n, path) for n in vs] kwargs[a] = v return kwargs @@ -698,8 +823,29 @@ def origin(self, value): @classmethod def _from_xml(cls, node, path): origin, _ = parse_origin(node) - mass = float(node.find("mass").attrib["value"]) + mass_node = node.find("mass") + if mass_node is None or "value" not in mass_node.attrib: + raise URDFError( + ' needs a element', + stage="urdf", + element=_describe(node), + ) + mass = float(mass_node.attrib["value"]) n = node.find("inertia") + if n is None: + raise URDFError( + " needs an element", + stage="urdf", + element=_describe(node), + ) + keys = ("ixx", "ixy", "ixz", "iyy", "iyz", "izz") + missing = [k for k in keys if k not in n.attrib] + if missing: + raise URDFError( + f" is missing attribute(s) {', '.join(missing)}", + stage="urdf", + element=_describe(node), + ) xx = float(n.attrib["ixx"]) xy = float(n.attrib["ixy"]) xz = float(n.attrib["ixz"]) @@ -1555,7 +1701,21 @@ def is_valid(self, cfg): # pragma nocover @classmethod def _from_xml(cls, node, path): kwargs = cls._parse(node, path) + if "type" not in node.attrib: + raise URDFError( + 'joint is missing the required "type" attribute', + stage="urdf", + element=_describe(node), + ) kwargs["joint_type"] = str(node.attrib["type"]) + for role in ("parent", "child"): + ref = node.find(role) + if ref is None or "link" not in ref.attrib: + raise URDFError( + f'joint is missing a <{role} link="..."/> element', + stage="urdf", + element=_describe(node), + ) kwargs["parent"] = node.find("parent").attrib["link"] kwargs["child"] = node.find("child").attrib["link"] axis = node.find("axis") @@ -1728,18 +1888,18 @@ def __init__( self._material_map[x.name] = x # check for duplicate names - if len(self._links) > len( - set([x.name for x in self._links]) - ): # pragma nocover # noqa - raise ValueError("Duplicate link names") - if len(self._joints) > len( - set([x.name for x in self._joints]) - ): # pragma nocover # noqa - raise ValueError("Duplicate joint names") - if len(self._transmissions) > len( - set([x.name for x in self._transmissions]) - ): # pragma nocover # noqa - raise ValueError("Duplicate transmission names") + for kind, items in ( + ("link", self._links), + ("joint", self._joints), + ("transmission", self._transmissions), + ): + names = [x.name for x in items] + duplicates = sorted({n for n in names if names.count(n) > 1}) + if duplicates: + raise URDFError( + f"duplicate {kind} name(s): {', '.join(duplicates)}", + stage="urdf", + ) @property def name(self): @@ -1886,20 +2046,19 @@ def loadstr(str_obj, file_obj, base_path=None): _base_path = base_path if isinstance(str_obj, str): - # if os.path.isfile(file_obj): - parser = ETT.XMLParser() - bytes_obj = BytesIO(bytes(str_obj, "utf-8")) - tree = ETT.parse(bytes_obj, parser=parser) - # path, _ = os.path.split(file_obj) - + source = BytesIO(bytes(str_obj, "utf-8")) else: # pragma nocover - parser = ETT.XMLParser() - tree = ETT.parse(file_obj, parser=parser) - path, _ = os.path.split(file_obj.name) + source = file_obj - node = tree.getroot() - path = None - return URDF._from_xml(node, path) + try: + tree = ETT.parse(source, parser=ETT.XMLParser()) + except ETT.ParseError as e: + line, column = getattr(e, "position", (None, None)) + raise URDFError( + f"not well-formed XML: {e}", stage="xml", line=line, column=column + ) from e + + return _parse_child(URDF, tree.getroot(), None) def _validate_transmissions(self): """Raise an exception if any transmissions are invalidly specified. diff --git a/tests/test_urdf_errors.py b/tests/test_urdf_errors.py new file mode 100644 index 000000000..9854a0b1d --- /dev/null +++ b/tests/test_urdf_errors.py @@ -0,0 +1,164 @@ +""" +URDF/xacro loading failures must say what, where and in which element. + +Before this, a bad file surfaced as a bare KeyError('forearm'), an XML +ParseError pointing into text the user never saw (the xacro-expanded +output), or a ValueError with no element name. See issue #673. +""" + +import io +import unittest +from pathlib import Path + +import roboticstoolbox as rtb +from roboticstoolbox.models.URDF.URDFRobot import URDF_file +from roboticstoolbox.tools.urdf import URDF, URDFError + +MINIMAL = """ + + + + + + + + + + + + + + + + + +""" + + +def load(text): + return URDF_file(io.StringIO(text)) + + +class TestURDFErrors(unittest.TestCase): + def test_valid_text_still_loads(self): + links, name, path = load(MINIMAL) + self.assertEqual(name, "two_link") + self.assertIsNone(path) + robot = rtb.Robot(links) + self.assertEqual(robot.n, 1) + self.assertAlmostEqual(robot.links[1].m, 1.0) + + def test_is_a_valueerror(self): + # existing `except ValueError` handlers keep working + self.assertIsInstance(URDFError("x"), ValueError) + with self.assertRaises(ValueError): + load(MINIMAL.replace('', '')) + + def test_source_not_well_formed_xml(self): + text = MINIMAL.replace('', '', + '\n' + ' \n' + ' \n' + " ", + ) + with self.assertRaises(URDFError) as cm: + load(text) + e = cm.exception + self.assertEqual(e.stage, "xacro") + self.assertIn("no_such_pkg_xyz", str(e)) + # the root cause is dug out of xacro's wrapping, so the hint is shown + self.assertIn("update_package_cache", str(e)) + + def test_joint_refers_to_undefined_link(self): + text = MINIMAL.replace('', '') + with self.assertRaises(URDFError) as cm: + load(text) + e = cm.exception + self.assertEqual(e.stage, "urdf") + self.assertIn("forearm", str(e)) + self.assertIn("j1", str(e)) + self.assertIn('', e.elements) + # the expanded URDF is saved and the line points at the joint in it + self.assertIsNotNone(e.expanded_file) + expanded = Path(e.expanded_file) + self.assertTrue(expanded.is_file()) + self.assertIsNotNone(e.line) + self.assertIn('name="j1"', expanded.read_text().splitlines()[e.line - 1]) + + def test_joint_missing_type(self): + text = MINIMAL.replace('', '') + with self.assertRaises(URDFError) as cm: + load(text) + e = cm.exception + self.assertEqual(e.stage, "urdf") + self.assertIn("type", str(e)) + self.assertIn('', e.elements) + + def test_unsupported_joint_type_names_the_joint(self): + text = MINIMAL.replace('type="revolute"', 'type="wobbly"') + with self.assertRaises(URDFError) as cm: + load(text) + e = cm.exception + self.assertIn("wobbly", str(e)) + self.assertIn('', e.elements) + + def test_missing_mass_value_names_the_link(self): + text = MINIMAL.replace('', "") + with self.assertRaises(URDFError) as cm: + load(text) + e = cm.exception + self.assertEqual(e.stage, "urdf") + self.assertIn("mass", str(e)) + # innermost element first, enclosing link last + self.assertEqual(e.elements[0], "") + self.assertIn('', e.elements) + + def test_bad_number_names_attribute_and_element(self): + text = MINIMAL.replace('effort="10"', 'effort="ten"') + with self.assertRaises(URDFError) as cm: + load(text) + e = cm.exception + self.assertIn("effort", str(e)) + self.assertIn("ten", str(e)) + self.assertIn('', e.elements) + + def test_duplicate_link_names(self): + text = MINIMAL.replace('', '') + with self.assertRaises(URDFError) as cm: + load(text) + self.assertIn("duplicate link", str(cm.exception)) + self.assertIn("arm", str(cm.exception)) + + def test_loadstr_reports_xml_line(self): + with self.assertRaises(URDFError) as cm: + URDF.loadstr("\n\n", None) + e = cm.exception + self.assertEqual(e.stage, "xml") + self.assertEqual(e.line, 3) + + def test_missing_file(self): + with self.assertRaises(FileNotFoundError) as cm: + URDF_file("no_such_dir/no_such_file.urdf") + self.assertIn("no_such_file.urdf", str(cm.exception)) + + +if __name__ == "__main__": + unittest.main()