[RQT-JTC] Find limits only in jtc joints | add robot_description combo

rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>336</width>
-    <height>317</height>
+    <width>616</width>
+    <height>594</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -26,8 +26,11 @@
          <widget class="QComboBox" name="jtc_combo"/>
         </item>
         <item row="1" column="0">
-         <widget class="QComboBox" name="cm_combo"/>
+          <widget class="QComboBox" name="cm_combo"/>
         </item>
+        <item row="1" column="2">
+          <widget class="QComboBox" name="robot_description_combo"/>
+         </item>
         <item row="0" column="0">
          <widget class="QLabel" name="cm_list_label">
           <property name="toolTip">
@@ -51,6 +54,16 @@
           </property>
          </widget>
         </item>
+        <item row="0" column="2">
+         <widget class="QLabel" name="robot_description_list_label">
+          <property name="text">
+           <string>robot description topic</string>
+          </property>
+          <property name="buddy">
+            <cstring>robot_description_combo</cstring>
+           </property>
+         </widget>
+        </item>
        </layout>
       </item>
       <item>
@@ -172,8 +185,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>314</width>
-           <height>109</height>
+           <width>566</width>
+           <height>300</height>
           </rect>
          </property>
         </widget>
@@ -195,6 +208,7 @@
  <tabstops>
   <tabstop>cm_combo</tabstop>
   <tabstop>jtc_combo</tabstop>
+  <tabstop>robot_description_combo</tabstop>
   <tabstop>enable_button</tabstop>
  </tabstops>
  <resources/>

rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py

@@ -27,26 +27,47 @@
 from math import pi
 
 import rclpy
+import rclpy.subscription
 from std_msgs.msg import String
 
 description = ""
+robot_description_subscriber_created = False
+subscription = None
 
 
 def callback(msg):
     global description
     description = msg.data
 
 
-def subscribe_to_robot_description(node, key="robot_description"):
+def subscribe_to_robot_description(
+    node, key="robot_description"
+) -> rclpy.subscription.Subscription:
+    global robot_description_subscriber_created
+    global subscription
     qos_profile = rclpy.qos.QoSProfile(depth=1)
     qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL
     qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE
 
-    node.create_subscription(String, key, callback, qos_profile)
+    subscription = node.create_subscription(String, key, callback, qos_profile)
+    robot_description_subscriber_created = True
+    return subscription
 
 
-def get_joint_limits(node, use_smallest_joint_limits=True):
+def unsubscribe_to_robot_description(node) -> rclpy.subscription.Subscription:
+    global subscription
+    if subscription is not None:
+        node.destroy_subscription(subscription)
+
+
+def get_joint_limits(node, joints_names, use_smallest_joint_limits=True):
+    global robot_description_subscriber_created
     global description
+
+    if not robot_description_subscriber_created:
+        print("First select robot description topic name!")
+        return
+
     use_small = use_smallest_joint_limits
     use_mimic = True
 
@@ -71,54 +92,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

rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py

@@ -29,7 +29,11 @@
 
 from .utils import ControllerLister, ControllerManagerLister
 from .double_editor import DoubleEditor
-from .joint_limits_urdf import get_joint_limits, subscribe_to_robot_description
+from .joint_limits_urdf import (
+    get_joint_limits,
+    subscribe_to_robot_description,
+    unsubscribe_to_robot_description,
+)
 from .update_combo import update_combo
 
 # TODO:
@@ -170,19 +174,30 @@ def __init__(self, context):
         self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
         self._update_jtc_list_timer.start()
 
-        # subscriptions
-        subscribe_to_robot_description(self._node)
+        # Timer for running controller updates
+        self._update_robot_description_list_timer = QTimer(self)
+        self._update_robot_description_list_timer.setInterval(
+            int(1000.0 / self._ctrlrs_update_freq)
+        )
+        self._update_robot_description_list_timer.timeout.connect(
+            self._update_robot_description_list
+        )
+        self._update_robot_description_list_timer.start()
 
         # Signal connections
         w = self._widget
         w.enable_button.toggled.connect(self._on_jtc_enabled)
         w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
         w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
+        w.robot_description_combo.currentIndexChanged[str].connect(
+            self._on_robot_description_change
+        )
 
         self._cmd_pub = None  # Controller command publisher
         self._state_sub = None  # Controller state subscriber
 
         self._list_controllers = None
+        self._list_robot_descriptions = None
 
     def shutdown_plugin(self):
         self._update_cmd_timer.stop()
@@ -238,7 +253,10 @@ 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)
+                )  # Lazy evaluation
         valid_jtc = []
         if self._robot_joint_limits:
             for jtc_info in running_jtc:
@@ -252,6 +270,18 @@ def _update_jtc_list(self):
         # Update widget
         update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
 
+    def _update_robot_description_list(self):
+        if not self._list_robot_descriptions:
+            self._widget.robot_description_combo.clear()
+        self._list_robot_descriptions = []
+
+        topics_with_types = self._node.get_topic_names_and_types()
+        for topic_with_type in topics_with_types:
+            if "std_msgs/msg/String" in topic_with_type[1]:
+                self._list_robot_descriptions.append(topic_with_type[0])
+
+        update_combo(self._widget.robot_description_combo, sorted(self._list_robot_descriptions))
+
     def _on_speed_scaling_change(self, val):
         self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
 
@@ -272,6 +302,13 @@ def _on_cm_change(self, cm_ns):
         else:
             self._list_controllers = None
 
+    def _on_robot_description_change(self, robot_description):
+        unsubscribe_to_robot_description(self._node)
+
+        subscribe_to_robot_description(self._node, robot_description)
+        self._widget.jtc_combo.clear()
+        self._update_jtc_list()
+
     def _on_jtc_change(self, jtc_name):
         self._unload_jtc()
         self._jtc_name = jtc_name