Freedrive Controller

[VinDp]

The PR aims at introducing the possibility of enabling the freedrive mode through the ROS2 driver. It relies on the implementation of a specific controller to handle it, in order to correctly deactivate other controllers and avoid unsafe jumps of the robot once the freedrive mode is switched off.

[gavanderhoorn]

Would an action be nicer, semantically?

Start of action -> enable freedrive.

Cancel action -> stop freedrive.

The goal client handle would become something similar to a 'token' or 'mutex' (not exactly, as it wouldn't be as safe or strict, but conceptually).

Concurrent goal -> reject (something/someone else already has 'control' of the freedrive mode).

Event on robot or robot-controller causes freedrive to be disengaged -> abort action (notifies client something happened). A subscriber can't do that.

it would also make it (slightly) harder for something to take over control of the robot by just spamming Bool messages.

And it would make it semantically clearer it controls some specific functionality (ie: because a custom action goal needs to be submitted, instead of a generic Bool).

[fmauch]

Would an action be nicer, semantically?

I have to say, that makes sense! Thank you for the input :-)

[gavanderhoorn]

One aspect that would need some thought though: time-out (ie: when should freedrive be stopped due to the client no longer 'responding').

There might be some way to exploit liveliness QoS or something, but I'm not sure how that would work with actions.

[VinDp]

Currently reached a first version working state, still missing:

• Handling timeout
• Tests

Tested on URSim, requires #34.

[gavanderhoorn]

High-level comment (nice work already though): the (proposed?) action is called EnableFreeDriveMode.

To me (non-native speaker though) this implies the action of enabling freedrive mode -- so changing the state of the robot from not in freedrive mode to having freedrive mode active -- is what this is about. That's a bit strange, as it seems like that would almost be an instantaneous transition, not something which would require feedback and a ROS action.

I realise something like "put-and-keep-robot-in-freedrive-mode" is a bit silly as a name, but that seems to be more what this controller does, correct?

(note: I'm not asking for the name to be changed necessarily. Just thought I'd point it out)

[fmauch]

One aspect that would need some thought though: time-out (ie: when should freedrive be stopped due to the client no longer 'responding').

There might be some way to exploit liveliness QoS or something, but I'm not sure how that would work with actions.

I wrapped my head around this and I currently don't see a way of doing this.

One option would be to use a separate topic for a keepalive signal that could be used opt-in or opt-out, but that feels like a cumbersome workaround.

High-level comment (nice work already though): the (proposed?) action is called EnableFreeDriveMode.

I was also stumbling upon that. Maybe "use freedrive mode" could be a proper label? That could imply that, as long as this action is running, the robot's freedrive mode is used. I don't quite like the word "use". Matching synonyms could be "utilize", "operate", "run".

[VinDp]

I see the point on the action name and I agree that "enable" is not the best choice to describe what the action does. If we exclude using two words (like StartandKeepFreedriveMode), I would also choose something similar to "use", and tha regard maybe I like "run" more.

[gavanderhoorn]

I wrapped my head around this and I currently don't see a way of doing this.

I haven't looked into it, but would there be a way to retrieve the liveness QoS data/settings for the feedback or perhaps status publisher and see whether it's still connected to a client?

It might need some unorthodox reaching around/into the action-machinery, but DDS certainly supports these kinds of things.

---

Edit: a quick search seems to imply it's possible to configure custom QoS for specific Action servers. See rclcpp_actions::Server<..>::create_server<..>(..) fi. It now just calls rcl_action_server_get_default_options(), which provides -- as the name implies -- a default set of QoS (here).

Not a full solution yet, but perhaps an interesting 'in'.

[fmauch]

I've been testing around with that already on fmauch@f3a2e3d (Another feature branch, another action, but I just had that at hand).

Setting a lifespan (which seemed the most obvious thing for me) of 100ms seemed not to have any effect. My test was running a trajectory through our example_move script and then exiting that script during motion.

One question is, whether the underlying action server implementation would care at all, if this would happen. If the message would expire on a DDS level - would that even be visible to the publisher / the action server? Then, API access is a whole other story.

But thank you for supporting me that I have been poking in the right direction.

[gavanderhoorn]

Setting QoS is just one thing that would be needed. I would probably go for Liveliness instead actually.

You'd then have to register a callback to be called whenever the Liveliness requirements are no longer met.

I can't really look for it right now, but I'm pretty sure I've seen discussion/PRs around callbacks for these kinds of events in the ROS 2 stack (perhaps even at RMW level).

[fmauch]

I just tested things locally and had discussions with others.

I would like to stick with the heartbeat topic instead of the action as we had it initially for now because of the following reasons:

• Handling timeout using the action doesn't really seem straightforward. From my understanding (and that of my colleagues who know QoS stuff better than I do) it is not intended for a publisher to know whether or not a certain subscriber went away or not.
• Some people were also saying that this is a great misuse of an action. From their point of view an action should be something that's requested for execution, informs the caller about the progress while executing the thing and then finishing at some point either successful or not. Our action here is never supposed to be finished and we don't give the caller any feedback (we could think of some, though). Instead, we want feedback from the caller, whether it is still alive or not.
• With local tests the action wasn't even reliably canceled when using ros2 action send goal from the command line. As long as we haven't figured out a reliable way to handle timeout, I don't really want to take that route.
• Using the heartbeat topic should be straightforward to use. Yes, there can be potentially multiple clients requesting to keep freedrive mode active but that's easy to debug with standard ROS tools.
• Others have come up with using the heartbeat topic, as well, e.g. Add free drive mode support Universal_Robots_ROS_Driver#718

I'm not saying that this is the right interface to use, but for the sake of getting this PR forward to a mergable state, I would say that another, maybe semantically more meaningful interface can be added at a later stage.