feat: Using lifecycle nodes to their full potential #35

marj3220 · 2024-12-18T14:06:21Z

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 · 2024-12-18T14:12:06Z

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

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)
on_deactivate() is called
on_activate() is called
perform_command_mode_switch() is called again to request starting the interfaces again (which, by ODrive convention, enables CLOSED_LOOP_CONTROL again)

Variant 2

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

Variant 3

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.
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 · 2024-12-19T11:29:27Z

odrive_ros2_control/src/odrive_hardware_interface.cpp

@@ -138,16 +143,33 @@ CallbackReturn ODriveHardwareInterface::on_activate(const State&) {

    // This can be called several seconds before the controller finishes starting.
    // Therefore we enable the ODrives only in perform_command_mode_switch().
+


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.

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

samuelsadok · 2024-12-19T11:47:42Z

odrive_ros2_control/src/odrive_hardware_interface.cpp

+        axis.send(msg); // Set control mode
+    }
+
+    return any_enabled;


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

marj3220 · 2024-12-19T14:55:38Z

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 · 2024-12-19T15:12:07Z

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 · 2024-12-19T15:28:33Z

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.

…d for ROS2 Control cycling

samuelsadok

cool looks mostly good now, just some minor comments

samuelsadok · 2024-12-20T10:50:10Z

odrive_ros2_control/src/odrive_hardware_interface.cpp

+        control_msg.Control_Mode = CONTROL_MODE_TORQUE_CONTROL;
+    } else {
+        RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "No control mode specified");
+        state_msg.Axis_Requested_State = AXIS_STATE_IDLE;


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 · 2024-12-20T11:22:32Z

odrive_ros2_control/src/odrive_hardware_interface.cpp

@@ -36,7 +36,9 @@ class ODriveHardwareInterface final : public hardware_interface::SystemInterface

 private:
    void on_can_msg(const can_frame& frame);
+    void set_axis_command_mode(Axis axis);


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

samuelsadok · 2024-12-20T11:26:07Z

odrive_ros2_control/src/odrive_hardware_interface.cpp

+
+    axis.send(clear_error_msg);
+    axis.send(state_msg);
+    axis.send(control_msg);


control_msg should be sent before the other two

samuelsadok reviewed Dec 19, 2024

View reviewed changes

marj3220 force-pushed the main branch from a6fdc4c to 404254d Compare December 19, 2024 21:03

Adding control and clear error functionality to the on_activate metho…

3fb205f

…d for ROS2 Control cycling

marj3220 force-pushed the main branch from 404254d to 3fb205f Compare December 19, 2024 21:04

samuelsadok reviewed Dec 20, 2024

View reviewed changes

Handling no control mode case independantly + using const.

a886851

samuelsadok approved these changes Jan 10, 2025

View reviewed changes

samuelsadok merged commit 8f61abb into odriverobotics:main Jan 10, 2025
4 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat: Using lifecycle nodes to their full potential #35

feat: Using lifecycle nodes to their full potential #35

marj3220 commented Dec 18, 2024 •

edited

Loading

CLAassistant commented Dec 18, 2024 •

edited

Loading

samuelsadok left a comment

samuelsadok Dec 19, 2024

marj3220 Dec 19, 2024

samuelsadok Dec 19, 2024

marj3220 commented Dec 19, 2024

marj3220 commented Dec 19, 2024

samuelsadok commented Dec 19, 2024

samuelsadok left a comment

samuelsadok Dec 20, 2024

samuelsadok Dec 20, 2024

samuelsadok Dec 20, 2024

		@@ -138,16 +143,33 @@ CallbackReturn ODriveHardwareInterface::on_activate(const State&) {

		// This can be called several seconds before the controller finishes starting.
		// Therefore we enable the ODrives only in perform_command_mode_switch().

feat: Using lifecycle nodes to their full potential #35

feat: Using lifecycle nodes to their full potential #35

Conversation

marj3220 commented Dec 18, 2024 • edited Loading

CLAassistant commented Dec 18, 2024 • edited Loading

samuelsadok left a comment

Choose a reason for hiding this comment

samuelsadok Dec 19, 2024

Choose a reason for hiding this comment

marj3220 Dec 19, 2024

Choose a reason for hiding this comment

samuelsadok Dec 19, 2024

Choose a reason for hiding this comment

marj3220 commented Dec 19, 2024

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 commented Dec 19, 2024

samuelsadok commented Dec 19, 2024

samuelsadok left a comment

Choose a reason for hiding this comment

samuelsadok Dec 20, 2024

Choose a reason for hiding this comment

samuelsadok Dec 20, 2024

Choose a reason for hiding this comment

samuelsadok Dec 20, 2024

Choose a reason for hiding this comment

marj3220 commented Dec 18, 2024 •

edited

Loading

CLAassistant commented Dec 18, 2024 •

edited

Loading

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