Skip to content

Conversation

peter-mitrano-ar
Copy link

The Problem

If you launched rqt_joint_trajectory_controller with one JTC running, then started a new JTC with different joints, it would not show up.

How to reproduce the problem

  • Start a robot (I used fake hardware) with one JTC (e.g. left_arm_jtc) running for some joints
  • ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller and see you controller listed there
  • Then enable a second controller (e.g. right_arm_jtc) which controls some different joints
  • Notice that the drop-down of available JTCs in the RQT app does not include the new JTC. If you restart the GUI, you can then see both controllers. But you should not need to restart!

Why does this happen?

Because the timer callback code actually only load of the joints limits from the URDF once, and only checks for joints "used" by the JTC, ignoring all other joints. Then when we check for new JTCs, they get filtered out as "invalid" because the code checks if we know the limits of the joints, which we do not if the new JTC has different joints than the initial set.

Proposed solution

Load all joint limits in the robot description into JointTrajectoryController._robot_joint_limits.
This still assumes the URDF never changes.
But now you can start new JTCs and they are automatically listed!

To be honest the diff display makes this look more confusing than it is. All I did was move the if name in joints_names: check inside the except IndexError clause. That way, we still throw an error if somehow the urdf does not contain a limit for a joint which is needed by a JTC.


Also, I snuck in one other small fix.
The change to utils.py fixes this error message. You can get this error (randomly) when starting JTC just because node.get_service_names_and_types() sometimes takes a second to return everything.

Traceback (most recent call last):
  File "/home/peter.mitrano/code/p2_4_jazzy_ws/build/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py", line 229, in _update_cm_list
    update_combo(self._widget.cm_combo, self._list_cm())
                                        ^^^^^^^^^^^^^^^
  File "/home/peter.mitrano/code/p2_4_jazzy_ws/build/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py", line 185, in __call__
    self._cm_list = get_controller_managers(self._ns, self._cm_list)
                    ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/peter.mitrano/code/p2_4_jazzy_ws/build/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py", line 80, in get_controller_managers
    ns_list += [ns for ns in ns_list_curr if ns not in ns_list and is_controller_manager(node, ns)]
                                                                   ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/peter.mitrano/code/p2_4_jazzy_ws/build/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py", line 101, in is_controller_manager
    if not _srv_exists(node, cm_ns + srv_name, cm_services[srv_name]):
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/peter.mitrano/code/p2_4_jazzy_ws/build/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py", line 155, in _srv_exists
    srv_obtained_type = srv_info[0][1][0]
                        ~~~~~~~~^^^

Please let me know if I should also make PRs for humble/jazzy!

the problem was that we initially load only the joints limits for the joints used in active JTCs.
Then in the future when we check for JTCs, we ignore any JTCs if a joint is missing.

my fix is to load all joint limits in the robot description.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

1 participant