[FEAT] Publish robot control mode

[ahaffemayer]

Partial implementation of #1658. Step 1-3.5 of the plan. Opening as a draft to check direction before I do the message and finish the broadcaster.

Content

RobotModeData is read from the primary client in read() and control_mode exposed as gpio/robot_control_mode.

Verified in URSim, 0 at rest, 1 in freedrive, 2 in force mode.

However I have 4 questions:

• I used getRobotModeData() rather than getRobotMode(), so the broadcaster can fill the whole RobotModeDataMsg from one call. Also robot_mode alone is already on gpio/robot_mode from RTDE. Does that match your intent?
• There's no ControlMode enum in ur_client_library; control_mode_ is a bare uint8_t. Would you prefer one added there alongside RobotMode, or just constants in ur_msgs/RobotModeDataMsg?
• getRobotModeData() returns null until the first package arrives, so I need a "not received yet" value. Currently a local CONTROL_MODE_UNKNOWN_ = 255. Should that live in RobotModeDataMsg instead so consumers can test for it?
• getRobotModeData() takes a mutex and a copy on every call, and read() runs at 500 Hz against a source that updates at ~10 Hz if I understood correctly. Should we move it to asyncThread instead?

To do

RobotModeDataMsg fields + constants, ur_robot_mode_broadcaster, docs, tests, example, real-hardware check.

[ahaffemayer]

I've put my own name on the copyright line since I'm contributing personally; happy to change it if you'd prefer a different attribution.

[urfeex]

This is definitively going in the right direction. To answer your questions:

I used getRobotModeData() rather than getRobotMode(), so the broadcaster can fill the whole RobotModeDataMsg from one call. Also robot_mode alone is already on gpio/robot_mode from RTDE. Does that match your intent?

Yes, absolutely. Sorry that I was imprecise in my task description.

There's no ControlMode enum in ur_client_library; control_mode_ is a bare uint8_t. Would you prefer one added there alongside RobotMode, or just constants in ur_msgs/RobotModeDataMsg?

We can do both, but they are independent of each other. Let's start with adding the constants in the message for now.

getRobotModeData() returns null until the first package arrives, so I need a "not received yet" value. Currently a local CONTROL_MODE_UNKNOWN_ = 255. Should that live in RobotModeDataMsg instead so consumers can test for it?

I think it would be good if it lived inside the message, yes.

getRobotModeData() takes a mutex and a copy on every call, and read() runs at 500 Hz against a source that updates at ~10 Hz if I understood correctly. Should we move it to asyncThread instead?

This is definitively a valid concern. This will lead you with the issue of thread synchronization, though. One idea would be to keep the non-double buffer inside an atomic variable, so inside write() we can reliably read from that buffer and put data into the double buffer used in the handle.
Even better than using an async thread, you could register a consumer against the primary client that updates the atomic buffer directly (e.g. by passing a simple lambda).

On the same thing: When we add the controller to the standard set we can have it running as an async controller with it's custom rate. Then, we can also remove the publish_rate parameter. Speaking about parameters: I think, you don't use (and don't need) the tf_prefix parameter, right? Thus, we could potentially omit parameters for this controller completely.