[rqt_jtc] Use urdf_parser_py

rqt_joint_trajectory_controller/package.xml

@@ -25,6 +25,7 @@
   <exec_depend>controller_manager_msgs</exec_depend>
   <exec_depend>python_qt_binding</exec_depend>
   <exec_depend>python3-rospkg</exec_depend>
+  <exec_depend>urdfdom_py</exec_depend>
   <exec_depend>trajectory_msgs</exec_depend>
   <exec_depend>rclpy</exec_depend>
   <exec_depend>rqt_gui</exec_depend>

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 <robot> 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 <ros2_control> and <gazebo> 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 <robot> element.
-    # Non-joint tags like <link>, <ros2_control>, <gazebo> 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 <limit> 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 <limit> 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
+        maxval = joint.limit.upper
+
+        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
 

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 <limit> 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 <limit> 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 <limit> element is
+    present. Our code checks for this and raises an exception for joints
+    that the active controller manages.
     """
     urdf = _robot(
         '<link name="j_link"/>'
@@ -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(
         '<link name="j_link"/>'
@@ -318,10 +316,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 <limit> element missing the required velocity attribute.
+    The error comes from the library itself, before our code inspects
+    the joint.
     """
     urdf = _robot(
         '<link name="j_link"/>'
@@ -330,5 +330,5 @@ def test_missing_velocity_raises():
         '<limit lower="-1.0" upper="1.0" effort="5"/>'
         "</joint>"
     )
-    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"])