diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui
index bc43f202e8..6d5d77556a 100644
--- a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui
+++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui
@@ -6,8 +6,8 @@
0
0
- 336
- 317
+ 616
+ 594
@@ -26,8 +26,11 @@
-
-
+
+ -
+
+
-
@@ -51,6 +54,16 @@
+ -
+
+
+ robot description topic
+
+
+ robot_description_combo
+
+
+
-
@@ -172,8 +185,8 @@
0
0
- 314
- 109
+ 566
+ 300
@@ -195,6 +208,7 @@
cm_combo
jtc_combo
+ robot_description_combo
enable_button
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 bdde80dd54..03c756a4a1 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
@@ -27,9 +27,12 @@
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):
@@ -37,16 +40,34 @@ def callback(msg):
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
diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
index 4cc6c901af..bdea23b279 100644
--- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
+++ b/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