[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 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
 

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,28 @@ 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(1.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()
@@ -253,6 +266,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()
 
@@ -273,6 +298,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