Make diff_drive_controller a ChainableControllerInterface #1485

arthurlovekin · 2025-01-10T06:32:50Z

Using dea3d3c as a reference, I made the diff_drive_controller into a ChainableControllerInterface. I used a RealtimeBuffer to handle the incoming velocity command messages, and I also added lifecycle checks and support for the speed_limiter that were not included in dea3d3c. I also added some tests that run the chainable controller in unchained mode, and I intend to test it in chained mode in one of my current projects (though I couldn't readily figure out how to make this into a test).

christophfroehlich · 2025-01-10T09:02:58Z

Please always install pre-commit to avoid failing tests ;) otherwise, run it manually with pre-commit run --all

regarding tests: have a look at the mecanum controller, it is chainable anf there are exactly those tests already available

arthurlovekin · 2025-01-11T01:43:01Z

I've fixed my formatting by running pre-commit run --all-files and added a test for chained mode, using the mecanum controller as a reference.

codecov · 2025-01-11T06:51:52Z

Codecov Report

Attention: Patch coverage is 88.29787% with 22 lines in your changes missing coverage. Please review.

Project coverage is 84.15%. Comparing base (e0b929b) to head (794b77c).

Files with missing lines	Patch %	Lines
...iff_drive_controller/src/diff_drive_controller.cpp	81.57%	11 Missing and 3 partials ⚠️
...ive_controller/test/test_diff_drive_controller.cpp	92.85%	8 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1485      +/-   ##
==========================================
+ Coverage   83.86%   84.15%   +0.29%     
==========================================
  Files         122      121       -1     
  Lines       11148    11298     +150     
  Branches      948      955       +7     
==========================================
+ Hits         9349     9508     +159     
+ Misses       1486     1473      -13     
- Partials      313      317       +4

Flag	Coverage Δ
unittests	`84.15% <88.29%> (+0.29%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...ive_controller/test/test_diff_drive_controller.cpp	`94.32% <92.85%> (-0.87%)`	⬇️
...iff_drive_controller/src/diff_drive_controller.cpp	`72.94% <81.57%> (+1.17%)`	⬆️

... and 4 files with indirect coverage changes

christophfroehlich

Thanks for the follow-up. In principle this looks very good, some comments:

shouldn't we also check for proper exported reference interfaces like here?
in principle I'm fine with the change from RealtimeBox to RealtimeBuffer, but this soon might be changed to the lock-free queue. Is there a reason to change this in this PR? If not, I'd ask to remove the change because it is out-of-scope here.

arthurlovekin · 2025-01-11T17:22:02Z

To be honest I did not know the full implications of switching from a RealtimeBox to RealtimeBuffer, but I chose the change because every other example of chainable interface (eg. mecanum, passthrough, chainable-diff-drive) used it. I figured it probably had a reason for being used, but at the very least using the RealtimeBuffer would provide consistency across the chainable controllers. It seemed like the RealtimeBuffer was designed specifically for use-cases like the chainable interface, where the reference_interfaces could either be safely updated at a realtime rate or more slowly through the publisher, whereas the RealtimeBox would simply make a best-effort attempt at getting and setting the variables (which may in fact be sufficient for this use case) but didn't explicitly account for realtime vs non-realtime. Let me know if you would like me to proceed with changing RealtimeBuffer back to RealtimeBox, and I will do it.

I agree that there should be a check for properly exported reference interfaces. I will imitate the mecanum drive when_controller_configured_expect_properly_exported_interfaces test. I will try to do that later today.

with chained mode tests + export reference interfaces test

arthurlovekin · 2025-01-12T02:15:55Z

I just added a test for export_reference_interfaces().

christophfroehlich

OK I see your point regarding the RT buffer (I also think that the box was not the right thing to use). Thanks for adding the additional test.

I tested this version with the demos in the non-chained mode, and it still works like expected.

For future PRs: It is not desired to squash the commits (we do that at the time of the merge in the base branch), it just takes more time to review because I can't see now what has changed since the time of my last review.

arthurlovekin · 2025-01-12T18:31:15Z

Apologies, in the future I will not squash commits while the PR is in progress. The changes I made were to add the test TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported), and I renamed TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) to TEST_F( TestDiffDriveController, command_and_state_interface_configuration_succeeds_when_wheels_are_specified) just to make it more clear that that test evaluates the state_interface_configuration() and command_interface_configuration() functions. The "Compare" button next to my force-push above shows these changes alone.

christophfroehlich · 2025-01-12T20:40:33Z

The "Compare" button next to my force-push above shows these changes alone.

you are right, I haven't realized that button before. You never stop learning ;)

mergify · 2025-01-13T12:33:36Z

This pull request is in conflict. Could you fix it @arthurlovekin?

arthurlovekin · 2025-01-13T23:23:29Z

Is there a way that I can fix the Rolling ABI compatibility check? It seems to be broken because of the change from ControllerInterface to ChainableControllerInterface and I see no way around that.

Side question: I see in the documentation that there is a Working Group meeting every second Wednesday, but I cannot find the link to join. Would it be possible for me to attend?

olivier-stasse · 2025-01-14T05:50:33Z

Side question: I see in the documentation that there is a Working Group meeting every second Wednesday, but I cannot find the link to join. Would it be possible for me to attend?

Dear @arthurlovekin the announcement, in ROS discourse, for tomorrow's meeting group is here: https://discourse.ros.org/t/ros2-control-working-group-meeting-15th-january-2025/41528

To join I copied paste the invitation in the announcement:
"You can join the meeting through google groups or consult the calendar for the Google Meet link (note that the calendar may use a different time zone than yours)."

saikishor

Thanks for the nice contribution. I've some questions around the code that I've reviewed. Thank you

saikishor · 2025-01-12T22:28:38Z

diff_drive_controller/src/diff_drive_controller.cpp

+  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
+  {
+    if (!is_halted)
+    {
+      halt();
+      is_halted = true;
+    }
+    return controller_interface::return_type::OK;
  }


Do we need this? The controller update cycle only happens in the active state

This was in the old update() function and I kept it in because without it I failed tests (eg. "cleanup" in test_diff_drive_controller.cpp) that were calling update after deactivation. Whether or not to keep it depends on your philosophy: how much do you trust the external controller manager to not call update after deactivating the node?

It has to work that way AFAIK
https://github.com/ros-controls/ros2_control/blob/00c7088394773a61a8e6a9daf614591e31a2b55d/controller_manager/src/controller_manager.cpp#L2438

It first checks if it is active and then runs the update callback

If there are tests linked to it. Maybe we should handle this in a different PR. What's your opinion @ros-controls/ros2-maintainers ?

diff_drive_controller/src/diff_drive_controller.cpp

saikishor · 2025-01-14T07:17:34Z

diff_drive_controller/src/diff_drive_controller.cpp

+      if (
+        cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) ||
+        current_time_diff < cmd_vel_timeout_)
+      {
+        received_velocity_msg_ptr_.writeFromNonRT(msg);
+      }
+      else
+      {
+        RCLCPP_WARN(
+          get_node()->get_logger(),
+          "Ignoring the received message (timestamp %.10f) because it is older than "
+          "the current time by %.10f seconds, which exceeds the allowed timeout (%.4f)",
+          rclcpp::Time(msg->header.stamp).seconds(), current_time_diff.seconds(),
+          cmd_vel_timeout_.seconds());
+      }


This logic won't work if the header contains zeros
if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0))

You should modify the logic to be able to work with it

I am not sure what you mean, perhaps I am misunderstanding. I just added a test demonstrating that it works the way I expect it to. Let me know if my expectation is not correct.

saikishor · 2025-01-14T07:19:52Z

diff_drive_controller/src/diff_drive_controller.cpp

@@ -498,6 +571,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
 controller_interface::CallbackReturn DiffDriveController::on_activate(
  const rclcpp_lifecycle::State &)
 {
+  // Set default value in command
+  reset_controller_reference_msg(*(received_velocity_msg_ptr_.readFromRT()), get_node());


Personally, I don't like the idea of changing information of what is inside the buffer. The buffers should only be used to get but not modify it simultaneously.

Agreed. I'm trying to think of a good way to rework the reset_controller_reference_msg. In any case, I think any resetting should be done in deactivate().

diff_drive_controller/src/diff_drive_controller.cpp

saikishor · 2025-01-14T07:22:53Z

diff_drive_controller/src/diff_drive_controller.cpp

+void reset_controller_reference_msg(
+  const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg,
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
+{
+  msg->header.stamp = node->now();
+  msg->twist.linear.x = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.linear.y = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.linear.z = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.angular.x = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.angular.y = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
+}


Do we need to set the the velocities to NaN especially?, why not simply zero? This way you can just overload with the default constructor one.

If there is a specific reason, do you mind explaining here?

I copied this from last year's chainable-diff-drive branch. I think there is probably a better way to do this (using the default constructor), but I need to think it through. What we want to capture is that there is a difference between reading a 0.0 (which would have been intentionally published to stop the robot) vs reading a NaN (which indicates that the controller simply hasn't received any command). We need to put that value (Nan or 0) into the received_velocity_msg_ptr_ RealtimeBuffer on initialization because we don't want to be pulling undefined values from the buffer.

I moved the reset_controller_reference_msg function into a private reset_buffers() member function. reset_buffers() is called in reset(), which is used by on_configure() and on_cleanup(). on_deactivate() just uses reset_buffers().

saikishor · 2025-01-14T07:29:51Z

diff_drive_controller/src/diff_drive_controller.cpp

+  previous_two_commands_.push({{0.0, 0.0}});  // needs zeros (not NaN) to catch early accelerations
+  previous_two_commands_.push({{0.0, 0.0}});


I've a question, do we need to also reset this to zeros in the on_activate?
I'm not sure what would happen if you are deactivating and activating the controller and it might have the previous values still right?. May be I'm wrong here

I think you are correct that if you deactivate and then activate then the previous_two_commands and reference_interfaces will keep the values that they had. However, in my mind these should be reset in deactivate(), and activate() should be able to assume that it is activating out of the default configured but inactive state.

I removed the reset of the queue from on_activate() and moved it to the on_deactivate()

small fixes: use isfinite not isnan Co-authored-by: Sai Kishor Kothakota <[email protected]>

christophfroehlich · 2025-01-14T21:31:45Z

Is there a way that I can fix the Rolling ABI compatibility check? It seems to be broken because of the change from ControllerInterface to ChainableControllerInterface and I see no way around that.

This change truly is ABI breaking, but this is no show-stopper for this PR. Furthermore, for controllers this is not really an issue because we load them as plugins dynamically.

christophfroehlich

Can you please fix the merge conflict after #1478?

arthurlovekin · 2025-01-15T00:10:36Z

@christophfroehlich the merge conflict should be resolved

arthurlovekin · 2025-01-15T00:40:01Z

One thing that occurred to me as I was writing the reset_buffer() code: If you want to switch from one controller to another, is it desirable for this controller to halt() on_deactivate()? Perhaps I misunderstand the controller_manager because it seems like you would want to be able to switch without stopping the motors. If you switch into the diff_drive_controller from another one it also seems like there could be issues with the acceleration limiter, since from the diff_drive_controller's perspective its previous_two_commands have been set to zero, but you may be switching at a time when the other controller is commanding a high value. (but if you don't set the previous_two_commands to zero then a single starting command at the beginning will be able to bypass the acceleration limits, which can only be applied if multiple commands have been sent).

christophfroehlich

Please fix the pre-commit (cpplint) error. (consider installing pre-commit to avoid this the next time)

diff_drive_controller/src/diff_drive_controller.cpp

github-actions bot requested review from aprotyas, ARK3r, bmagyar, destogl, mamueluth, Raivias and rosterloh January 10, 2025 06:33

arthurlovekin force-pushed the diff-drive-chainable-1457 branch from 19b7630 to a353628 Compare January 11, 2025 01:37

christophfroehlich requested changes Jan 11, 2025

View reviewed changes

diff_drive_controller is chainable

52ee68a

with chained mode tests + export reference interfaces test

arthurlovekin force-pushed the diff-drive-chainable-1457 branch from a353628 to 52ee68a Compare January 12, 2025 02:14

christophfroehlich changed the title ~~Make diff_drive_controller a ChainableControllerInterface (fixes #1457)~~ Make diff_drive_controller a ChainableControllerInterface Jan 12, 2025

christophfroehlich linked an issue Jan 12, 2025 that may be closed by this pull request

Convert the diff_drive_controller into a ChainableControllerInterface in the master branch #1457

Open

christophfroehlich previously approved these changes Jan 12, 2025

View reviewed changes

saikishor requested changes Jan 14, 2025

View reviewed changes

Apply suggestions from code review

ff3ceeb

small fixes: use isfinite not isnan Co-authored-by: Sai Kishor Kothakota <[email protected]>

arthurlovekin dismissed christophfroehlich’s stale review via ff3ceeb January 14, 2025 17:37

arthurlovekin added 3 commits January 14, 2025 12:30

reset_buffers()

4fb9bf9

formatting

5e1470b

deactivate_then_activate test

a8a45fd

christophfroehlich requested changes Jan 14, 2025

View reviewed changes

arthurlovekin added 3 commits January 14, 2025 15:49

Added test command_with_zero_timestamp_is_accepted_with_warning

f27a723

accidentally activated twice

fe063fe

Merge branch 'master' into diff-drive-chainable-1457

b677091

arthurlovekin requested review from saikishor and christophfroehlich January 15, 2025 00:26

christophfroehlich requested changes Jan 15, 2025

View reviewed changes

Fix linting "Using C-style cast" error

6f3c7a5

christophfroehlich reviewed Jan 15, 2025

View reviewed changes

diff_drive_controller/src/diff_drive_controller.cpp Outdated Show resolved Hide resolved

og fix of linting typecast error

794b77c

christophfroehlich approved these changes Jan 15, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Make diff_drive_controller a ChainableControllerInterface #1485

Make diff_drive_controller a ChainableControllerInterface #1485

arthurlovekin commented Jan 10, 2025

christophfroehlich commented Jan 10, 2025

arthurlovekin commented Jan 11, 2025

codecov bot commented Jan 11, 2025 •

edited

Loading

christophfroehlich left a comment

arthurlovekin commented Jan 11, 2025

arthurlovekin commented Jan 12, 2025

christophfroehlich left a comment

arthurlovekin commented Jan 12, 2025

christophfroehlich commented Jan 12, 2025

mergify bot commented Jan 13, 2025

arthurlovekin commented Jan 13, 2025

olivier-stasse commented Jan 14, 2025 •

edited

Loading

saikishor left a comment

saikishor Jan 12, 2025

arthurlovekin Jan 14, 2025

saikishor Jan 14, 2025

saikishor Jan 14, 2025

saikishor Jan 14, 2025

arthurlovekin Jan 14, 2025

saikishor Jan 14, 2025

arthurlovekin Jan 14, 2025

saikishor Jan 14, 2025

arthurlovekin Jan 14, 2025

arthurlovekin Jan 14, 2025

saikishor Jan 14, 2025

arthurlovekin Jan 14, 2025

arthurlovekin Jan 14, 2025

christophfroehlich commented Jan 14, 2025

christophfroehlich left a comment

arthurlovekin commented Jan 15, 2025

arthurlovekin commented Jan 15, 2025

christophfroehlich left a comment

		previous_two_commands_.push({{0.0, 0.0}}); // needs zeros (not NaN) to catch early accelerations
		previous_two_commands_.push({{0.0, 0.0}});

Make diff_drive_controller a ChainableControllerInterface #1485

Are you sure you want to change the base?

Make diff_drive_controller a ChainableControllerInterface #1485

Conversation

arthurlovekin commented Jan 10, 2025

christophfroehlich commented Jan 10, 2025

arthurlovekin commented Jan 11, 2025

codecov bot commented Jan 11, 2025 • edited Loading

Codecov Report

christophfroehlich left a comment

Choose a reason for hiding this comment

arthurlovekin commented Jan 11, 2025

arthurlovekin commented Jan 12, 2025

christophfroehlich left a comment

Choose a reason for hiding this comment

arthurlovekin commented Jan 12, 2025

christophfroehlich commented Jan 12, 2025

mergify bot commented Jan 13, 2025

arthurlovekin commented Jan 13, 2025

olivier-stasse commented Jan 14, 2025 • edited Loading

saikishor left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

christophfroehlich commented Jan 14, 2025

christophfroehlich left a comment

Choose a reason for hiding this comment

arthurlovekin commented Jan 15, 2025

arthurlovekin commented Jan 15, 2025

christophfroehlich left a comment

Choose a reason for hiding this comment

codecov bot commented Jan 11, 2025 •

edited

Loading

olivier-stasse commented Jan 14, 2025 •

edited

Loading