Implementing prioritized merging of controllers for safety and teleoperation

[patrykcieslak]

Hello! I would like to ask the developers about the best approach to implement safety and teleoperation capabilities in the ros_control framework. We are controlling an autonomous underwater vehicle and many times we are facing the need for taking over the control by the operator or by the always running safety system which for example commands to surface the vehicle in case of navigation issues, low altitude or other technical problems. Moreover, we need to be able to take over the control of the robot with a joystick while running a new type of autonomous controller. The way we were solving the issues by now was to implement a node that is merging the velocity and position requests based on priorities and axes of the robot and injecting the commands at the position or velocity level which were in a cascade connection.

We would like to know what would be the best way to integrate this functionality in ros_control framework. We have two ideas:

1. Implement a hardware interface with multiple command inputs per joint, which we merge in the write() method using some rules.
2. Use a hardware interface connected with 3 controller managers, each of them with a set of controllers loaded.

Ad1.

• we define 3 joint effort interfaces
• we have to define 3 joint state interface handles per joint (and three names ) because there is no way to define in the controller list which joint effort interface to attach the controller to
• we need to indicate a disabled joint by sending some big number for example (to know how to merge commands) Ad2.
• can we actually do this without risking serious problems?

Any other idea will be appreciated!

Using an external node to merge requests and then just publish to the controllers is not a good option because we want to have the basic tested controllers to be used for safety and teleoperation, independent from the new controllers we develop and test, however controlling the same axes of the robot.

Regards, Patryk

[destogl]

Hi @patrykcieslak,

I needed a bit of time to think about your issue.

2. Use a hardware interface connected with 3 controller managers, each of them with a set of controllers loaded.

I am really not sure how you plan to realize this because this is not intended and probably not possible to do.

1. Implement a hardware interface with multiple command inputs per joint, which we merge in the write() method using some rules.

A more interesting and feasible approach :)

This goes in the right direction, but why not to do this even simpler. Why don't you define multiple controller and a “mission_control” node, then switches them as needed? This means that when you press a specific button on a joystick, this node would switch controllers using “switch_controllers” service in controller manager. This will immediately stop current running controller and switch to the other one. A few clients of mine are actually using his for very similar use-cases. Would this be feasible in your case?

For the curiosity, are your controllers open-source? I would love to check them if they are :)

[patrykcieslak]

Hello @destogl,

Thanks for your answer!

Before you wrote the message I have already implemented a solution based on 3 controller managers. There is no problem to instantiate multiple CMs in one hardware interface and nothing stops you from setting the same resources for all of them. Since our case has a fixed structure of priority: safety -> teleoperation -> user, it was possible to use a very simple strategy when updating control. What I do is simply put NAN in the cmd vector before I start the update and then update each of the CM's in a sequence from the lowest priority to the highest, checking if the memory of cmd was changed by checking if it is still NAN for every component. This way I can merge the commands for consecutive priorities and do one write to the robot at the end. The good thing for us is that we separate safety and teleoperation from the user written controllers, in terms of namespace, in terms of services to switch and list, and so on. We also plan to subclass the controller manager to, e.g., disable unloading controllers for the safety/teleop. The idea is to make it as hard as possible for the user to mess up the fixed functionality of the robot while testing his own ideas.

In terms of new controllers, I have not yet developed any for ROS control, but I will be doing it soon. We are working with Task-Priority and I am interested in MPC with Gaussian Process. I am not sure when we will release it to the public though.

Cheers!

[destogl]

There is no problem to instantiate multiple CMs in one hardware interface and nothing stops you from setting the same resources for all of them.

This sounds like ros_control. Are you using ROS (1) or ROS 2?

Since our case has a fixed structure of priority: safety -> teleoperation -> user, it was possible to use a very simple strategy when updating control. What I do is simply put NAN in the cmd vector before I start the update and then update each of the CM's in a sequence from the lowest priority to the highest, checking if the memory of cmd was changed by checking if it is still NAN for every component. This way I can merge the commands for consecutive priorities and do one write to the robot at the end.

This sounds very much like "controller-chaining". We are planning to implement this early next year.