From d2a17d4d0babab128fedeac8feb70776eeb837fa Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Tue, 11 Dec 2018 22:00:12 +0100 Subject: [PATCH 01/10] rqt_joint_trajectory_controller joint_limit update minor modification on rqt_joint_trajectory_controller made robot_description to be redundant to allow clear / separated namespaces for multiple robots in use --- .../joint_limits_urdf.py | 5 +++-- .../joint_trajectory_controller.py | 17 ++++++++++++++--- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py index 7cdd52c9d..2a14784db 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -11,14 +11,15 @@ import rospy -def get_joint_limits(key='robot_description', use_smallest_joint_limits=True): +def get_joint_limits(key='robot_description', use_smallest_joint_limits=True, description=None): 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 - description = rospy.get_param(key) + if description is None: + description = rospy.get_param(key) robot = xml.dom.minidom.parseString(description)\ .getElementsByTagName('robot')[0] free_joints = {} diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index e74395973..a3f8c14b8 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -186,6 +186,7 @@ def __init__(self, context): self._state_sub = None # Controller state subscriber self._list_controllers = None + self.__ns_checked =[] def shutdown_plugin(self): self._update_cmd_timer.stop() @@ -239,8 +240,17 @@ def _update_jtc_list(self): # 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() # Lazy evaluation + try: + __description = rospy.get_param('robot_description') + if running_jtc and not self._robot_joint_limits: + self._robot_joint_limits = get_joint_limits(ns=ns, description=__description) + except KeyError: + ns=self._cm_ns.rsplit('/', 1)[0] + if ns not in self.__ns_checked: + __description = rospy.get_param('{}/robot_description'.format(ns)) + for _jnt, _lims in get_joint_limits(description=__description).iteritems(): + self._robot_joint_limits[_jnt] = _lims + self.__ns_checked.append(ns) valid_jtc = [] for jtc_info in running_jtc: has_limits = all(name in self._robot_joint_limits @@ -250,6 +260,7 @@ def _update_jtc_list(self): valid_jtc_names = [data.name for data in valid_jtc] # Update widget + rospy.loginfo('show jtcs: {}'.format(valid_jtc_names)) update_combo(self._widget.jtc_combo, sorted(valid_jtc_names)) def _on_speed_scaling_change(self, val): @@ -268,9 +279,9 @@ def _on_cm_change(self, cm_ns): # 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 + self._update_jtc_list() def _on_jtc_change(self, jtc_name): self._unload_jtc() From 47919cd1b38093fef031d6d89b1f813e8f6cc137 Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Tue, 11 Dec 2018 22:08:19 +0100 Subject: [PATCH 02/10] removed leftover logging command --- .../joint_trajectory_controller.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index a3f8c14b8..a39fc7608 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -260,7 +260,6 @@ def _update_jtc_list(self): valid_jtc_names = [data.name for data in valid_jtc] # Update widget - rospy.loginfo('show jtcs: {}'.format(valid_jtc_names)) update_combo(self._widget.jtc_combo, sorted(valid_jtc_names)) def _on_speed_scaling_change(self, val): From 29830f18277c7db52a43a59fa8d60f132cd56c7e Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Wed, 12 Dec 2018 16:29:44 +0100 Subject: [PATCH 03/10] minor bug fix ignores controller manager which do not have explicit access to a robot_description file within their or a global namespace o should not be incompatible with any previous functionality as far as I can judge --- .../joint_trajectory_controller.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index a39fc7608..0aa65b6c9 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -247,10 +247,13 @@ def _update_jtc_list(self): except KeyError: ns=self._cm_ns.rsplit('/', 1)[0] if ns not in self.__ns_checked: - __description = rospy.get_param('{}/robot_description'.format(ns)) - for _jnt, _lims in get_joint_limits(description=__description).iteritems(): - self._robot_joint_limits[_jnt] = _lims - self.__ns_checked.append(ns) + try: + __description = rospy.get_param('{}/robot_description'.format(ns)) + for _jnt, _lims in get_joint_limits(description=__description).iteritems(): + self._robot_joint_limits[_jnt] = _lims + self.__ns_checked.append(ns) + except KeyError: + rospy.logerr('Could not find a valid robot_description parameter') valid_jtc = [] for jtc_info in running_jtc: has_limits = all(name in self._robot_joint_limits From cc4448c68007e75c663985ee5b2cabd7d30aa0ad Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Thu, 13 Dec 2018 14:55:08 +0100 Subject: [PATCH 04/10] corrected some old yet horrible mistake in code namespace variable ns was not properly assigned when there was a robot_description paramter set' --- .../joint_trajectory_controller.py | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 0aa65b6c9..dca00f3e6 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -243,17 +243,18 @@ def _update_jtc_list(self): try: __description = rospy.get_param('robot_description') if running_jtc and not self._robot_joint_limits: - self._robot_joint_limits = get_joint_limits(ns=ns, description=__description) + self._robot_joint_limits = get_joint_limits(description=__description) except KeyError: - ns=self._cm_ns.rsplit('/', 1)[0] - if ns not in self.__ns_checked: - try: - __description = rospy.get_param('{}/robot_description'.format(ns)) - for _jnt, _lims in get_joint_limits(description=__description).iteritems(): - self._robot_joint_limits[_jnt] = _lims - self.__ns_checked.append(ns) - except KeyError: - rospy.logerr('Could not find a valid robot_description parameter') + rospy.loginfo('Could not find robot_description parameter') + ns=self._cm_ns.rsplit('/', 1)[0] + if ns not in self.__ns_checked: + try: + __description = rospy.get_param('{}/robot_description'.format(ns)) + for _jnt, _lims in get_joint_limits(description=__description).iteritems(): + self._robot_joint_limits[_jnt] = _lims + self.__ns_checked.append(ns) + except KeyError: + rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns)) valid_jtc = [] for jtc_info in running_jtc: has_limits = all(name in self._robot_joint_limits From df66e38a9c4ad20ff5c4a28d29ec123e8f76fba9 Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Fri, 14 Dec 2018 18:01:10 +0100 Subject: [PATCH 05/10] Incorporated changes / comments from @ipa-mdl from pull-request #391 In short: - changed two underscore variables to standard variable declarations - changed parameter call of robot_description to be inside condition check to prevent number of parameter calls --- .../joint_trajectory_controller.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index dca00f3e6..527fd01a2 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -186,7 +186,7 @@ def __init__(self, context): self._state_sub = None # Controller state subscriber self._list_controllers = None - self.__ns_checked =[] + self._ns_checked = [] def shutdown_plugin(self): self._update_cmd_timer.stop() @@ -241,18 +241,18 @@ def _update_jtc_list(self): # for _all_ their joints running_jtc = self._running_jtc_info() try: - __description = rospy.get_param('robot_description') if running_jtc and not self._robot_joint_limits: - self._robot_joint_limits = get_joint_limits(description=__description) + _description = rospy.get_param('robot_description') + self._robot_joint_limits = get_joint_limits(description=_description) except KeyError: rospy.loginfo('Could not find robot_description parameter') ns=self._cm_ns.rsplit('/', 1)[0] - if ns not in self.__ns_checked: + if ns not in self._ns_checked: try: - __description = rospy.get_param('{}/robot_description'.format(ns)) - for _jnt, _lims in get_joint_limits(description=__description).iteritems(): + _description = rospy.get_param('{}/robot_description'.format(ns)) + for _jnt, _lims in get_joint_limits(description=_description).iteritems(): self._robot_joint_limits[_jnt] = _lims - self.__ns_checked.append(ns) + self._ns_checked.append(ns) except KeyError: rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns)) valid_jtc = [] From 1c0092843d5569c3e65c74bf3728cf320518be29 Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Fri, 14 Dec 2018 19:26:24 +0100 Subject: [PATCH 06/10] minimal update add namespace to list before KeyError can occure otherwise recheck loop will mess up performance --- .../joint_trajectory_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 527fd01a2..1b25b7bfd 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -249,10 +249,10 @@ def _update_jtc_list(self): ns=self._cm_ns.rsplit('/', 1)[0] if ns not in self._ns_checked: try: + self._ns_checked.append(ns) _description = rospy.get_param('{}/robot_description'.format(ns)) for _jnt, _lims in get_joint_limits(description=_description).iteritems(): self._robot_joint_limits[_jnt] = _lims - self._ns_checked.append(ns) except KeyError: rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns)) valid_jtc = [] From 6b0d09dad16d5657dbd592c50b52e2eb9f75f297 Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Thu, 10 Jan 2019 00:16:13 +0100 Subject: [PATCH 07/10] adjusted description lookup to getUrdf(root_nh, robot_description) behavior of joint trajectory controller --- .../joint_trajectory_controller_impl.h | 4 +-- .../joint_trajectory_controller.py | 31 +++++++++---------- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 8bd6e4855..c6afbd757 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -89,14 +89,14 @@ urdf::ModelSharedPtr getUrdf(const ros::NodeHandle& nh, const std::string& param ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ")."); return urdf::ModelSharedPtr(); - } + } } // Check for robot_description in root else if (!urdf->initParam("robot_description")) { ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter"); return urdf::ModelSharedPtr(); - } + } return urdf; } diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 1b25b7bfd..40e4b1157 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -186,7 +186,7 @@ def __init__(self, context): self._state_sub = None # Controller state subscriber self._list_controllers = None - self._ns_checked = [] + self._cm_checked = [] def shutdown_plugin(self): self._update_cmd_timer.stop() @@ -240,21 +240,20 @@ def _update_jtc_list(self): # List of running controllers with a valid joint limits specification # for _all_ their joints running_jtc = self._running_jtc_info() - try: - if running_jtc and not self._robot_joint_limits: - _description = rospy.get_param('robot_description') - self._robot_joint_limits = get_joint_limits(description=_description) - except KeyError: - rospy.loginfo('Could not find robot_description parameter') - ns=self._cm_ns.rsplit('/', 1)[0] - if ns not in self._ns_checked: - try: - self._ns_checked.append(ns) - _description = rospy.get_param('{}/robot_description'.format(ns)) - for _jnt, _lims in get_joint_limits(description=_description).iteritems(): - self._robot_joint_limits[_jnt] = _lims - except KeyError: - rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns)) + if running_jtc: + ns=self._cm_ns.rsplit('/', 1)[0] + if ns not in self._cm_checked: + try: + self._cm_checked.append(ns) + for _jnt, _lims in get_joint_limits(description=rospy.get_param('{}/robot_description'.format(ns))).iteritems(): + self._robot_joint_limits[_jnt] = _lims + except KeyError: + rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns)) + try: + for _jnt, _lims in get_joint_limits(description=rospy.get_param('robot_description')).iteritems(): + self._robot_joint_limits[_jnt] = _lims + except KeyError: + rospy.loginfo('Could not find robot_description parameter') valid_jtc = [] for jtc_info in running_jtc: has_limits = all(name in self._robot_joint_limits From 9ccf8ba7cadd6913bca86561ca3a2c5352027344 Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Wed, 12 Feb 2020 13:28:15 +0100 Subject: [PATCH 08/10] list to set adjustment --- .../joint_trajectory_controller.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 40e4b1157..940d6ef3f 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -186,7 +186,7 @@ def __init__(self, context): self._state_sub = None # Controller state subscriber self._list_controllers = None - self._cm_checked = [] + self._cm_checked = set() def shutdown_plugin(self): self._update_cmd_timer.stop() @@ -241,12 +241,12 @@ def _update_jtc_list(self): # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc: - ns=self._cm_ns.rsplit('/', 1)[0] + ns = self._cm_ns.rsplit('/', 1)[0] if ns not in self._cm_checked: try: - self._cm_checked.append(ns) for _jnt, _lims in get_joint_limits(description=rospy.get_param('{}/robot_description'.format(ns))).iteritems(): self._robot_joint_limits[_jnt] = _lims + self._cm_checked.add(ns) except KeyError: rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns)) try: @@ -319,7 +319,7 @@ def _load_jtc(self): 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] = {} + self._joint_pos[name] = dict() # Update joint display try: From bc3203e94b451455a812e24cb68e0f2eb58470b7 Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Wed, 12 Feb 2020 13:38:04 +0100 Subject: [PATCH 09/10] cleaned useless changes to reduce merge content --- .../joint_trajectory_controller_impl.h | 4 ++-- .../joint_trajectory_controller.py | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 0d533bbf9..5dd82d8ea 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -90,14 +90,14 @@ urdf::ModelSharedPtr getUrdf(const ros::NodeHandle& nh, const std::string& param ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ")."); return urdf::ModelSharedPtr(); - } + } } // Check for robot_description in root else if (!urdf->initParam("robot_description")) { ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter"); return urdf::ModelSharedPtr(); - } + } return urdf; } diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 940d6ef3f..92345d1ca 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -244,14 +244,14 @@ def _update_jtc_list(self): ns = self._cm_ns.rsplit('/', 1)[0] if ns not in self._cm_checked: try: - for _jnt, _lims in get_joint_limits(description=rospy.get_param('{}/robot_description'.format(ns))).iteritems(): - self._robot_joint_limits[_jnt] = _lims + for jnt, lims in get_joint_limits(description=rospy.get_param('{}/robot_description'.format(ns))).iteritems(): + self._robot_joint_limits[jnt] = lims self._cm_checked.add(ns) except KeyError: rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns)) try: - for _jnt, _lims in get_joint_limits(description=rospy.get_param('robot_description')).iteritems(): - self._robot_joint_limits[_jnt] = _lims + for jnt, lims in get_joint_limits(description=rospy.get_param('robot_description')).iteritems(): + self._robot_joint_limits[jnt] = lims except KeyError: rospy.loginfo('Could not find robot_description parameter') valid_jtc = [] @@ -319,7 +319,7 @@ def _load_jtc(self): 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] = dict() + self._joint_pos[name] = {} # Update joint display try: From 3489fd366a2af7f7b60d08f3dc1ed4dac9cea2b9 Mon Sep 17 00:00:00 2001 From: Volker Gabler Date: Wed, 12 Feb 2020 13:39:32 +0100 Subject: [PATCH 10/10] changed exception outoput to warning level --- .../joint_trajectory_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 92345d1ca..3e0f76c0f 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -248,12 +248,12 @@ def _update_jtc_list(self): self._robot_joint_limits[jnt] = lims self._cm_checked.add(ns) except KeyError: - rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns)) + rospy.logwarn('Could not find a valid robot_description parameter in namespace {}'.format(ns)) try: for jnt, lims in get_joint_limits(description=rospy.get_param('robot_description')).iteritems(): self._robot_joint_limits[jnt] = lims except KeyError: - rospy.loginfo('Could not find robot_description parameter') + rospy.logwarn('Could not find robot_description parameter') valid_jtc = [] for jtc_info in running_jtc: has_limits = all(name in self._robot_joint_limits