Minor bug fix in rqt_joint_trajectory_controller

[VGab]

Hi,

I don't know if this is needed in general, but I ran into some problems when working with multiple robots without a joint robot_description parameter being set within ROS. The gui crushes due to a KeyError as there is no parameter set.
This bug fix just runs an additional search in the subnamespace of the controller manager and finally ignores the description and thus the joint limits evaluation from joint_limits_urtf.py, which is called from joint_trajectory_controller.py.

The changes have been tested on a ubuntu 18.04 bionic with ROS melodic and to the best of my knowledge backward compatibility is guaranteed. Nevertheless, as my insight into this package may be limited, I hope my adjustments do also meet your contribution guidelines and tests

[mathias-luedtke]

Thank for raising this issue.

However, I don't agree with the look-up strategy.
We should replicated the actual behavior of joint_trajectory_controller:

1. Try robot_description in root namespace of the controller node ()
2. Crawl up ALL parent namespaces.

If I am not mistaken, your code is not crawling all namespaces.

Perhaps it would be even better to change the behavior in both places to crawl up from controller namespace (eg. with initParamWithNodeHandle)..
This would even allow to use different URDFs for different controllers.

[mathias-luedtke]

Perhaps it would be even better to change the behavior in both places to crawl up from controller namespace

This could by achieved by calling some private API:

rospy.client._init_param_server()
_, _, param_name = rospy.client._param_server.target.searchParam(self._cm_ns, 'robot_description`)
description = rospy.get_param(param_name)

Please note: this does not work if the description gets remapped within the controller node.

[VGab]

It depends on the use-case, which solution is preferable.
If one wants to adjust to controller managers being spawned at runtime this would require a param client as part of the class or instantiated at every update check, instead of the string list I added. For this case I think both solution are equally fine.
In case the description call is only to be initiated upon start for all namespaces, your solution is definitely the superior one.
As I am not sure about the preferred use case, I think it is best you decide which version to proceed with.

[mathias-luedtke]

Please note: this does not work if the description gets remapped within the controller node.

One (simple) work-around:
joint_trajectory_controller could set a _resolved_description_name param, which could be used by rqt_joint_trajectory_controller.

[mathias-luedtke]

@VGab: I don't really understand your comment.

[VGab]

I think we are both misunderstanding each other a little, I think that is to my poor explanation. I ll give it another 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)

is the version that was implemented, assuming that if there exist a global description which contains all valid URDF data there is.
In my current case I have multiple robots running under separate namespaces, so there exists no global description or an incomplete description paramter.
As a consequence the namespace of the currently selected controller-manager is checked for a description parameter and stored in the list self._ns_checked such that one can prevent rechecking at every iteration

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

In addition, it is also not sufficient to check all description parameters upon start as robots may be added to an environment at runtime. As it can be seen in this function, the joint limits dict is obtained sequentially instead of only once
Your suggestion has the advantage of storing less "unused data", e.g. joint limits of other controllers, currently not activated, and the names of the namespaces being checked before. Bbut assuming that there are multiple description packages within your ROS environment, every change requires a parameter search call using searchParam.
So I think the assumption that there should exist a robot_description within the namespace of the controller is sufficient, to live with the fallback solution I added here
But for sure the most convenient solution is definitely the combined version of exchanging the _resolved_description_name with the joint_trajectory_controller

[mathias-luedtke]

I understood your problem with the different namespaces..
I just wanted to point out that we should look for that parameter in the same places (and order) joint_trajectory_controller would search for it, otherwise rqt_joint_trajectory_controller might use the wrong one..
For example, there could be a global description and a specialised one in the sub-namespaces.

The only problem is that some names might be remapped globally.
In this case we would need the same remapping in rqt, which is quite complicated.

Bbut assuming that there are multiple description packages within your ROS environment, every change requires a parameter search call using searchParam.

I don't want to support dynamic description changes.
But different controller instances might have different descriptions assigned.

[mathias-luedtke]

I'd prefer the following approach:

1. change joint_trajectory_controller to search robot_description from controller ns upwards.
2. Expose remapped parameter name as parameter.
3. In rqt_joint_trajectory_controller read this paramter, fall-back to old behavior.

[VGab]

Tbh, I think the current way, the joint_trajectory_controller is implemented seems to be most convenient, as it first checks the private namespace of the controller manager and then falls back to the global namespace if there is nothing to be found. If we would adjust the URDF parser the way you mentioned here, the only advantage would be to find description parameters in between the current and the global namespace, which I do not really see as an advantage, but I may have misundersood something here.

Regarding your comment before: it was actually my intention from start, that the GUI follows the behavior of the controller, as the controller itself has no problem with the namespace nesting / separation. It is just the GUI that fails to adjust to the description parameter lookup. One has to admit that the GUI is monitoring all active controller managers and controllers, while the joint_trajectory_controller and controller manager isn't, such that the parsing has to initiated every time a new controller manager is spawned.

I don't want to support dynamic description changes either, that's why every namespace is only parsed once in the current implementation and stored in the list such that the description is not altered.
I just meant that new robots and thus new namespaces can be spawned at runtime, which then have to be parsed. All of them having separate controller managers, controllers and descriptions, of course.

[mathias-luedtke]

it first checks the private namespace of the controller manager and then falls back to the global namespace

No, first it tries to find the description in root_nh (defaults to the global namespace of the control node, but can be arbitrary) and then tries the global namespace of the control node and all parent namespaces!

If the control node was launched with <remap from="robot_description" to="/bla/blub"/>, rqt_joint_trajectory_controller would need to read it from /bla/blub.

[gavanderhoorn]

Perhaps it would be even better to change the behavior in both places to crawl up from controller namespace

This could by achieved by calling some private API:

rospy.client._init_param_server()
_, _, param_name = rospy.client._param_server.target.searchParam(self._cm_ns, 'robot_description`)
description = rospy.get_param(param_name)

Please note: this does not work if the description gets remapped within the controller node.

@ipa-mdl: I don't understand everything in this issue, but wouldn't rospy.search_param('robot_description') do what you suggested?

[mathias-luedtke]

wouldn't rospy.search_param('robot_description') do what you suggested?

No, because it starts the search in rqt's namespace and crawls upwards

[gavanderhoorn]

wouldn't rospy.search_param('robot_description') do what you suggested?

No, because it starts the search in rqt's namespace and crawls upwards

Right. I'd missed the part where the search must start in the controller manager ns.

[VGab]

I adjusted the checking to the behavior of getUrdf(root_nh, "robot_description") in joint_trajectory_controller_impl.h, which actually only required changing order of checking the namespaces.

Regarding the suggestion to adjust the behavior of joint_trajectory_controller, I rather disagree as this could theoretically lead to false parameter lookup procedures. In my opinion, having either a private and global description per controller manager is sufficient and should not be extended any further. Aside from that, joint_trajectory_controller would behave different than all other controllers after the change. But I might be wrong on the latter.

[bmagyar]

@ipa-mdl and @VGab , is this still required? Can we advance on it?

[bmagyar]

Ping

[VGab]

Regarding my personal ROS-setup, that spawns robots in their individual namespace with a separate robot_description parameter for each of them, I depend on this issue to be fixed.Even though I can continue to work on my personal fork, I would welcome a merge.
I just merged the latest melodic-devel version for testing etc and removed some code-dirt to ease the integration. Thanks

[Meguenani]

Dear @VGab,
Did you succeed in anyway to catch the "robot_description" from different namespaces (different robots) in the rqt joint trajectory controller ?
Thx