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