[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,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
+            # <ros2_control> 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