feat: Using lifecycle nodes to their full potential

[marj3220]

Similar to this PR, pressing and releasing the estop on our robot causes communication with the hardware interface to be cut off.

Based on my understanding of the lifecycle nodes concept, deactivating and reactivating the hardware should restore CLOSED_LOOP_CONTROL state.

Implementing this functionality would align the hardware interface more closely with the intended behavior of lifecycle nodes.

[CLAassistant]

All committers have signed the CLA.

[samuelsadok]

I'm having some gaps in ROS2 understanding to fully assess this PR. What's the behavior of ROS2/ros2_control when you cycle the active state to recover? There are multiple variants I could imagine:

Variant 1

1. perform_command_mode_switch() is called to request stopping all interfaces (and thus, by convention of the ODrive hardware interface, put the ODrive to IDLE)
2. on_deactivate() is called
3. on_activate() is called
4. perform_command_mode_switch() is called again to request starting the interfaces again (which, by ODrive convention, enables CLOSED_LOOP_CONTROL again)

Variant 2

1. on_deactivate() is called and implicitly also means "stop all interfaces", which by ODrive convention puts the axis to IDLE
2. on_activate() is called. All interfaces are still considered stopped.
3. perform_command_mode_switch() is called to request starting the interfaces again (which puts the axis to CLOSED_LOOP_CONTROL)

Variant 3

1. on_deactivate() is called but it does not implicitly stop any interfaces. Implementers of the hardware interface can decide if motor is stopped, put to IDLE, etc.
2. on_activate() is called. Since the interfaces weren't implicitly stopped, the axis is expected to automatically resume operation with the same mode as when on_deactivate() was called.

Could you run a test to verify which of these variants is what's happening?

The link you sent is for managed nodes which may be good for reference, but which I believe does not directly apply here (unless we're expected to know that ros2_control is a managed node and active/inactive statemachine is directly passed through to the hardware interfaces).

Additional high level comments:

• "Restoring communication" is not the right way to think about this, it's rather "restoring CLOSED_LOOP_CONTROL state" (which may have been exited for any reason, including but not limited to communication being interrupted).
• Please rebase such that git history is clean (avoid merges and multiple commits for a single logical change and keep formatting changes in a formatting-only commit)

[samuelsadok]

The comment above suggests that the axis should not be put to CLOSED_LOOP_CONTROL in here. But I need to fill in the gap in understanding to know what's the best thing to do.

[marj3220]

If it helps, I show when on_activate() and perform_command_mode_switch() is called during the initialization in my screenshots.

[samuelsadok]

I think sending the actual state request should also be part of this function

[marj3220]

Thank you for the detailed review, @samuelsadok!

When cycling the active state of the hardware interface, the behavior aligns with what you described in Variant 3.

For more context, perform_command_mode_switch() is invoked only during the initialization of the hardware controller when it specifies the control modes of the hardware interfaces (e.g., during launch) or when a mode change is explicitly requested—via terminal or another node. In scenarios where the control mode remains constant, such as when only velocity control is required, perform_command_mode_switch() is typically called just once.

There are some screenshots below of our terminal to demonstrate this behavior on our robot.

I forgot to add some context, the interfaces and controllers are indeed managed nodes. This source code confirms that SystemInterface (the parent class of ODriveHardwareInterface) inherits from LifecycleNodeInterface.

For your high-level comments: you’re absolutely correct. I’ll revise the PR description to clarify the focus on restoring the CLOSED_LOOP_CONTROL state rather than "restoring communication." I’ll also clean up the git history.

Thanks again for your feedback!

Initialization of the interfaces and the controller (showing when perform_command_switch() and on_activate is called with the proposed code)

On cycling the active state of the ODrives

[marj3220]

For more clarity, here is another screenshot where I added a print for when the code is in on_activate() or in perform_command_mode_switch() and used grep to only see the parts of interest.

[samuelsadok]

I see, that clears things up, thanks for the elaboration. So it seems that the active/inactive state is mostly independent from the "command mode". Extrapolating from this, I would assume that perform_command_mode_switch() could even be called while the hardware interface is inactive.

In that case I suggest the following change:

• Add a new state variable bool active_
• on_activate() and on_deactivate() set this to true/false
• set_axis_command_mode() takes into account active_, and if it's false, treats it as an IDLE request. The function also takes care of sending all messages related to a state change (set control mode, clear errors, request state).
• set_axis_command_mode() called from on_activate(), on_deactivate() and perform_command_mode_switch().

This way, on the first call to on_activate() the CLOSED_LOOP_CONTROL request is deferred to perform_command_mode_switch(), but on a subsequent on_deactivate()/on_activate() cycle, CLOSED_LOOP_CONTROL is requested immediately. At each point we send a consistent set of messages and it also works if perform_command_mode_switch() is called while inactive.

[samuelsadok]

cool looks mostly good now, just some minor comments

[samuelsadok]

I think it's best to treat this case and the !active_ case the same way, by sending a single IDLE request message. So only send the clear_error_msg and control_msg conditionally. The way it's written here, if all interfaces are stopped, it's sending a Set_Controller_Mode_msg_t with an undefined Control_Mode.

[samuelsadok]

Axis axis => const Axis&
(and with that the definition of send() needs to be changed to const)

[samuelsadok]

control_msg should be sent before the other two