fix issue of not listing new JTCs (#1891)

Author: peter-mitrano-ar

rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py

@@ -71,55 +71,55 @@ def get_joint_limits(node, joints_names, use_smallest_joint_limits=True):
                     continue
                 name = child.getAttribute("name")
 
-                if name in joints_names:
+                try:
+                    limit = child.getElementsByTagName("limit")[0]
                     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:
+                        minval = float(limit.getAttribute("lower"))
+                        maxval = float(limit.getAttribute("upper"))
+                    except ValueError:
+                        if jtype == "continuous":
+                            minval = -pi
+                            maxval = pi
+                        else:
                             raise Exception(
-                                f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!"
+                                f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!"
                             )
-                    except IndexError:
+                    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
+                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
     return free_joints

rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py

@@ -152,6 +152,8 @@ def _srv_exists(node, srv_name, srv_type):
 
     srv_list = node.get_service_names_and_types()
     srv_info = [item for item in srv_list if item[0] == srv_name]
+    if len(srv_info) == 0:
+        return False
     srv_obtained_type = srv_info[0][1][0]
     return srv_type == srv_obtained_type