[RQT-JTC] limits from jtc controlled joints (#1146)

(cherry picked from commit 0ae3e42)
ros-controls · Aug 26, 2024 · 066db65 · 066db65
1 parent fe71689
commit 066db65
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 47 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
@@ -45,7 +45,7 @@ def subscribe_to_robot_description(node, key="robot_description"):
     node.create_subscription(String, key, callback, qos_profile)
 
 
-def get_joint_limits(node, use_smallest_joint_limits=True):
+def get_joint_limits(node, joints_names, use_smallest_joint_limits=True):
     global description
     use_small = use_smallest_joint_limits
     use_mimic = True
@@ -71,54 +71,56 @@ def get_joint_limits(node, use_smallest_joint_limits=True):
                 if jtype == "fixed":
                     continue
                 name = child.getAttribute("name")
-                try:
-                    limit = child.getElementsByTagName("limit")[0]
+
+                if name in joints_names:
                     try:
-                        minval = float(limit.getAttribute("lower"))
-                        maxval = float(limit.getAttribute("upper"))
-                    except ValueError:
-                        if jtype == "continuous":
-                            minval = -pi
-                            maxval = pi
-                        else:
+                        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 lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!"
+                                f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!"
                             )
-                    try:
-                        maxvel = float(limit.getAttribute("velocity"))
-                    except ValueError:
+                    except IndexError:
                         raise Exception(
-                            f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!"
+                            f"Missing limits tag for the joint : {name} in the robot_description!"
                         )
-                except IndexError:
-                    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/...oint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/...oint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
@@ -238,7 +238,8 @@ def _update_jtc_list(self):
         # for _all_ their joints
         running_jtc = self._running_jtc_info()
         if running_jtc and not self._robot_joint_limits:
-            self._robot_joint_limits = get_joint_limits(self._node)  # Lazy evaluation
+            for jtc_info in running_jtc:
+                self._robot_joint_limits = get_joint_limits(self._node, _jtc_joint_names(jtc_info))
         valid_jtc = []
         if self._robot_joint_limits:
             for jtc_info in running_jtc: