Controller sorting and proper execution in a chain (Fixes #853)

[saikishor]

Hello!

This PR addresses the issue of controllers sorting in proper order(#853), in order to be able to execute them in a proper sequential order. The newly added test fails at the first instance and after doing proper controller sorting it passes.

Thank you,

[codecov-commenter]

Codecov Report

Merging #1063 (1dc67ae) into master (925f5f3) will decrease coverage by 2.79%.
Report is 508 commits behind head on master.
The diff coverage is 31.53%.

❗ Your organization is not using the GitHub App Integration. As a result you may experience degraded service beginning May 15th. Please install the Github App Integration for your organization. Read more.

@@            Coverage Diff             @@
##           master    #1063      +/-   ##
==========================================
- Coverage   34.61%   31.83%   -2.79%     
==========================================
  Files          52       94      +42     
  Lines        2981    10475    +7494     
  Branches     1855     7130    +5275     
==========================================
+ Hits         1032     3335    +2303     
- Misses        310      801     +491     
- Partials     1639     6339    +4700     

Flag	Coverage Δ
unittests	31.83% <31.53%> (-2.79%)	⬇️

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

Files Changed	Coverage Δ
controller_manager/src/controller_manager.cpp	38.56% <ø> (-1.15%)	⬇️
controller_manager/src/ros2_control_node.cpp	0.00% <0.00%> (ø)
..._interface/include/hardware_interface/actuator.hpp	100.00% <ø> (ø)
...ce/include/hardware_interface/async_components.hpp	0.00% <0.00%> (ø)
...re_interface/include/hardware_interface/sensor.hpp	100.00% <ø> (ø)
...re_interface/include/hardware_interface/system.hpp	100.00% <ø> (ø)
...erface/src/mock_components/fake_generic_system.cpp	100.00% <ø> (ø)
...e_interface/src/mock_components/generic_system.cpp	52.29% <ø> (ø)
hardware_interface/src/resource_manager.cpp	46.76% <ø> (-6.87%)	⬇️
hardware_interface/src/sensor.cpp	50.52% <ø> (ø)
... and 70 more

... and 20 files with indirect coverage changes

[christophfroehlich]

LGTM

[sgmurray]

Consider a 5 controller chain:

A => B => C => D => E

controller_sorting(B,C) == true
controller_sorting(C,D) == true
But, controller_sorting(B,D) == false

This is a violation of :
if comp(a, b) == true and comp(b, c) == true then comp(a, c) == true
https://en.cppreference.com/w/cpp/named_req/Compare

required of the comparison function parameter by std::sort.

[saikishor]

Consider a 5 controller chain:

A => B => C => D => E

controller_sorting(B,C) == true controller_sorting(C,D) == true But, controller_sorting(B,D) == false

This is a violation of : if comp(a, b) == true and comp(b, c) == true then comp(a, c) == true https://en.cppreference.com/w/cpp/named_req/Compare

required of the comparison function parameter by std::sort.

@sgmurray Thank you for pointing out. You were right. This wouldn't work with the current version. I've started working on the fix. I more or less have it. In the next few days, I will add the logic and will add a proper test case.

Thank you,

[saikishor]

Consider a 5 controller chain:

A => B => C => D => E

controller_sorting(B,C) == true controller_sorting(C,D) == true But, controller_sorting(B,D) == false

This is a violation of : if comp(a, b) == true and comp(b, c) == true then comp(a, c) == true https://en.cppreference.com/w/cpp/named_req/Compare

required of the comparison function parameter by std::sort.

@sgmurray Now the newly added changes should already work for the use-case that you have mentioned, but also for the complex scenario such as controller branching, as in a controller that commands 2 different controller chains. Thank you again for the comment.

Best Regards,
Sai Kishor Kothakota

[sgmurray]

This is a topological sorting problem. I don't think using std::sort is the right approach.

The set of controllers form a directed acyclic graph. Each node represents a controller and an edge from node A to node B means that A's command is B's reference.

Maybe we could create a Directed Acyclic Graph class. This class would support:

1. Adding/removing nodes
2. Adding/removing edges
3. Getting the nodes in topological order
4. Getting children/parents of a node

The controller manager would then call the Directed Acyclic Graph class every time it does something to change the graph or when it needs to know some property of the graph. This would allow us to pull the logic involving the graph out of the controller manager.

[olivier-stasse]

This is a topological sorting problem. I don't think using std::sort is the right approach.

The set of controllers form a directed acyclic graph. Each node represents a controller and an edge from node A to node B means that A's command is B's reference.

Maybe we could create a Directed Acyclic Graph class. This class would support:

1. Adding/removing nodes

2. Adding/removing edges

3. Getting the nodes in topological order

4. Getting children/parents of a node

The controller manager would then call the Directed Acyclic Graph class every time it does something to change the graph or when it needs to know some property of the graph. This would allow us to pull the logic involving the graph out of the controller manager.

I would say that this is the right approach with another caveat. A lot of controllers are using the previous value of a controller. For instance B will need the previous value of D. If you do not take into account time, this will create a cycle in the graph. The only way to break the cycle is to consider the time dependency.

[bmagyar]

Thank you all for the insightful comments on this topic!
I appreciate all your feedback & work on this and would like to keep chipping away at the problem.

With that in mind I have one question for everyone:
can this PR still be used in the shorter term (aka merging today/tomorrow)?

Of course that is only given that we can understand & fix the issues highlighted by the tests without cheating ;)
Going forward I'm fairly convinced that the suggestion of @sgmurray is the proper way.

[saikishor]

Hello @sgmurray and @olivier-stasse

Yes, I think the Directed Acyclic Graph is a very nice idea. We can consider implementing the same in the near future.

Right now, this PR achieves some level of sorting that was not existing right now with most of the cases that we face. So, do you guys agree to merge it and then work on the proper version to replace it?

[sgmurray]

"I would say that this is the right approach with another caveat. A lot of controllers are using the previous value of a controller. For instance B will need the previous value of D. If you do not take into account time, this will create a cycle in the graph. The only way to break the cycle is to consider the time dependency. "

I don't think the current pull request handles this. If B uses the previous value of D, I think this current pull request could end up inserting D before B in the list.

@olivier-stasse How could the controller manager determine that B is using a pervious value of D and that it is safe execute B before D?

This PR checks for command interface/state interface matches to determine precedence relationships. Should we also check for command interface/reference interface matches?

@saikishor
Have you tried a test with 2 independent chains?
2=>4=>6
1=>3=>5

I wrote up a quick test for std::sort and it failed.

`int main()
{
vector nums{2,6,5,1,4,3};

auto comp = [](const int& a, const int& b) {
    if (a % 2 == b % 2) {
        return a < b;
    }
    return false;
};

sort(nums.begin(), nums.end(), comp);
for (auto num : nums) {
    cout << num << " , ";
}
cout << endl;

}`

Result. Failure. 4 occurs after 6 and 3 occurs after 5.
2 , 6 , 1 , 5 , 4 , 3 ,

[christophfroehlich]

I would say that this is the right approach with another caveat. A lot of controllers are using the previous value of a controller. For instance B will need the previous value of D. If you do not take into account time, this will create a cycle in the graph. The only way to break the cycle is to consider the time dependency.

As a control engineer using Matlab/Simulink, this is a typical encountered problem called "Algebraic Loops". It happens if there is no break of the connection with a time delay, e.g., the next solver loop. If we open the framework for such algebraic loops, we easily get numerical issues in solving this problem. (how are the values propagated in the loop, are there any dynamics inside etc..).
I think we should not design for such configurations and solve that with a time delay until the next update step of the controller-manager. (B will use the value of D from the last update step).

I suggest merging this PR for the simple forward directed loops, and prohibit other loop configurations in the meantime.

[bmagyar]

Thank you!

[bmagyar]

@destogl needs more time to review it

[destogl]

Just few things to disucss.

[destogl]

Should we go with the “new” nomenclature here?

Should we first merge “estimated” interfaces and then do rename, or do the renaming first.

Suggested change

	* @note The following conditions needs to be handled while ordering the controller list
	* 1. The controllers that do not use any state or command interfaces are updated first
	* 2. The controllers that use only the state system interfaces only are updated next
	* 3. The controllers that use any of an another controller's reference interface are updated
	* before the preceding controller
	* 4. The controllers that use the controller's estimated interfaces are updated after the
	* preceding controller
	* 5. The controllers that only use the system's command interfaces are updated last
	* 6. All inactive controllers go at the end of the list
	* @note The following conditions needs to be handled while ordering the controller list
	* 1. The controllers that do not use any feedback or output interfaces are updated first
	* 2. The controllers that use only the state system interfaces only are updated next
	* 3. The controllers that use any of an another controller's reference interface are updated
	* before the preceding controller
	* 4. The controllers that use the controller's state interfaces are updated after the
	* preceding controller
	* 5. The controllers that only use the system's command interfaces are updated last
	* 6. All inactive controllers go at the end of the list

What do you mean with “system's” command interfaces? Do you mean HW command interfaces?

[destogl]

This docs should be probably moved somewhere else.

[saikishor]

Should we go with the “new” nomenclature here?

Maybe it would be easier to have the renaming to the new nomenclature everything at once. As it is not started, it might create confusion to the community.

Should we first merge “estimated” interfaces and then do rename, or do the renaming first.

I think it is better to merge the "estimated" interfaces first and then do the renaming together (or) may be we can start a renaming branch on top of the estimated interfaces branch, that would also work right?

What do you mean with “system's” command interfaces? Do you mean HW command interfaces?

Yes, you are right. I mean HW command interfaces. I will be changing it.

This docs should be probably moved somewhere else.

Where do you think would be the right place.