diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index b887eb086b..bd6ee64276 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 + urdfdom_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 542663f1bd..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 @@ -18,20 +18,23 @@ # 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 warns about on stderr during +# parsing. We strip non-standard tags to keep logs clean. +_URDF_STANDARD_TAGS = frozenset({"link", "joint", "transmission", "material"}) + + def callback(msg): global description description = msg.data @@ -45,6 +48,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 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 + 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 +99,74 @@ 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 to suppress urdf_parser_py stderr warnings + # for unrecognised tags like + 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 + 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 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..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( '' @@ -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 element missing the required velocity attribute. + The error comes from the library itself, before our code inspects + the joint. """ urdf = _robot( '' @@ -330,5 +330,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"])