From 4504e8d36157770b024997208975cbc40045f050 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= <48183611+Noel215@users.noreply.github.com> Date: Fri, 29 Jul 2022 09:19:59 +0200 Subject: [PATCH] port rqt_joint_trajectory_controller to ros2 (#356) --- rqt_joint_trajectory_controller/package.xml | 35 ++ rqt_joint_trajectory_controller/plugin.xml | 21 + .../resource/double_editor.ui | 95 ++++ .../resource/joint_trajectory_controller.ui | 202 +++++++ .../resource/off.svg | 267 ++++++++++ .../resource/on.svg | 231 ++++++++ .../resource/rqt_joint_trajectory_controller | 0 .../resource/scope.png | Bin 0 -> 3205 bytes .../resource/scope.svg | 269 ++++++++++ .../__init__.py | 0 .../double_editor.py | 110 ++++ .../joint_limits_urdf.py | 100 ++++ .../joint_trajectory_controller.py | 500 ++++++++++++++++++ .../rqt_joint_trajectory_controller.py | 8 + .../update_combo.py | 57 ++ .../rqt_joint_trajectory_controller/utils.py | 416 +++++++++++++++ rqt_joint_trajectory_controller/setup.cfg | 4 + rqt_joint_trajectory_controller/setup.py | 33 ++ 18 files changed, 2348 insertions(+) create mode 100644 rqt_joint_trajectory_controller/package.xml create mode 100644 rqt_joint_trajectory_controller/plugin.xml create mode 100644 rqt_joint_trajectory_controller/resource/double_editor.ui create mode 100644 rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui create mode 100644 rqt_joint_trajectory_controller/resource/off.svg create mode 100644 rqt_joint_trajectory_controller/resource/on.svg create mode 100644 rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller create mode 100644 rqt_joint_trajectory_controller/resource/scope.png create mode 100644 rqt_joint_trajectory_controller/resource/scope.svg create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py create mode 100755 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py create mode 100644 rqt_joint_trajectory_controller/setup.cfg create mode 100644 rqt_joint_trajectory_controller/setup.py diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml new file mode 100644 index 0000000000..343f733339 --- /dev/null +++ b/rqt_joint_trajectory_controller/package.xml @@ -0,0 +1,35 @@ + + + + rqt_joint_trajectory_controller + 2.5.0 + Graphical frontend for interacting with joint_trajectory_controller instances. + + Bence Magyar + Mathias Lüdtke + Enrique Fernandez + + Apache-2.0 + + http://wiki.ros.org/rqt_joint_trajectory_controller + + Adolfo Rodriguez Tsouroukdissian + Noel Jimenez Garcia + + control_msgs + controller_manager_msgs + python_qt_binding + python3-rospkg + trajectory_msgs + rclpy + rqt_gui + rqt_gui_py + qt_gui + + + ament_python + + + diff --git a/rqt_joint_trajectory_controller/plugin.xml b/rqt_joint_trajectory_controller/plugin.xml new file mode 100644 index 0000000000..4feade126e --- /dev/null +++ b/rqt_joint_trajectory_controller/plugin.xml @@ -0,0 +1,21 @@ + + + + Graphical frontend for interacting with joint_trajectory_controller instances. + + + + + folder + Plugins related to robot tools. + + + resource/scope.png + + Monitor and send commands to running joint trajectory controllers. + + + + diff --git a/rqt_joint_trajectory_controller/resource/double_editor.ui b/rqt_joint_trajectory_controller/resource/double_editor.ui new file mode 100644 index 0000000000..547a90220f --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/double_editor.ui @@ -0,0 +1,95 @@ + + + double_editor + + + true + + + + 0 + 0 + 249 + 121 + + + + + 0 + 0 + + + + Form + + + + + + true + + + + 0 + 0 + + + + + 0 + 0 + + + + 100 + + + Qt::Horizontal + + + false + + + false + + + + + + + true + + + + 0 + 0 + + + + + 60 + 0 + + + + + 100 + 0 + + + + QAbstractSpinBox::UpDownArrows + + + false + + + 6 + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui new file mode 100644 index 0000000000..3f3b6c1186 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui @@ -0,0 +1,202 @@ + + + joint_trajectory_controller + + + + 0 + 0 + 336 + 317 + + + + Joint trajectory controller + + + + + + + + + + + + + + + + + + + + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + + + controller manager ns + + + cm_combo + + + + + + + controller + + + jtc_combo + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + <html><head/><body><p>Enable/disable sending commands to the controller.</p></body></html> + + + + + + + off.svg + on.svg + on.svgoff.svg + + + + 48 + 48 + + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + joints + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + false + + + false + + + + + + + 0 + 0 + + + + QFrame::NoFrame + + + QFrame::Plain + + + Qt::ScrollBarAsNeeded + + + Qt::ScrollBarAlwaysOff + + + true + + + + + 0 + 0 + 314 + 109 + + + + + + + + + + + + speed scaling + + + + + + + + cm_combo + jtc_combo + enable_button + + + + diff --git a/rqt_joint_trajectory_controller/resource/off.svg b/rqt_joint_trajectory_controller/resource/off.svg new file mode 100644 index 0000000000..4c91a11260 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/off.svg @@ -0,0 +1,267 @@ + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + red power button + 08 12 2006 + + + molumen + + + red power button + + + + icon + button + design + UI + interface + power + switch + on + off + red + glossy + toggle + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/on.svg b/rqt_joint_trajectory_controller/resource/on.svg new file mode 100644 index 0000000000..233d356054 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/on.svg @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + + + + image/svg+xml + + green power button + 08 12 2006 + + + molumen + + + green power button + + + + icon + button + design + UI + interface + power + switch + on + off + green + glossy + toggle + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller b/rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_joint_trajectory_controller/resource/scope.png b/rqt_joint_trajectory_controller/resource/scope.png new file mode 100644 index 0000000000000000000000000000000000000000..24c089b70a3d2f984edd161dc746b54a250c6af4 GIT binary patch literal 3205 zcmV;040`j4P)T-R~Oe|64%^_$%@!y!eAvK5PBAik=o^$?H^{-QPrp0^D*I9zcDc^YG4gJCkFN9y!X12*&Z@rcK z$5>g;>)w3x&7C-oZ+q`w6_G#m-v3?!h{zQ9Z|B@UD5d`9_U+qdO}`4@&Ye52dGG%Y zxbhY6^GW4rBJvlvZ{NPR7+3yv{cMD z!yUd`yN)0L?<%EUy?y((gB1>xW%<`4(o22G-;RFD$Yoz{<=$OqvC)cw*DC4 zW#`;q0zU#!%k{kfQ|BB%Nq^1QXNd6S9`)OOKCvf!SR62qm_J+WJAjI~-uoK>pV?E8 z%6y&ukf^rAxF%t&D08tGawz0myxca}E(n)&KxFS553!`6%;eo1D`rQG`06nKWp$BkECt z2d4RqVKyNQ9D+If=KMO_9lfUWQh9SVa|QremZ5cJu|*8SJpL{Ku=bt^q0+RH8vBhN zd#yI#{a(go67kVTm-yx9Clv9Rkt$HiasKRCo$GPyfL|JV4FIs5RAj|Sb26S6&)h0= zol-&^hIAWkZa%fcw_i9#2ae88#{N?Q<-hLn(0h)8l)~5z04Scw(;hYeEVt*J!#Rh? zuZM?hJ`0c{)#d}GX~YS8%}adinF6If4?lR3ga3PxAT+%6ZAUL@lLQf332Odmr3kbl z5QWwXrDZb%t!6aw+e2sp06@ZXA++j&6~irDt3-%`fM#50|5B4?_lV)cT|PUUqD0ZS z+M>5VqjUEX^(bbV&%pVLGb*Tf6NjXcP}mad9Bw65$*KpuUyyhcz!H`CWg3m@Lhsu4 zQmC~Xfbs-7pb^%&)@t+oEsqk<@u0yZ?xRF8nKfwb47vVv%)_*Wn&y<&5C;+Ms7WiT z)9i-y8Y71Jgqh7S)>M(OG@_py?*b02k(e_Nl&PctaC>?K7(6NoltXF>)&(?bDP>;c z*B?*0KRiN-}}3ak1Vrs%B(E0z9M_? zYINoW%Laco)94C0SBbhRB4T!)m zoFE8S+k3qJYEHeGf>-?D`@-GGA(LB7LaD39hImNYWhIXw<97brR5hd&Eaz^AQ3!#-$ za5YUU4T7MxA_@a)QB0>>;;g1WapbPRc!L(j%w!y$z_)-adxFpRsn;E?okyHJ+~vXO zm`)JW=(o7EJE5JlX+#Ora)$M?#>ponaTP`YQKINwjW{nlfTSK!s|nqt#^p;*+TAI6 zR!282wH;5psS$)IvCOo^lnK3S$0Xb!+4~R>K0i*F#C?J&VK{Ek-WhW9xtLkp!B29g zMT!Letdw}%NxaCfN-MOgzHlXs4$Ecw_uk|21d*cN1Tz+D4Udn6C{8)J-{bUj#_^*W zp%En%iNbl0bq3=s&U>^LlvEigiDLG4DmM>?0ht|QFnDoz zpl~_;e8>k68l0YVxw>ySIu7{o?v#VcG2>!Jtq^n>(?4$0yF6sCxlccBQCdTyJ+;tP z>ngn+KoA6CttAXYy0s2c4+vF2rxnx8b4q7O%Zy2pl9m~w90-I#Mc{6ya_ey)a#vuz zT~>~zGmP>HkEX|bc3%^H5>h(DB%d-ZQjD{tE`nmhz56Y8uXvul0n=iiFbD{m4n3Pu zIzzoo&{|`yUC`180AU!4bB-iQci_nR}_g6cau>O1U+zaplGlWqyOb zVGZ4I^n)3*G9wgEbQF>2xvY7>S}P)g)|&5przDIvo0yx=EB@=}d%SZUVWYoy1&u<}NSX%6K&B;_{rC&8VrE zRy`z)OO6lQ zA3$i#W;I%trMK2Holg1q@EAW<1WL2#8mKa0uihg$z0A)318!Wsf*H4%ltNP)T)nCh zpl}g!*HI6It_`t%y>)E$0=(Z$$=*Y=t5FI9Wyti1LorMa6CxE6go^$_Ms5pQhl*#j z7G+ub#mq|pixX~`jVN#+LNT6F^o4PwpvF_|>_ew+kfVq))fiFKg6iJr^M{7R!wFeE zoj&hVJl%xb|j8*|!6qoj529s(&pVrB;aN&V7007onLnG}VoyXz4P`WC6 z2@sW*Sy=%uOn|j^drzzuhnMmd2>~Y_;|;khn3;_6s79wdp_%NHnUaR8ku(NmSwc21 zADsBj3f8k$&umAqc-1d^0RW4`GT#qwkwTZUS&gzZtP{!o~i zjB*aZ%SvDU1^xAJq+2ocLN=;V<{=h zdqX0Xp^{@J!zPc81{@7X4D%@s7ce^6ycpI2YvT(H6?r|Q)9o3mvm1k|-u7}qN0w#T zS!^x(fw|kfRL4)|3WmjqU)@*W1$FtSpe%PE1q_N2nagmv?cuW}OwRZr^Adi0Bem94 z@g&zOoNFiN-IwttqjJorgCp9X?(y_q%7cTLUpzWtP>d+O!7tj;ZGH6lE1HD`=f+y< z&V|?c^!tT3bK5Ld)iaK%NjaDu^TCnj{kw|XoH8mVOw1HJ2XJA!yl7m_?_*cjJf~mD zm5*;|st^3q8tc}q3bKira#Fx-T4Lu1j&;>N&bqs|?0?I+QUZQk?Xwj8xw~eC zEeA~D9W$3QD)aUCSHIbI(|pktjeg8G=k{~Hp+&L*jwD_+)(2N3B0!wv6c94Qbrhi*)}_OKgkK zdF7Q?x+3xc@RZjuxK)x}wz%;3`%?rxQ#3x;v_7t)T}1x*op;{(^M!uxAKfp%{POoi z#x7wwATKWbM93Uc?oEI)w?_=CnEBn-ur)i r_uY4YbpCe}8vwp;^0@5#wUGY@C17*|Z}9e100000NkvXXu0mjf>V71? literal 0 HcmV?d00001 diff --git a/rqt_joint_trajectory_controller/resource/scope.svg b/rqt_joint_trajectory_controller/resource/scope.svg new file mode 100644 index 0000000000..ee4d4da371 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/scope.svg @@ -0,0 +1,269 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py new file mode 100644 index 0000000000..23a1f2c806 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from ament_index_python.packages import get_package_share_directory + +from python_qt_binding import loadUi +from python_qt_binding.QtCore import Signal +from python_qt_binding.QtWidgets import QWidget + + +class DoubleEditor(QWidget): + # TODO: + # - Actually make bounds optional + # + # - Support wrapping mode + # + # - Support unspecified (+-Inf) lower and upper bounds (both, or one) + # + # - Allow to specify the step and page increment sizes + # (right-click context menu?) + # + # - Use alternative widget to slider for values that wrap, or are + # unbounded. + # QwtWheel could be a good choice, dials are not so good because they + # use lots of vertical (premium) screen space, and are fine for wrapping + # values, but not so much for unbounded ones + # + # - Merge with existing similar code such as rqt_reconfigure's + # DoubleEditor? + """ + Widget that allows to edit the value of a floating-point value. + Optionally subject to lower and upper bounds. + """ + + valueChanged = Signal(float) + + def __init__(self, min_val, max_val): + super(DoubleEditor, self).__init__() + + # Preconditions + assert min_val < max_val + + # Cache values + self._min_val = min_val + self._max_val = max_val + + # Load editor UI + ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), + 'resource', 'double_editor.ui') + loadUi(ui_file, self) + + # Setup widget ranges and slider scale factor + self.slider.setRange(0, 100) + self.slider.setSingleStep(1) + self._scale = (max_val - min_val) / \ + (self.slider.maximum() - self.slider.minimum()) + self.spin_box.setRange(min_val, max_val) + self.spin_box.setSingleStep(self._scale) + + # Couple slider and spin box together + self.slider.valueChanged.connect(self._on_slider_changed) + self.spin_box.valueChanged.connect(self._on_spinbox_changed) + + # Ensure initial sync of slider and spin box + self._on_spinbox_changed() + + def _slider_to_val(self, sval): + return self._min_val + self._scale * (sval - self.slider.minimum()) + + def _val_to_slider(self, val): + return round(self.slider.minimum() + (val - self._min_val) / + self._scale) + + def _on_slider_changed(self): + val = self._slider_to_val(self.slider.value()) + self.spin_box.blockSignals(True) # Prevents updating the command twice + self.spin_box.setValue(val) + self.spin_box.blockSignals(False) + self.valueChanged.emit(val) + + def _on_spinbox_changed(self): + val = self.spin_box.value() + self.slider.blockSignals(True) # Prevents updating the command twice + self.slider.setValue(self._val_to_slider(val)) + self.slider.blockSignals(False) + self.valueChanged.emit(val) + + def setValue(self, val): + if val != self.spin_box.value(): + self.spin_box.blockSignals(True) + self.spin_box.setValue(val) # Update spin box first + self._on_spinbox_changed() # Sync slider with spin box + self.spin_box.blockSignals(False) + + def value(self): + return self.spin_box.value() 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 new file mode 100644 index 0000000000..6f339ba387 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python + +# TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got +# Exception: Required attribute not set in XML: upper +# upper is an optional attribute, so I don't understand what's going on +# See comments in https://github.com/ros/urdfdom/issues/36 + +import xml.dom.minidom +from math import pi + +import rclpy +from std_msgs.msg import String + +description = "" + + +def callback(msg): + global description + description = msg.data + + +def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=True): + global description + use_small = use_smallest_joint_limits + use_mimic = True + + # Code inspired on the joint_state_publisher package by David Lu!!! + # https://github.com/ros/robot_model/blob/indigo-devel/ + # joint_state_publisher/joint_state_publisher/joint_state_publisher + + 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, "/robot_description", callback, qos_profile) + rclpy.spin_once(node) + + free_joints = {} + dependent_joints = {} + + if description != "": + robot = xml.dom.minidom.parseString(description)\ + .getElementsByTagName('robot')[0] + + # Find all non-fixed joints + for child in robot.childNodes: + if child.nodeType is child.TEXT_NODE: + continue + if child.localName == 'joint': + jtype = child.getAttribute('type') + if jtype == 'fixed': + continue + name = child.getAttribute('name') + try: + limit = child.getElementsByTagName('limit')[0] + except: + continue + if jtype == 'continuous': + minval = -pi + maxval = pi + else: + try: + minval = float(limit.getAttribute('lower')) + maxval = float(limit.getAttribute('upper')) + except: + continue + try: + maxvel = float(limit.getAttribute('velocity')) + except: + continue + 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/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 new file mode 100644 index 0000000000..f3c98f9fe0 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -0,0 +1,500 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import print_function +import os +import rclpy +import threading +from ament_index_python.packages import get_package_share_directory + +from qt_gui.plugin import Plugin +from python_qt_binding import loadUi +from python_qt_binding.QtCore import QTimer, Signal +from python_qt_binding.QtWidgets import QWidget, QFormLayout + +from control_msgs.msg import JointTrajectoryControllerState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +from .utils import ControllerLister, ControllerManagerLister +from .double_editor import DoubleEditor +from .joint_limits_urdf import get_joint_limits +from .update_combo import update_combo + +# TODO: +# - Better UI suppor for continuous joints (see DoubleEditor TODO) +# - Can we load controller joints faster?, it's currently pretty slow +# - If URDF is reloaded, allow to reset the whole plugin? +# - Allow to configure: +# - URDF location +# - Command publishing and state update frequency +# - Controller manager and jtc monitor frequency +# - Min trajectory duration +# - Fail gracefully when the URDF or some other requisite is not set +# - Could users confuse the enable/disable button with controller start/stop +# (in a controller manager sense)? +# - Better decoupling between model and view +# - Tab order is wrong. For the record, this did not work: +# QWidget.setTabOrder(self._widget.controller_group, +# self._widget.joint_group) +# QWidget.setTabOrder(self._widget.joint_group, +# self._widget.speed_scaling_group) + +# NOTE: +# Controller enable/disable icons are in the public domain, and available here: +# freestockphotos.biz/photos.php?c=all&o=popular&s=0&lic=all&a=all&set=powocab_u2 + + +class JointTrajectoryController(Plugin): + """ + Graphical frontend for a C{JointTrajectoryController}. + + There are two modes for interacting with a controller: + 1. B{Monitor mode} Joint displays are updated with the state reported + by the controller. This is a read-only mode and does I{not} send + control commands. Every time a new controller is selected, it starts + in monitor mode for safety reasons. + + 2. B{Control mode} Joint displays update the control command that is + sent to the controller. Commands are sent periodically evan if the + displays are not being updated by the user. + + To control the aggressiveness of the motions, the maximum speed of the + sent commands can be scaled down using the C{Speed scaling control} + + This plugin is able to detect and keep track of all active controller + managers, as well as the JointTrajectoryControllers that are I{running} + in each controller manager. + For a controller to be compatible with this plugin, it must comply with + the following requisites: + - The controller type contains the C{JointTrajectoryController} + substring, e.g., C{position_controllers/JointTrajectoryController} + - The controller exposes the C{command} and C{state} topics in its + ROS interface. + + Additionally, there must be a URDF loaded with a valid joint limit + specification, namely position (when applicable) and velocity limits. + + A reference implementation of the C{JointTrajectoryController} is + available in the C{joint_trajectory_controller} ROS package. + """ + + _cmd_pub_freq = 10.0 # Hz + _widget_update_freq = 30.0 # Hz + _ctrlrs_update_freq = 1 # Hz + _min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration + + jointStateChanged = Signal([dict]) + + def __init__(self, context): + super(JointTrajectoryController, self).__init__(context) + self.setObjectName('JointTrajectoryController') + self._node = rclpy.node.Node("rqt_joint_trajectory_controller") + self._executor = None + self._executor_thread = None + + # Create QWidget and extend it with all the attributes and children + # from the UI file + self._widget = QWidget() + ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), + 'resource', + 'joint_trajectory_controller.ui') + loadUi(ui_file, self._widget) + self._widget.setObjectName('JointTrajectoryControllerUi') + ns = self._node.get_namespace()[1:-1] + self._widget.controller_group.setTitle('ns: ' + ns) + + # Setup speed scaler + speed_scaling = DoubleEditor(1.0, 100.0) + speed_scaling.spin_box.setSuffix('%') + speed_scaling.spin_box.setValue(50.0) + speed_scaling.spin_box.setDecimals(0) + speed_scaling.setEnabled(False) + self._widget.speed_scaling_layout.addWidget(speed_scaling) + self._speed_scaling_widget = speed_scaling + speed_scaling.valueChanged.connect(self._on_speed_scaling_change) + self._on_speed_scaling_change(speed_scaling.value()) + + # Show _widget.windowTitle on left-top of each plugin (when + # it's set in _widget). This is useful when you open multiple + # plugins at once. Also if you open multiple instances of your + # plugin at once, these lines add number to make it easy to + # tell from pane to pane. + if context.serial_number() > 1: + self._widget.setWindowTitle(self._widget.windowTitle() + + (' (%d)' % context.serial_number())) + # Add widget to the user interface + context.add_widget(self._widget) + + # Initialize members + self._jtc_name = [] # Name of selected joint trajectory controller + self._cm_ns = [] # Namespace of the selected controller manager + self._joint_pos = {} # name->pos map for joints of selected controller + self._joint_names = [] # Ordered list of selected controller joints + self._robot_joint_limits = {} # Lazily evaluated on first use + + # Timer for sending commands to active controller + self._update_cmd_timer = QTimer(self) + self._update_cmd_timer.setInterval(int(1000.0 / self._cmd_pub_freq)) + self._update_cmd_timer.timeout.connect(self._update_cmd_cb) + + # Timer for updating the joint widgets from the controller state + self._update_act_pos_timer = QTimer(self) + self._update_act_pos_timer.setInterval(int(1000.0 / + self._widget_update_freq)) + self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) + + # Timer for controller manager updates + self._list_cm = ControllerManagerLister() + self._update_cm_list_timer = QTimer(self) + self._update_cm_list_timer.setInterval(int(1000.0 / + self._ctrlrs_update_freq)) + self._update_cm_list_timer.timeout.connect(self._update_cm_list) + self._update_cm_list_timer.start() + + # Timer for running controller updates + self._update_jtc_list_timer = QTimer(self) + self._update_jtc_list_timer.setInterval(int(1000.0 / + self._ctrlrs_update_freq)) + self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) + self._update_jtc_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) + + self._cmd_pub = None # Controller command publisher + self._state_sub = None # Controller state subscriber + + self._list_controllers = None + + def shutdown_plugin(self): + self._update_cmd_timer.stop() + self._update_act_pos_timer.stop() + self._update_cm_list_timer.stop() + self._update_jtc_list_timer.stop() + self._unregister_state_sub() + self._unregister_cmd_pub() + self._unregister_executor() + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('cm_ns', self._cm_ns) + instance_settings.set_value('jtc_name', self._jtc_name) + + def restore_settings(self, plugin_settings, instance_settings): + # Restore last session's controller_manager, if present + self._update_cm_list() + cm_ns = instance_settings.value('cm_ns') + cm_combo = self._widget.cm_combo + cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] + try: + idx = cm_list.index(cm_ns) + cm_combo.setCurrentIndex(idx) + # Resore last session's controller, if running + self._update_jtc_list() + jtc_name = instance_settings.value('jtc_name') + jtc_combo = self._widget.jtc_combo + jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] + try: + idx = jtc_list.index(jtc_name) + jtc_combo.setCurrentIndex(idx) + except (ValueError): + pass + except (ValueError): + pass + + # def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget + # title bar + # Usually used to open a modal configuration dialog + + def _update_cm_list(self): + update_combo(self._widget.cm_combo, self._list_cm()) + + def _update_jtc_list(self): + # Clear controller list if no controller information is available + if not self._list_controllers: + self._widget.jtc_combo.clear() + return + + # List of running controllers with a valid joint limits specification + # 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 + valid_jtc = [] + if (self._robot_joint_limits): + for jtc_info in running_jtc: + has_limits = all(name in self._robot_joint_limits + for name in _jtc_joint_names(jtc_info)) + if has_limits: + valid_jtc.append(jtc_info) + valid_jtc_names = [data.name for data in valid_jtc] + + # Update widget + update_combo(self._widget.jtc_combo, sorted(valid_jtc_names)) + + def _on_speed_scaling_change(self, val): + self._speed_scale = val / self._speed_scaling_widget.slider.maximum() + + def _on_joint_state_change(self, actual_pos): + assert(len(actual_pos) == len(self._joint_pos)) + for name in actual_pos.keys(): + self._joint_pos[name]['position'] = actual_pos[name] + + def _on_cm_change(self, cm_ns): + self._cm_ns = cm_ns + if cm_ns: + self._list_controllers = ControllerLister(cm_ns) + # NOTE: Clear below is important, as different controller managers + # might have controllers with the same name but different + # configurations. Clearing forces controller re-discovery + self._widget.jtc_combo.clear() + self._update_jtc_list() + else: + self._list_controllers = None + + def _on_jtc_change(self, jtc_name): + self._unload_jtc() + self._jtc_name = jtc_name + if self._jtc_name: + self._load_jtc() + + def _on_jtc_enabled(self, val): + # Don't allow enabling if there are no controllers selected + if not self._jtc_name: + self._widget.enable_button.setChecked(False) + return + + # Enable/disable joint displays + for joint_widget in self._joint_widgets(): + joint_widget.setEnabled(val) + + # Enable/disable speed scaling + self._speed_scaling_widget.setEnabled(val) + + if val: + # Widgets send desired position commands to controller + self._update_act_pos_timer.stop() + self._update_cmd_timer.start() + else: + # Controller updates widgets with actual position + self._update_cmd_timer.stop() + self._update_act_pos_timer.start() + + def _load_jtc(self): + # Initialize joint data corresponding to selected controller + running_jtc = self._running_jtc_info() + self._joint_names = next(_jtc_joint_names(x) for x in running_jtc + if x.name == self._jtc_name) + for name in self._joint_names: + self._joint_pos[name] = {} + + # Update joint display + try: + layout = self._widget.joint_group.layout() + for name in self._joint_names: + limits = self._robot_joint_limits[name] + joint_widget = DoubleEditor(limits['min_position'], + limits['max_position']) + layout.addRow(name, joint_widget) + # NOTE: Using partial instead of a lambda because lambdas + # "will not evaluate/look up the argument values before it is + # effectively called, breaking situations like using a loop + # variable inside it" + from functools import partial + par = partial(self._update_single_cmd_cb, name=name) + joint_widget.valueChanged.connect(par) + except: + # TODO: Can we do better than swallow the exception? + from sys import exc_info + print('Unexpected error:', exc_info()[0]) + + # Enter monitor mode (sending commands disabled) + self._on_jtc_enabled(False) + + # Setup ROS interfaces + jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) + state_topic = jtc_ns + '/state' + cmd_topic = jtc_ns + '/joint_trajectory' + self._state_sub = self._node.create_subscription(JointTrajectoryControllerState, + state_topic, + self._state_cb, + 1) + self._cmd_pub = self._node.create_publisher(JointTrajectory, + cmd_topic, + 1) + + self.jointStateChanged.connect(self._on_joint_state_change) + + self._executor = rclpy.executors.SingleThreadedExecutor() + self._executor.add_node(self._node) + self._executor_thread = threading.Thread(target=self._executor.spin, daemon=True) + self._executor_thread.start() + + def _unload_jtc(self): + # Stop updating the joint positions + try: + self.jointStateChanged.disconnect(self._on_joint_state_change) + except: + pass + + # Reset ROS interfaces + self._unregister_state_sub() + self._unregister_cmd_pub() + self._unregister_executor() + + # Clear joint widgets + # NOTE: Implementation is a workaround for: + # https://bugreports.qt-project.org/browse/QTBUG-15990 :( + + layout = self._widget.joint_group.layout() + if layout is not None: + while layout.count(): + layout.takeAt(0).widget().deleteLater() + # Delete existing layout by reparenting to temporary + QWidget().setLayout(layout) + self._widget.joint_group.setLayout(QFormLayout()) + + # Reset joint data + self._joint_names = [] + self._joint_pos = {} + + # Enforce monitor mode (sending commands disabled) + self._widget.enable_button.setChecked(False) + + def _running_jtc_info(self): + from .utils import filter_by_type, filter_by_state + + controller_list = self._list_controllers() + jtc_list = filter_by_type(controller_list, + 'JointTrajectoryController', + match_substring=True) + running_jtc_list = filter_by_state(jtc_list, 'active') + return running_jtc_list + + def _unregister_cmd_pub(self): + if self._cmd_pub is not None: + self._node.destroy_publisher(self._cmd_pub) + self._cmd_pub = None + + def _unregister_state_sub(self): + if self._state_sub is not None: + self._node.destroy_subscription(self._state_sub) + self._state_sub = None + + def _unregister_executor(self): + if self._executor is not None: + self._executor.shutdown() + self._executor_thread.join() + self._executor = None + + def _state_cb(self, msg): + actual_pos = {} + for i in range(len(msg.joint_names)): + joint_name = msg.joint_names[i] + joint_pos = msg.actual.positions[i] + actual_pos[joint_name] = joint_pos + self.jointStateChanged.emit(actual_pos) + + def _update_single_cmd_cb(self, val, name): + self._joint_pos[name]['command'] = val + + def _update_cmd_cb(self): + dur = [] + traj = JointTrajectory() + traj.joint_names = self._joint_names + point = JointTrajectoryPoint() + for name in traj.joint_names: + pos = self._joint_pos[name]['position'] + cmd = pos + try: + cmd = self._joint_pos[name]['command'] + except (KeyError): + pass + max_vel = self._robot_joint_limits[name]['max_velocity'] + dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) + point.positions.append(cmd) + duration = rclpy.duration.Duration(seconds=(max(dur) / self._speed_scale)) + point.time_from_start = duration.to_msg() + traj.points.append(point) + + self._cmd_pub.publish(traj) + + def _update_joint_widgets(self): + joint_widgets = self._joint_widgets() + for id in range(len(joint_widgets)): + joint_name = self._joint_names[id] + try: + joint_pos = self._joint_pos[joint_name]['position'] + joint_widgets[id].setValue(joint_pos) + except (KeyError): + pass # Can happen when first connected to controller + + def _joint_widgets(self): # TODO: Cache instead of compute every time? + widgets = [] + layout = self._widget.joint_group.layout() + for row_id in range(layout.rowCount()): + widgets.append(layout.itemAt(row_id, + QFormLayout.FieldRole).widget()) + return widgets + + +def _jtc_joint_names(jtc_info): + # NOTE: We assume that there is at least one hardware interface that + # claims resources (there should be), and the resource list is fetched + # from the first available interface + + joint_names = [] + for interface in jtc_info.claimed_interfaces: + name = interface.split("/")[0] + joint_names.append(name) + + return joint_names + + +def _resolve_controller_ns(cm_ns, controller_name): + """ + Resolve a controller's namespace from that of the controller manager. + Controllers are assumed to live one level above the controller + manager, e.g. + + >>> _resolve_controller_ns('/path/to/controller_manager', 'foo') + '/path/to/foo' + + In the particular case in which the controller manager is not + namespaced, the controller is assumed to live in the root namespace + + >>> _resolve_controller_ns('/', 'foo') + '/foo' + >>> _resolve_controller_ns('', 'foo') + '/foo' + + @param cm_ns Controller manager namespace (can be an empty string) + @type cm_ns str + @param controller_name Controller name (non-empty string) + @type controller_name str + @return Controller namespace + @rtype str + """ + assert(controller_name) + ns = cm_ns.rsplit('/', 1)[0] + if ns != '/': + ns += '/' + ns += controller_name + return ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py new file mode 100755 index 0000000000..d0eb860fc5 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 + +import sys + +from rqt_gui.main import Main + +main = Main() +sys.exit(main.main(sys.argv, standalone='rqt_joint_trajectory_controller')) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py new file mode 100644 index 0000000000..64e9565cd6 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +def update_combo(combo, new_vals): + """ + Update the contents of a combo box with a set of new values. + + If the previously selected element is still present in the new values, it + will remain as active selection, even if its index has changed. This will + not trigger any signal. + + If the previously selected element is no longer present in the new values, + the combo will unset its selection. This will trigger signals for changed + element. + """ + selected_val = combo.currentText() + old_vals = [combo.itemText(i) for i in range(combo.count())] + + # Check if combo items changed + if not _is_permutation(old_vals, new_vals): + # Determine if selected value is in the new list + selected_id = -1 + try: + selected_id = new_vals.index(selected_val) + except (ValueError): + combo.setCurrentIndex(-1) + + # Re-populate items + combo.blockSignals(True) # No need to notify these changes + combo.clear() + combo.insertItems(0, new_vals) + combo.setCurrentIndex(selected_id) # Restore selection + combo.blockSignals(False) + + +def _is_permutation(a, b): + """ + @type a [] + @type b [] + @return True if C{a} is a permutation of C{b}, false otherwise + @rtype bool + """ + return len(a) == len(b) and sorted(a) == sorted(b) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py new file mode 100644 index 0000000000..e2b6ffa438 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# NOTE: The Python API contained in this file is considered UNSTABLE and +# subject to change. +# No backwards compatibility guarrantees are provided at this moment. + + +import rclpy +from controller_manager_msgs.srv import ListControllers + +# Names of controller manager services, and their respective types +_LIST_CONTROLLERS_STR = 'list_controllers' +_LIST_CONTROLLERS_TYPE = 'controller_manager_msgs/srv/ListControllers' +_LIST_CONTROLLER_TYPES_STR = 'list_controller_types' +_LIST_CONTROLLER_TYPES_TYPE = 'controller_manager_msgs/srv/ListControllerTypes' +_LOAD_CONTROLLER_STR = 'load_controller' +_LOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/LoadController' +_UNLOAD_CONTROLLER_STR = 'unload_controller' +_UNLOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/UnloadController' +_SWITCH_CONTROLLER_STR = 'switch_controller' +_SWITCH_CONTROLLER_TYPE = 'controller_manager_msgs/srv/SwitchController' +_RELOAD_CONTROLLER_LIBS_STR = 'reload_controller_libraries' +_RELOAD_CONTROLLER_LIBS_TYPE = ('controller_manager_msgs/srv/' + 'ReloadControllerLibraries') + +# Map from service names to their respective type +cm_services = { + _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, + _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, + _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, + _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, + _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, + _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE +} + + +def get_controller_managers(namespace='/', initial_guess=None): + """ + Get list of active controller manager namespaces. + + @param namespace: Namespace where to look for controller managers. + @type namespace: str + @param initial_guess: Initial guess of the active controller managers. + Typically c{initial_guess} is the output of a previous call to this method, + and is useful when periodically checking for changes in the list of + active controller managers. + Elements in this list will go through a lazy validity check (as opposed to + a full name+type API verification), so providing a good estimate can + significantly reduce the number of ROS master queries incurred by this + method. + @type initial_guess: [str] + @return: Sorted list of active controller manager namespaces. + @rtype: [str] + """ + ns_list = [] + if initial_guess is not None: + ns_list = initial_guess[:] # force copy + + # Get list of (potential) currently running controller managers + node = rclpy.node.Node("get_controller_managers_node") + ns_list_curr = _sloppy_get_controller_managers(node, namespace) + + # Update initial guess: + # 1. Remove entries not found in current list + # 2. Add new untracked controller managers + ns_list[:] = [ns for ns in ns_list if ns in ns_list_curr] + ns_list += [ns for ns in ns_list_curr + if ns not in ns_list and is_controller_manager(node, ns)] + + return sorted(ns_list) + + +def is_controller_manager(node, namespace): + """ + Check if the input namespace exposes the controller_manager ROS interface. + + This method has the overhead of several ROS master queries + (one per ROS API member). + + @param namespace: Namespace to check + @type namespace: str + @return: True if namespace exposes the controller_manager ROS interface + @rtype: bool + """ + cm_ns = namespace + if not cm_ns or cm_ns[-1] != '/': + cm_ns += '/' + for srv_name in cm_services.keys(): + if not _srv_exists(node, cm_ns + srv_name, cm_services[srv_name]): + return False + return True + + +def _sloppy_get_controller_managers(node, namespace): + """ + Get list of I{potential} active controller manager namespaces. + + The method name is prepended with I{sloppy}, and the returned list contains + I{potential} active controller managers because it does not perform a + thorough check of the expected ROS API. + It rather tries to minimize the number of ROS master queries. + + This method has the overhead of one ROS master query. + + @param namespace: Namespace where to look for controller managers. + @type namespace: str + @return: List of I{potential} active controller manager namespaces. + @rtype: [str] + """ + # refresh the list of controller managers we can find + srv_list = node.get_service_names_and_types() + + ns_list = [] + for srv_info in srv_list: + match_str = '/' + _LIST_CONTROLLERS_STR + # First element of srv_name is the service name + if match_str in srv_info[0]: + ns = srv_info[0].split(match_str)[0] + if ns == '': + # controller manager services live in root namespace + # unlikely, but still possible + ns = '/' + ns_list.append(ns) + return ns_list + + +def _srv_exists(node, srv_name, srv_type): + """ + Check if a ROS service of specific name and type exists. + + This method has the overhead of one ROS master query. + + @param srv_name: Fully qualified service name + @type srv_name: str + @param srv_type: Service type + @type srv_type: str + """ + if not srv_name or not srv_type: + return False + + srv_list = node.get_service_names_and_types() + srv_info = [item for item in srv_list if item[0] == srv_name] + srv_obtained_type = srv_info[0][1][0] + return srv_type == srv_obtained_type + + +############################################################################### +# +# Convenience classes for querying controller managers and controllers +# +############################################################################### + +class ControllerManagerLister: + """ + Convenience functor for querying the list of active controller managers. + + Useful when frequently updating the list, as it internally performs + some optimizations that reduce the number of interactions with the + ROS master. + + Example usage: + >>> list_cm = ControllerManagerLister() + >>> print(list_cm()) + """ + + def __init__(self, namespace='/'): + self._ns = namespace + self._cm_list = [] + + def __call__(self): + """Get list of running controller managers.""" + self._cm_list = get_controller_managers(self._ns, self._cm_list) + return self._cm_list + + +class ControllerLister: + """ + Convenience functor for querying loaded controller data. + + The output of calling this functor can be used as input to the different + controller filtering functions available in this module. + + Example usage. Get I{running} controllers of type C{bar_base/bar}: + >>> list_controllers = ControllerLister('foo_robot/controller_manager') + >>> all_ctrl = list_controllers() + >>> running_ctrl = filter_by_state(all_ctrl, 'running') + >>> running_bar_ctrl = filter_by_type(running_ctrl, 'bar_base/bar') + """ + + def __init__(self, namespace='/controller_manager'): + """ + @param namespace Namespace of controller manager to monitor. + + @type namespace str + """ + self._node = rclpy.node.Node("controller_lister") + self._srv_name = namespace + '/' + _LIST_CONTROLLERS_STR + self._srv_client = self._create_client() + + """ + @return: Controller list. + @rtype: [controller_manager_msgs/ControllerState] + """ + + def __call__(self): + controller_list = self._srv_client.call_async(ListControllers.Request()) + rclpy.spin_until_future_complete(self._node, controller_list) + return controller_list.result().controller + + def _create_client(self): + return self._node.create_client(ListControllers, self._srv_name) + +############################################################################### +# +# Convenience methods for filtering controller state information +# +############################################################################### + + +def filter_by_name(ctrl_list, ctrl_name, match_substring=False): + """ + Filter controller state list by controller name. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_name: Controller name + @type ctrl_name: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified name + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'name', ctrl_name, match_substring) + + +def filter_by_type(ctrl_list, ctrl_type, match_substring=False): + """ + Filter controller state list by controller type. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_type: Controller type + @type ctrl_type: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified type + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'type', ctrl_type, match_substring) + + +def filter_by_state(ctrl_list, ctrl_state, match_substring=False): + """ + Filter controller state list by controller state. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_state: Controller state + @type ctrl_state: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified state + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'state', ctrl_state, match_substring) + + +def filter_by_hardware_interface(ctrl_list, + hardware_interface, + match_substring=False): + """ + Filter controller state list by controller hardware interface. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param hardware_interface: Controller hardware interface + @type hardware_interface: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified hardware interface + @rtype: [controller_manager_msgs/ControllerState] + """ + list_out = [] + for ctrl in ctrl_list: + for resource_set in ctrl.claimed_resources: + if match_substring: + if hardware_interface in resource_set.hardware_interface: + list_out.append(ctrl) + break + else: + if resource_set.hardware_interface == hardware_interface: + list_out.append(ctrl) + break + return list_out + + +def filter_by_resources(ctrl_list, + resources, + hardware_interface=None, + match_any=False): + """ + Filter controller state list by claimed resources. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param resources: Controller resources + @type resources: [str] + @param hardware_interface Controller hardware interface where to look for + resources. If specified, the requested resources will only be searched for + in this interface. If unspecified, all controller hardware interfaces will + be searched for; i.e., if a controller claims resources from multiple + interfaces, the method will succeed if _any_ interface contains the + requested resources (any or all, depending on the value of C{match_any}). + Specifying this parameter allows finer control over determining which + interfaces claim specific resources. + @param match_any: If set to False, all elements in C{resources} must + be claimed by the interface specified in C{hardware_interface} (or _any_ + interface, if C{hardware_interface} is unspecified) for a positive match. + Note that a controller's resources can contain additional entries than + those in C{resources}). + If set to True, at least one element in C{resources} must be claimed by + the interface specified in C{hardware_interface} (or _any_ interface, if + C{hardware_interface} is unspecified) for a positive match. + @type match_any: bool + @return: Controllers matching the specified hardware interface + @rtype: [controller_manager_msgs/ControllerState] + """ + list_out = [] + for ctrl in ctrl_list: + for resource_set in ctrl.claimed_resources: + if (hardware_interface is None or + hardware_interface == resource_set.hardware_interface): + for res in resources: + add_ctrl = not match_any # Initial flag value + if res in resource_set.resources: + if match_any: # One hit: enough to accept controller + add_ctrl = True + break + else: + if not match_any: # One miss: enough to discard controller + add_ctrl = False + break + if add_ctrl: + list_out.append(ctrl) + break + return list_out + + +def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): + """Filter input list by the value of its elements' attributes.""" + list_out = [] + for val in list_in: + if match_substring: + if attr_val in getattr(val, attr_name): + list_out.append(val) + else: + if getattr(val, attr_name) == attr_val: + list_out.append(val) + return list_out + +############################################################################### +# +# Convenience methods for finding potential controller configurations +# +############################################################################### + +# def get_rosparam_controller_names(namespace='/'): +# """ +# Get list of ROS parameter names that potentially represent a controller +# configuration. + +# Example usage: +# - Assume the following parameters exist in the ROS parameter: +# server: +# - C{/foo/type} +# - C{/xxx/type/xxx} +# - C{/ns/bar/type} +# - C{/ns/yyy/type/yyy} +# - The potential controllers found by this method are: + +# >>> names = get_rosparam_controller_names() # returns ['foo'] +# >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] + +# @param namespace: Namespace where to look for controllers. +# @type namespace: str +# @return: Sorted list of ROS parameter names. +# @rtype: [str] +# """ +# import rosparam +# list_out = [] +# all_params = rosparam.list_params(namespace) +# for param in all_params: +# # Remove namespace from parameter string +# if not namespace or namespace[-1] != '/': +# namespace += '/' +# param_no_ns = param.split(namespace, 1)[1] + +# # Check if parameter corresponds to a controller configuration +# param_split = param_no_ns.split('/') +# if (len(param_split) == 2 and param_split[1] == 'type'): +# list_out.append(param_split[0]) # It does! +# return sorted(list_out) diff --git a/rqt_joint_trajectory_controller/setup.cfg b/rqt_joint_trajectory_controller/setup.cfg new file mode 100644 index 0000000000..4c0b982e25 --- /dev/null +++ b/rqt_joint_trajectory_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/rqt_joint_trajectory_controller +[install] +install-scripts=$base/lib/rqt_joint_trajectory_controller diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py new file mode 100644 index 0000000000..584655963d --- /dev/null +++ b/rqt_joint_trajectory_controller/setup.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +from setuptools import setup +from glob import glob + +package_name = "rqt_joint_trajectory_controller" + +setup( + name=package_name, + version="0.0.1", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/resource", glob("resource/*.*")), + ("share/" + package_name, ["plugin.xml"]), + + ], + install_requires=["setuptools"], + zip_safe=True, + author="Noel Jimenez Garcia", + author_email="noel.jimenez@pal-robotics.com", + maintainer="Bence Magyar", + maintainer_email="bence.magyar.robotics@gmail.com", + license="Apache License, Version 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "rqt_joint_trajectory_controller = \ + rqt_joint_trajectory_controller.rqt_joint_trajectory_controller:main", + ], + }, +)