From f41e71113e4c49e575c137eeefc1181650d1932b Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Fri, 5 Sep 2025 16:26:34 +0200 Subject: [PATCH] fix issue of not listing new JTCs the problem was that we initially load only the joints limits for the joints used in active JTCs. Then in the future when we check for JTCs, we ignore any JTCs if a joint is missing. my fix is to load all joint limits in the robot description. --- .../joint_limits_urdf.py | 90 +++++++++---------- .../rqt_joint_trajectory_controller/utils.py | 2 + 2 files changed, 47 insertions(+), 45 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 e77c4410dd..1e5e80b7a9 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 @@ -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 diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py index 577cc1ab07..ec857b623d 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py +++ b/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