ControlLoop for controller manager #728

tylerjw · 2022-06-02T06:49:38Z

Signed-off-by: Tyler Weaver tylerjw@gmail.com

Fixes #726

tylerjw · 2022-06-02T06:56:30Z

Please refer to the diagram and discussion in the related issue for reference to understanding this change.

controller_manager/src/ros2_control_node.cpp

nbbrooks

I looked through the tests and made some suggestions around naming.

controller_manager/test/test_control_loop.cpp

mergify · 2022-06-08T08:19:16Z

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

destogl

We actually never planned for this control loop to grow so much, but this makes sense for sure! Thanks for the good explanations and for adding docs diagrams. Could you put those into the docs folder? And reference it so it does not get lost.

In general, I find the changes too complex only for the sake of testing. Is it possible to do this simpler?

controller_manager/include/controller_manager/control_loop.hpp

destogl · 2022-06-08T10:33:35Z

controller_manager/include/controller_manager/control_loop.hpp

+ */
+template <
+  typename NowInvocable, typename OkInvocable, typename WorkInvocable, typename SleepInvocable>
+void ControlLoop(


TBH, I am not certain that this code deserves it own file. Why not add it as a method into the ".cpp" file. This should also be testable then.

Furthermore, do we really need methods as arguments? IMHO, for this simple functionality this looks very overengineered.

It would not be testable if put in the .cpp file because I need to create a new version of it in the translation unit that is the test. This is the reason that templates always have to go in a header file if you want to use them in two separate translation units.

I added static_assert statements to this to make the compile errors nicer if a user tries to use this function and makes a mistake in the sort of function-like objects they pass into this.

Furthermore, do we really need methods as arguments? IMHO, for this simple functionality this looks very overengineered.

Do you have a suggestion for how to make this behavior testable without methods as arguments? IMHO, code that is not designed to be tested is almost always broken.

code that is not designed to be tested is almost always broken.

bit heavy-handed, isn't it?

I agree testability is good, but you cannot generalise like you're doing here. It isn't very constructive.

I'm sorry for being sarcastic and mean here. Please let me know what feels overly complex or over engineered. I'm happy to make changes or add documentation to make this code more readable or better in any way.

There is no need to apologise Tyler.

Obviously introducing tests is better than not doing it, and adapting code structure to allow for testing is better than not doing it.

There are different ways of testing though, and unit tests are only one of them. Not all of them require dependency injection or mocking.

I tried testing this loop externally with an integration test and was unable to figure out how to do it robustly or simply. My current robot driver I'm working on is effectively a test of this but I do think it is way too much code for a test.

You're misunderstanding me: I'm not telling you this is not a good approach, or you should use a different testing strategy.

I'm saying asserting that only "testable/tested code is fault-free" is not a claim you can make, and that using a different approach than dependency injection/mocking could also be used in some cases.

I'm not saying in this case.

"testable/tested code is fault-free" is an exaggeration for sure. As a person who makes a lot of mistakes, and finds writing code that does what I want hard, I find writing tests useful.

Theoretically, tests also help you in the future when you come back to something and want to make a change but not break what already exists.

controller_manager/src/ros2_control_node.cpp

destogl · 2022-06-08T10:34:50Z

controller_manager/src/ros2_control_node.cpp

-      // wait until we hit the end of the period
-      end_period += period;
-      std::this_thread::sleep_for(std::chrono::nanoseconds((end_period - cm->now()).nanoseconds()));
+    auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());


Very nice readability detail! Thank you!

destogl · 2022-06-08T10:39:37Z

controller_manager/src/ros2_control_node.cpp

+        std::chrono::system_clock::time_point(std::chrono::nanoseconds(time.nanoseconds())));
+    };
+    auto const ok = []() { return rclcpp::ok(); };
+    auto const do_work = [&cm](rclcpp::Time current_time, rclcpp::Duration period) {


Although better, I would also not consider this 100% correct. If we do changes here, we should then measure times separately between, “read”, “update” and “write” calls. Maybe calculating different periods is not necessary (but good to have), but we should have at least “current_time_read”, “current_time_update”, “current_time_write”.

You could add these separately inside the controller manager by having each of these functions take the time when they are called if you need that for something. Do you already have tooling or reasoning for wanting to expose those timing values?

Let us not add things because they sound good to have but we don't actually have a known use for them yet. This will just make the code have confusing extra variables. I would argue based on the response I got in the WG meeting when asking about the justification for the new time and period parameters passed to read/write that the change to add those is lacking a clear use-case.

destogl · 2022-06-08T12:08:37Z

controller_manager/src/ros2_control_node.cpp

+    };
+    auto const ok = []() { return rclcpp::ok(); };
+    auto const do_work = [&cm](rclcpp::Time current_time, rclcpp::Duration period) {
+      // Write is called first as the consistent rate of writing to hardware


I am uncertain if this is, in general, a good idea. This will cause first “write” always with wrong values. I would leave this as it is for now, but we should cover probably the three cases in the future:

the standard one when hardware read-update-write are executed in sequence

with sleep before write

without sleep where communication with the robot dictates the rate.

This will cause first “write” always with wrong values.

In my experience this is already a problem, this won't change the current behavior where write gets called with invalid values. At least on Galactic with my current driver, I have had to write code in the system interface to guard against this with the current implementation. Nothing about this change fixes that part of the broken behavior.

tylerjw · 2022-06-08T15:12:48Z

In general, I find the changes too complex only for the sake of testing. Is it possible to do this simpler?

This is taking a generic programming approach to solving this problem. I generally think correctness and robust testing to be much more valuable than reducing the number of things the programmer has to learn to read the code.

I can find you references if you'd like for why I am using template arguments over using std::function or something similar. The short answer is that template arguments here will generate much more performant code because std::function has to do heap allocations to enable its copy constructor and allow you to keep collections of them.

I do think this function is complex enough and separate enough from the main function to justify it being in its own file. If this was trivially easy to write in a correct way why was the main loop itself so broken? In my experience, code that is not designed to be tested is almost always trivially broken because it is too hard to program without errors.

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

Co-authored-by: G.A. vd. Hoorn <g.a.vanderhoorn@tudelft.nl> Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

Co-authored-by: Nathan Brooks <nbbrooks@gmail.com>

Signed-off-by: Tyler Weaver <tyler@picknik.ai>

Co-authored-by: Denis Štogl <denis@stogl.de>

Signed-off-by: Tyler Weaver <tyler@picknik.ai>

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

tylerjw · 2022-06-12T17:33:24Z

Could you put those into the docs folder? And reference it so it does not get lost.

I added an explanation of the control loop to the userdoc.rst and included the diagram.

In general, I find the changes too complex only for the sake of testing. Is it possible to do this simpler?

Is there something specific you think is too complex? I do not understand what about this change is more complex than what is normal in this project.

tylerjw · 2022-06-12T17:57:48Z

If you are curious about why I chose to use template arguments for the function parameters here are a couple of links. The basic reason is that because I don't need to store the functions in a runtime collection I use templates. In general, you should always use templates when passing functions if you can because if the type is known at compile time your compiler can do a much better job of optimizing it. If you are curious how std::function works and allows us to store function objects at runtime without inheritance, it uses a technique called type-errasure. The downside to type-erasure is that it has to do a heap allocation and does use virtual function dispatch via inheritance of an internal model template. This trick is a way of having a duck-typed interface in C++ without inheritance in your interface.

The best explanation of type-errasure I know of is this Sean Parent talk:

https://www.youtube.com/watch?v=bIhUE5uUFOA

I also just updated the function to have static_asserts that will cause compilation failures when the invocable template arguments have the wrong function interface. This should improve the compile errors generated by this code if a user creates their ros2_control_node and makes a mistake when creating the various functions used by the ControlLoop function.

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

…face Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

mergify · 2022-06-27T22:17:21Z

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

mergify · 2022-07-07T20:49:03Z

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

mergify · 2022-09-24T06:44:30Z

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

mergify · 2023-03-20T11:40:56Z

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

tylerjw · 2023-06-15T23:24:11Z

Closing as this is a dead PR.

github-actions bot requested review from bmagyar, destogl, harderthan, kellyprankin, livanov93, Serafadam and UsamaHamayun1 June 2, 2022 06:49

tylerjw force-pushed the 726-control-loop branch 2 times, most recently from ac4d425 to e129d3f Compare June 2, 2022 06:55

tylerjw force-pushed the 726-control-loop branch 2 times, most recently from 85bf7ab to 6afb3eb Compare June 2, 2022 07:07

gavanderhoorn reviewed Jun 2, 2022

View reviewed changes

controller_manager/src/ros2_control_node.cpp Outdated Show resolved Hide resolved

tylerjw force-pushed the 726-control-loop branch 2 times, most recently from 0910a39 to a19eb9e Compare June 2, 2022 07:35

tylerjw mentioned this pull request Jun 2, 2022

Logic in ros2_control_node for timing #726

Open

nbbrooks reviewed Jun 5, 2022

View reviewed changes

tylerjw force-pushed the 726-control-loop branch from 623febb to ce1cbb8 Compare June 6, 2022 17:21

destogl requested changes Jun 8, 2022

View reviewed changes

tylerjw and others added 7 commits June 12, 2022 11:16

ControlLoop for controller manager

33d2634

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

Update controller_manager/src/ros2_control_node.cpp

1a6aad6

Co-authored-by: G.A. vd. Hoorn <g.a.vanderhoorn@tudelft.nl> Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

Apply suggestions from code review

e18fd4d

Co-authored-by: Nathan Brooks <nbbrooks@gmail.com>

Tests without sleeping

3035d2b

Signed-off-by: Tyler Weaver <tyler@picknik.ai>

Update controller_manager/include/controller_manager/control_loop.hpp

fd4a9b4

Co-authored-by: Denis Štogl <denis@stogl.de>

Update to clang-format-12 and respond to feedback

ffa881f

Signed-off-by: Tyler Weaver <tyler@picknik.ai>

Add doxygen comments

aa678d0

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

tylerjw force-pushed the 726-control-loop branch from 5c05b73 to aa678d0 Compare June 12, 2022 17:18

tylerjw force-pushed the 726-control-loop branch 2 times, most recently from 0ec8659 to 9c43043 Compare June 12, 2022 17:43

Add documentation with diagram to doc

2760308

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

tylerjw force-pushed the 726-control-loop branch from 9c43043 to 2760308 Compare June 12, 2022 18:09

Assert that the function arguments of control_loop have correct inter…

2c11be7

…face Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

tylerjw force-pushed the 726-control-loop branch from b8b87ab to 2c11be7 Compare June 12, 2022 18:31

Merge branch 'master' into 726-control-loop

9ab28ee

bmagyar mentioned this pull request Jul 16, 2022

Add doxygen comments #777

Merged

tylerjw closed this Jun 15, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ControlLoop for controller manager #728

ControlLoop for controller manager #728

tylerjw commented Jun 2, 2022 •

edited

Loading

tylerjw commented Jun 2, 2022

nbbrooks left a comment

mergify bot commented Jun 8, 2022

destogl left a comment

destogl Jun 8, 2022

tylerjw Jun 8, 2022

tylerjw Jun 12, 2022

tylerjw Jun 12, 2022

gavanderhoorn Jun 12, 2022

tylerjw Jun 12, 2022

gavanderhoorn Jun 13, 2022

tylerjw Jun 13, 2022

gavanderhoorn Jun 13, 2022

tylerjw Jun 13, 2022

destogl Jun 8, 2022

destogl Jun 8, 2022

tylerjw Jun 8, 2022

destogl Jun 8, 2022

tylerjw Jun 8, 2022 •

edited

Loading

tylerjw commented Jun 8, 2022

tylerjw commented Jun 12, 2022

tylerjw commented Jun 12, 2022 •

edited

Loading

mergify bot commented Jun 27, 2022

mergify bot commented Jul 7, 2022

mergify bot commented Sep 24, 2022

mergify bot commented Mar 20, 2023

tylerjw commented Jun 15, 2023

ControlLoop for controller manager #728

ControlLoop for controller manager #728

Conversation

tylerjw commented Jun 2, 2022 • edited Loading

tylerjw commented Jun 2, 2022

nbbrooks left a comment

Choose a reason for hiding this comment

mergify bot commented Jun 8, 2022

destogl 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

tylerjw Jun 8, 2022 • edited Loading

Choose a reason for hiding this comment

tylerjw commented Jun 8, 2022

tylerjw commented Jun 12, 2022

tylerjw commented Jun 12, 2022 • edited Loading

mergify bot commented Jun 27, 2022

mergify bot commented Jul 7, 2022

mergify bot commented Sep 24, 2022

mergify bot commented Mar 20, 2023

tylerjw commented Jun 15, 2023

tylerjw commented Jun 2, 2022 •

edited

Loading

tylerjw Jun 8, 2022 •

edited

Loading

tylerjw commented Jun 12, 2022 •

edited

Loading