From 940c09eb87ade5dda80fd4ea534eaabe07395a3b Mon Sep 17 00:00:00 2001 From: root Date: Sat, 28 Mar 2026 23:43:28 +0000 Subject: [PATCH 1/7] rqt_jtc: Use urdf_parser_py --- rqt_joint_trajectory_controller/package.xml | 1 + .../joint_limits_urdf.py | 140 +++++++++--------- 2 files changed, 74 insertions(+), 67 deletions(-) diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index e632c68d9f..37fdf5ae7e 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -25,6 +25,7 @@ controller_manager_msgs python_qt_binding python3-rospkg + python3-urdf-parser-py trajectory_msgs rclpy rqt_gui diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 1e5e80b7a9..98fdbdc0d7 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -18,16 +18,12 @@ # https://github.com/ros/robot_model/blob/indigo-devel/ # joint_state_publisher/joint_state_publisher/joint_state_publisher -# TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got -# Exception: Required attribute not set in XML: upper -# upper is an optional attribute, so I don't understand what's going on -# See comments in https://github.com/ros/urdfdom/issues/36 - -import xml.dom.minidom from math import pi +import xml.etree.ElementTree as ET import rclpy from std_msgs.msg import String +from urdf_parser_py.urdf import Robot description = "" @@ -59,67 +55,77 @@ def get_joint_limits(node, joints_names, use_smallest_joint_limits=True): dependent_joints = {} if description != "": - robot = xml.dom.minidom.parseString(description).getElementsByTagName("robot")[0] + try: + # urdf_parser_py does not recognize non-URDF tags such as + # and raises an AssertionError. + # Strip them before parsing. + root = ET.fromstring(description) + for tag_name in ["ros2_control"]: + for element in root.findall(tag_name): + root.remove(element) + cleaned_description = ET.tostring(root, encoding="unicode") + robot = Robot.from_xml_string(cleaned_description) + except BaseException as e: + print(f"Unexpected error: {type(e)}") + return free_joints + + for joint in robot.joints: + if joint.type == "fixed": + continue + name = joint.name + + if joint.limit is None: + if name in joints_names: + raise Exception( + f"Missing limits tag for the joint : {name} in the robot_description!" + ) + continue - # Find all non-fixed joints - for child in robot.childNodes: - if child.nodeType is child.TEXT_NODE: + minval = joint.limit.lower if joint.limit.lower is not None else 0.0 + maxval = joint.limit.upper if joint.limit.upper is not None else 0.0 + + # urdf_parser_py defaults lower/upper to 0.0 when not + # specified in the URDF, so check for invalid range. + if minval >= maxval: + if joint.type == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name}" + f" of type : {joint.type} in the robot_description!" + ) + + if joint.limit.velocity is None: + raise Exception( + f"Missing velocity limits for the joint : {name}" + f" of type : {joint.type} in the robot_description!" + ) + maxvel = joint.limit.velocity + + if use_small and joint.safety_controller is not None: + if joint.safety_controller.soft_lower_limit is not None: + minval = max(minval, joint.safety_controller.soft_lower_limit) + if joint.safety_controller.soft_upper_limit is not None: + maxval = min(maxval, joint.safety_controller.soft_upper_limit) + + if use_mimic and joint.mimic is not None: + entry = {"parent": joint.mimic.joint} + if joint.mimic.multiplier is not None: + entry["factor"] = joint.mimic.multiplier + if joint.mimic.offset is not None: + entry["offset"] = joint.mimic.offset + dependent_joints[name] = entry continue - if child.localName == "joint": - jtype = child.getAttribute("type") - if jtype == "fixed": - continue - name = child.getAttribute("name") - - try: - limit = child.getElementsByTagName("limit")[0] - try: - minval = float(limit.getAttribute("lower")) - maxval = float(limit.getAttribute("upper")) - except ValueError: - if jtype == "continuous": - minval = -pi - maxval = pi - else: - raise Exception( - f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" - ) - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: - raise Exception( - f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" - ) - except IndexError: - if name in joints_names: - raise Exception( - f"Missing limits tag for the joint : {name} in the robot_description!" - ) - safety_tags = child.getElementsByTagName("safety_controller") - if use_small and len(safety_tags) == 1: - tag = safety_tags[0] - if tag.hasAttribute("soft_lower_limit"): - minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) - if tag.hasAttribute("soft_upper_limit"): - maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) - - mimic_tags = child.getElementsByTagName("mimic") - if use_mimic and len(mimic_tags) == 1: - tag = mimic_tags[0] - entry = {"parent": tag.getAttribute("joint")} - if tag.hasAttribute("multiplier"): - entry["factor"] = float(tag.getAttribute("multiplier")) - if tag.hasAttribute("offset"): - entry["offset"] = float(tag.getAttribute("offset")) - - dependent_joints[name] = entry - continue - - if name in dependent_joints: - continue - - joint = {"min_position": minval, "max_position": maxval} - joint["has_position_limits"] = jtype != "continuous" - joint["max_velocity"] = maxvel - free_joints[name] = joint + + if name in dependent_joints: + continue + + free_joints[name] = { + "min_position": minval, + "max_position": maxval, + "has_position_limits": joint.type != "continuous", + "max_velocity": maxvel, + } + return free_joints From e3a4df49b420cd6c5a0f11ff6db38b2013d68816 Mon Sep 17 00:00:00 2001 From: root Date: Wed, 1 Apr 2026 01:45:21 +0000 Subject: [PATCH 2/7] fix: replace python3-urdf-parser-py with urdfdom_py --- rqt_joint_trajectory_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 37fdf5ae7e..13abecba6e 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -25,7 +25,7 @@ controller_manager_msgs python_qt_binding python3-rospkg - python3-urdf-parser-py + urdfdom_py trajectory_msgs rclpy rqt_gui From 666ff21f3311fb538d69db19fa4e7019b5bf4f60 Mon Sep 17 00:00:00 2001 From: root Date: Mon, 13 Apr 2026 02:18:35 +0000 Subject: [PATCH 3/7] rqt_jtc: Use urdf_parser_py with whitelist preprocessing Replace xml.dom.minidom parsing with urdf_parser_py.urdf.Robot. Non-URDF tags such as and are stripped before parsing via a whitelist of standard URDF elements (link, joint, transmission, material), addressing review feedback about handling all vendor extensions rather than only ros2_control. --- .../joint_limits_urdf.py | 182 +++++++++--------- 1 file changed, 96 insertions(+), 86 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 542663f1bd..fb4b3a8d46 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -18,20 +18,24 @@ # https://github.com/ros/robot_model/blob/indigo-devel/ # joint_state_publisher/joint_state_publisher/joint_state_publisher -# TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got -# Exception: Required attribute not set in XML: upper -# upper is an optional attribute, so I don't understand what's going on -# See comments in https://github.com/ros/urdfdom/issues/36 - -import xml.dom.minidom +import xml.etree.ElementTree as ET from math import pi import rclpy from std_msgs.msg import String +from urdf_parser_py.urdf import Robot description = "" +# Tags defined as direct children of in the URDF specification. +# Any other tag is a vendor extension (ros2_control, gazebo, xacro +# remnants, etc.) that urdf_parser_py rejects with an AssertionError +# during strict validation. We whitelist the standard tags and strip +# everything else before parsing. +_URDF_STANDARD_TAGS = frozenset({"link", "joint", "transmission", "material"}) + + def callback(msg): global description description = msg.data @@ -45,6 +49,24 @@ def subscribe_to_robot_description(node, key="robot_description"): node.create_subscription(String, key, callback, qos_profile) +def _strip_non_urdf_tags(urdf_string): + """ + Remove non-URDF extension tags from a robot description string. + + Robot descriptions published to /robot_description commonly carry + vendor-specific extensions like and alongside + the standard URDF elements. urdf_parser_py validates strictly and + raises AssertionError on any unknown tag, so we keep only the tags + defined by the URDF specification and drop the rest. + """ + root = ET.fromstring(urdf_string) + # Iterate over a list copy because we are mutating root during the loop + for child in list(root): + if child.tag not in _URDF_STANDARD_TAGS: + root.remove(child) + return ET.tostring(root, encoding="unicode") + + def parse_joint_limits(urdf_string, joints_names, use_smallest_joint_limits=True): """ Parse joint position and velocity limits from a URDF XML string. @@ -78,87 +100,75 @@ def parse_joint_limits(urdf_string, joints_names, use_smallest_joint_limits=True free_joints = {} dependent_joints = {} - # minidom raises xml.parsers.expat.ExpatError for completely invalid XML. - # We let that propagate naturally — the caller (get_joint_limits) can - # decide how to handle it. For unit tests, we test our own logic only. - robot = xml.dom.minidom.parseString(urdf_string).getElementsByTagName("robot")[0] - - # Walk every direct child of the element. - # Non-joint tags like , , are naturally - # skipped because we only act when localName == "joint". - for child in robot.childNodes: - # minidom includes whitespace text nodes between elements — skip them - if child.nodeType is child.TEXT_NODE: + # Strip vendor extensions before strict urdf_parser_py validation. + # Parsing errors on the cleaned URDF propagate to the caller, which + # matches the old minidom-based behavior on malformed input. + cleaned = _strip_non_urdf_tags(urdf_string) + robot = Robot.from_xml_string(cleaned) + + for joint in robot.joints: + if joint.type == "fixed": + # Fixed joints have no DOF, so no slider is needed in the GUI + continue + + name = joint.name + + # No element at all means urdf_parser_py sets joint.limit to None + if joint.limit is None: + if name in joints_names: + raise Exception( + f"Missing limits tag for the joint : {name} in the robot_description!" + ) + # Joint is not managed by this controller, so we skip silently continue - if child.localName == "joint": - jtype = child.getAttribute("type") - if jtype == "fixed": - # Fixed joints have no DOF — no slider needed in the GUI - continue - name = child.getAttribute("name") - - try: - limit = child.getElementsByTagName("limit")[0] - - # minidom returns "" for absent attributes. - # float("") raises ValueError, which we catch below. - try: - minval = float(limit.getAttribute("lower")) - maxval = float(limit.getAttribute("upper")) - except ValueError: - if jtype == "continuous": - # Continuous joints have no position bounds by definition - minval = -pi - maxval = pi - else: - raise Exception( - f"Missing lower/upper position limits for the joint" - f" : {name} of type : {jtype} in the robot_description!" - ) - - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: - raise Exception( - f"Missing velocity limits for the joint" - f" : {name} of type : {jtype} in the robot_description!" - ) - - except IndexError: - # No element found at all for this joint - if name in joints_names: - raise Exception( - f"Missing limits tag for the joint" f" : {name} in the robot_description!" - ) - # Joint is not managed by this controller — skip silently - continue - - # Optionally narrow the range with safety controller soft limits - safety_tags = child.getElementsByTagName("safety_controller") - if use_small and len(safety_tags) == 1: - tag = safety_tags[0] - if tag.hasAttribute("soft_lower_limit"): - minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) - if tag.hasAttribute("soft_upper_limit"): - maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) - - # Mimic joints follow another joint — exclude from free_joints - mimic_tags = child.getElementsByTagName("mimic") - if use_mimic and len(mimic_tags) == 1: - tag = mimic_tags[0] - entry = {"parent": tag.getAttribute("joint")} - if tag.hasAttribute("multiplier"): - entry["factor"] = float(tag.getAttribute("multiplier")) - if tag.hasAttribute("offset"): - entry["offset"] = float(tag.getAttribute("offset")) - - dependent_joints[name] = entry - continue - - joint = {"min_position": minval, "max_position": maxval} - joint["has_position_limits"] = jtype != "continuous" - joint["max_velocity"] = maxvel - free_joints[name] = joint + + # urdf_parser_py defaults lower/upper to 0.0 when the attributes are + # absent in the URDF. We treat min >= max as "limits missing" and + # either fall back to -pi..pi for continuous joints or raise for + # revolute joints where valid bounds are required. + minval = joint.limit.lower if joint.limit.lower is not None else 0.0 + maxval = joint.limit.upper if joint.limit.upper is not None else 0.0 + + if minval >= maxval: + if joint.type == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint" + f" : {name} of type : {joint.type} in the robot_description!" + ) + + if joint.limit.velocity is None: + raise Exception( + f"Missing velocity limits for the joint" + f" : {name} of type : {joint.type} in the robot_description!" + ) + maxvel = joint.limit.velocity + + # Optionally narrow the range with safety_controller soft limits + if use_small and joint.safety_controller is not None: + if joint.safety_controller.soft_lower_limit is not None: + minval = max(minval, joint.safety_controller.soft_lower_limit) + if joint.safety_controller.soft_upper_limit is not None: + maxval = min(maxval, joint.safety_controller.soft_upper_limit) + + # Mimic joints follow another joint and go into dependent_joints + if use_mimic and joint.mimic is not None: + entry = {"parent": joint.mimic.joint} + if joint.mimic.multiplier is not None: + entry["factor"] = joint.mimic.multiplier + if joint.mimic.offset is not None: + entry["offset"] = joint.mimic.offset + dependent_joints[name] = entry + continue + + free_joints[name] = { + "min_position": minval, + "max_position": maxval, + "has_position_limits": joint.type != "continuous", + "max_velocity": maxvel, + } return free_joints From f6b7c8d3cb38778742f7c97625e2294c969ecad6 Mon Sep 17 00:00:00 2001 From: root Date: Mon, 13 Apr 2026 23:01:48 +0000 Subject: [PATCH 4/7] rqt_jtc: Update test_missing_velocity_raises for urdf_parser_py --- .../test/test_joint_limits_urdf.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py b/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py index 6306c90a33..b8cd85300b 100644 --- a/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py @@ -318,10 +318,12 @@ def test_revolute_joint_missing_lower_upper_raises(): def test_missing_velocity_raises(): - """Joint with no velocity attribute raises. + """Joint with no velocity attribute raises a parse error. - minidom returns "" for absent velocity. float("") raises ValueError. - Our except block turns that into a meaningful message. Our own logic. + urdf_parser_py performs strict XML validation during parsing and + rejects a element missing the required velocity attribute. + The error comes from the library itself, before our code inspects + the joint. """ urdf = _robot( '' @@ -330,5 +332,5 @@ def test_missing_velocity_raises(): '' "" ) - with pytest.raises(Exception, match="Missing velocity limits"): + with pytest.raises(Exception, match="Required attribute not set in XML: velocity"): parse_joint_limits(urdf, ["j"]) From fa6ca682e2d8aa58143a79dbf361ae373c143e11 Mon Sep 17 00:00:00 2001 From: root Date: Sat, 18 Apr 2026 04:24:39 +0000 Subject: [PATCH 5/7] rqt_jtc: Update test comments from minidom to urdf_parser_py --- .../test/test_joint_limits_urdf.py | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py b/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py index b8cd85300b..192671fd94 100644 --- a/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py @@ -108,8 +108,8 @@ def test_revolute_joint_has_position_limits_true(): # --------------------------------------------------------------------------- # Group 2: Continuous joint — like a wheel, no position bounds. -# When lower/upper are absent, minidom returns "", float("") raises -# ValueError, and our code must default to -pi / +pi so the slider +# When lower/upper are absent, urdf_parser_py defaults them to 0. +# Our code detects min >= max and defaults to -pi / +pi so the slider # has a usable range. # --------------------------------------------------------------------------- @@ -190,7 +190,6 @@ def test_multiple_joints_individual_limits_correct(): # Group 5: Safety controller soft limits. # When use_smallest_joint_limits=True, soft limits should narrow the range. # When False, only the hard limits from should be used. -# This is entirely our application logic — minidom knows nothing about it. # --------------------------------------------------------------------------- @@ -260,18 +259,18 @@ def test_driver_joint_present_when_follower_is_mimic(): # --------------------------------------------------------------------------- -# Group 7: Error cases — our application logic, not minidom's. -# minidom parses all of these successfully and returns data. -# Our code is the one that decides they are errors. +# Group 7: Error cases — validation by urdf_parser_py and our application +# logic. Some errors are caught by the library during parsing, others +# by our code after parsing succeeds. # --------------------------------------------------------------------------- def test_missing_limit_tag_for_required_joint_raises(): """Joint in joints_names with no element at all must raise. - minidom parses this fine — joint.getElementsByTagName("limit") just - returns an empty list, and [0] raises IndexError. Our except block - is what turns that into a meaningful exception message. + urdf_parser_py sets joint.limit to None when no element is + present. Our code checks for this and raises an exception for joints + that the active controller manages. """ urdf = _robot( '' @@ -302,9 +301,8 @@ def test_missing_limit_tag_for_unrequired_joint_skipped_silently(): def test_revolute_joint_missing_lower_upper_raises(): """Revolute joint with no lower/upper attributes raises. - minidom returns "" for absent attributes. float("") raises ValueError. - Our except block turns that into a meaningful message for non-continuous - joints. This is our own logic — worth testing. + urdf_parser_py defaults missing lower/upper to 0. Our code detects + the invalid range (min >= max) and raises for non-continuous joints. """ urdf = _robot( '' From 62387ae27a73556440e89189423353be44a620e6 Mon Sep 17 00:00:00 2001 From: root Date: Sat, 18 Apr 2026 04:37:31 +0000 Subject: [PATCH 6/7] rqt_jtc: Drop redundant None check on joint.limit.lower/upper --- .../rqt_joint_trajectory_controller/joint_limits_urdf.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index fb4b3a8d46..3f250bef4e 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -126,8 +126,8 @@ def parse_joint_limits(urdf_string, joints_names, use_smallest_joint_limits=True # absent in the URDF. We treat min >= max as "limits missing" and # either fall back to -pi..pi for continuous joints or raise for # revolute joints where valid bounds are required. - minval = joint.limit.lower if joint.limit.lower is not None else 0.0 - maxval = joint.limit.upper if joint.limit.upper is not None else 0.0 + minval = joint.limit.lower + maxval = joint.limit.upper if minval >= maxval: if joint.type == "continuous": From 294cba31f9be7b1290c35d287c535dce9204a689 Mon Sep 17 00:00:00 2001 From: root Date: Wed, 22 Apr 2026 13:37:29 +0000 Subject: [PATCH 7/7] rqt_jtc: Fix docstring --- .../joint_limits_urdf.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 3f250bef4e..5e5ae27df3 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -30,9 +30,8 @@ # Tags defined as direct children of in the URDF specification. # Any other tag is a vendor extension (ros2_control, gazebo, xacro -# remnants, etc.) that urdf_parser_py rejects with an AssertionError -# during strict validation. We whitelist the standard tags and strip -# everything else before parsing. +# remnants, etc.) that urdf_parser_py warns about on stderr during +# parsing. We strip non-standard tags to keep logs clean. _URDF_STANDARD_TAGS = frozenset({"link", "joint", "transmission", "material"}) @@ -55,9 +54,9 @@ def _strip_non_urdf_tags(urdf_string): Robot descriptions published to /robot_description commonly carry vendor-specific extensions like and alongside - the standard URDF elements. urdf_parser_py validates strictly and - raises AssertionError on any unknown tag, so we keep only the tags - defined by the URDF specification and drop the rest. + the standard URDF elements. urdf_parser_py prints warnings to stderr + for unrecognised tags (e.g. ). We strip them to keep + production logs clean. """ root = ET.fromstring(urdf_string) # Iterate over a list copy because we are mutating root during the loop @@ -100,9 +99,8 @@ def parse_joint_limits(urdf_string, joints_names, use_smallest_joint_limits=True free_joints = {} dependent_joints = {} - # Strip vendor extensions before strict urdf_parser_py validation. - # Parsing errors on the cleaned URDF propagate to the caller, which - # matches the old minidom-based behavior on malformed input. + # Strip vendor extensions to suppress urdf_parser_py stderr warnings + # for unrecognised tags like cleaned = _strip_non_urdf_tags(urdf_string) robot = Robot.from_xml_string(cleaned)