Async Function Handler for Controllers #1489

saikishor · 2024-04-18T13:56:28Z

Hello!

In this PR, I've added a specific class AsyncFunctionHandler which basically will help us run the update method in the case of the controller manager and read/write method in the case of hardware components asynchronously. This PR takes a bit different approach compared to PR : #1041. The idea behind both approaches is the same. The key objectives of this approach include:

The controller manager functions solely as a scheduler, with each controller/hardware component responsible for managing its own threads.
Enhancing modularity by enabling the reuse of the same class and logic across controllers and hardware components.
Ensuring proper isolation for testing of the asynchronous handling aspect.
This might break ABI but is backward compatible.

I would like to hear your opinion on this approach. @VX792 It would be great if you could take a look at this approach and comment with your opinion.

Thank you!

Needs: ros-controls/realtime_tools#168
Closes #1469

VX792

I think the concept is pretty cool, especially because this is what I also think would be intuitive from the user's point of view.
For the technical side I left a few comments / questions for a few things that are not yet clear to me, but I think the overall approach is good. The only thing I'm not entirely convinced about is the synchronization part, because I think it would make the most sense inside the handles.

As an example, instead of triggering the entire async controller update, it would be better to trigger only the set_ and get_value() operations that the update executes; that way we could block for a very minimal amount of time.

Also, if we're going with this implementation, then feel free to delete the unused ControllerThreadWrapper and the other leftover stuff from the controller manager's header too.

VX792 · 2024-04-18T21:05:38Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ async_update_stop_ = false;
+ trigger_in_progress_ = false;
+ async_update_return_ = T();
+ thread_ = std::thread(


This is very minor, but here we are spawning a thread in the first CM update, which is relatively expensive. Is it possible to create this thread in the on_activate() function, and only do some work when the state changes to active, and sleep otherwise?

For most use cases it shouldn't matter, but if we have 3-4 async controllers combined with a high update rate then it could mess up the timing in the first iteration.

I agree, I will see what I can do to do with at the activate cycle

VX792 · 2024-04-18T21:09:48Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ {
+ break;
+ }
+ async_update_return_ = async_function_(current_update_time_, current_update_period_);


So this is the issue I wasn't able to solve without rewriting the handles. Do you have an idea on how we could make this thread safe?

This calls the controller update(), and if it coincides with the CM thread's write or read, then this is a data race, as both of those write to the same memory through a double ptr.

With the current version, I agree we will have this issue. WIth #1240 we might be able to avoid this issue with the getter and setters that it is going to provide

Actually I'm not sure even that will solve it, because if you take this code from the JTC for example:

for (size_t index = 0; index < dof_; ++index) { trajectory_point_interface[index] = joint_interface[index].get().get_value(); // get_state() from #1240 }

and you're in the async update cycle, while the HWIF from the CM thread does

for (size_t index = 0; index < dof_; ++index) { hw_states_[index] = some_value // set_state() with the updated version }

at the same time, then it doesn't matter if the access itself is thread safe, the async JTC will do the assignment and calculations based on a mixture of old and new state interfaces.

For async HW, maybe it could be possible to disable plain getters / setters and instead use a grouped set_states and get_states function? That way you could wait until all values are set by the JTC, and update the states only after that! (e.g. lock until all states are read, or all states are set)

As a quick example something like this could easily be extended for multiple types:

std::map<const char*, double> interface_name_and_value = { {"Chocolate", 12.0}, {"Coffee", 14.0}, {"Cake", 15.0}}; template<typename T> struct my_pair { my_pair() = delete; my_pair(const char* first, T second) : first_(first), second_(second) {} const char* first_; T second_; }; template <typename T, typename ...Pairs> void fun(const my_pair<T>& first_pair, const Pairs&... pairs) { interface_name_and_value[first_pair.first_] = first_pair.second_; if constexpr (sizeof...(pairs) > 0) { fun(pairs...); } } // Usage: // const my_pair p1 = {"Chocolate", 18.0}; // const my_pair p2 = {"Cake", 15.2}; // const my_pair p3 = {"Coffee", 198.2}; // lock(); // fun(p1, p2, p3); // unlock()

Plain folds are not be enough if the data is stored in some data structure though, but I think this could work with index sequences / recursive templates.

What do you think about this? The code might not be very readable, but I think this could ensure data consistency later.

Regarding this, I agree, that there might be cases where the data might not be perfectly synchronized, this is the case we are more or less talking about in the other thread. For now, we can print the warning message as you mentioned and we shall try to fix it in future versions.

The batch reading and updating is a good idea, but I don't know if the Maintainers allow that level of changes at this point before Jazzy, we can try to do something like that in the future, so we could have some data consistency.

What do you say?

controller_interface/include/controller_interface/async_function_handler.hpp

VX792 · 2024-04-18T21:56:44Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ /**
+ * If the async method is running, it will wait for the current async method call to finish.
+ */
+ void wait_for_trigger_cycle_to_finish()


Is this a placeholder? I haven't seen this fn called anywhere other than the tests.

Well it is for testing purpose, but if can be used alongside before stopping the async thread. If I use this at the beginning of the stop_async_update, may be we won't be having the situation you were describing

VX792 · 2024-04-18T22:20:35Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ * If the async update method is still running, it will return the last return value of the async
+ * function.
+ *
+ * \note In the case of controllers, The controller manager is responsible


Instead of having the controller manager act as the scheduler, would it make sense to trigger the async updates based on whether the hardware component whose command interfaces the controller is using has any data ready?

In the latter case, it might be possible to make async HW and controllers work together!

As an example, let's say I'd like to use an async controller combined with a KUKA-specific external interface, where the timing is always handled inside the internal motion stack instead of the CM, and therefore would work better as an async HW component.

I'm not yet sure if there's a legit use case where this would make sense, I think it's just interesting to think about if the design we choose should permit the combination of the two or not.

This design process would be cool. I'm not sure about triggering only when the data is ready.

For instance, having a chainable controller A -> B -> HW. A is async and running at a lower rate like 200 Hz and controller B is async and running at 500 Hz closing the loop locally with the actual values (So, we will need to trigger the controller B anyway as it needs to close the loop with the same reference values) and then command to the HW on each loop, if the HW is async then it gets the references on it's cycle (Some hardware like elmo respectively they expect a reference sent constantly at a certain frequency, in that case this mechanism would also be a problem right?

Right now, I couldn't think of a way to make both HW and controllers work together for both cases. It would be really cool as we have a data dependency

What do you think?

I see, that makes sense.

Also it's a difficult question because afaik there is no definition on what the behaviour of an async HW and async controller combination should be, so maybe at least a warning could be given if the user tries to use this combination, because otherwise there's no information if a certain configuration is invalid or not.

What do you think about that?

I think it is always nice to warn the user about the data synchronization issue between async controllers and async hardware combination. Where do you think is the right place to do it?. The resource manager at the time of configuring the hardware can print this message of possible discrepancies.

Also in most of the case, the controller manager runs at almost same frequency as the hardware even though it is run async, because mostly it doesn't make sense to compute the same command twice and being unable to send to the hardware.

VX792 · 2024-04-18T22:22:04Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ async_update_condition_.notify_one();
+ trigger_status = true;
+ }
+ const T return_value = async_update_return_;


Do you think std::future would be more idiomatic here, instead of a condition variable? As I see we're doing some calculations in the async thread, and returning it when it's ready. I think we'd also have one less flag to keep track of.

I initially thought of using future, but if there is a case that the update takes time and it doesn't have it's current update cycle's return state, then I think it makes sense to continue with its old return state without triggering another cycle, rather than waiting for the future result.

If this above situation occurs, then the controller manager will print a missing update cycle message, so the user knows that he has to lower the frequency of his controller as the computation is taking longer than what he initially estimated

That's fair.

Maybe it's possible to compute a timeout using the time & period for the future's wait_for() / wait_until() fn but probably it's needless complexity.

Yeah, I thought so, but I rather prefer that the conditional variable is not awake after a certain duration, because that's exactly what we want to avoid with proper triggering

saikishor

@VX792 Thank you for the extensive review. I've answered the comments

The only thing I'm not entirely convinced about is the synchronization part, because I think it would make the most sense inside the handles.

I'm expecting to achieve some sort of synchronization with #1240 and also triggering this way, if we have a chainable controller, the async will be triggered in the same order, so some sort of synchronization would be achieved better than the current one

As an example, instead of triggering the entire async controller update, it would be better to trigger only the set_ and get_value() operations that the update executes; that way we could block for a very minimal amount of time.

How do we deal with the cases, where the update cycle of a controller take longer?. For instance, the modern techniques like Model Predictive Control, Whole Body Control etc runs some optimization in the update cycle and this time entirely depends on the objective or constraints they are solving along with the degrees of freedom of the robot

Also, if we're going with this implementation, then feel free to delete the unused ControllerThreadWrapper and the other leftover stuff from the controller manager's header too.

Okay, first I wanted to get the feedback if this approach is interesting for the community or not and in a different PR I could remove them so we have less changes in the PR to review

controller_interface/include/controller_interface/async_function_handler.hpp

saikishor · 2024-04-19T21:59:25Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ async_update_stop_ = false;
+ trigger_in_progress_ = false;
+ async_update_return_ = T();
+ thread_ = std::thread(


I agree, I will see what I can do to do with at the activate cycle

saikishor · 2024-04-19T22:01:12Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ {
+ break;
+ }
+ async_update_return_ = async_function_(current_update_time_, current_update_period_);


With the current version, I agree we will have this issue. WIth #1240 we might be able to avoid this issue with the getter and setters that it is going to provide

saikishor · 2024-04-19T22:02:43Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ /**
+ * If the async method is running, it will wait for the current async method call to finish.
+ */
+ void wait_for_trigger_cycle_to_finish()


Well it is for testing purpose, but if can be used alongside before stopping the async thread. If I use this at the beginning of the stop_async_update, may be we won't be having the situation you were describing

saikishor · 2024-04-20T07:34:33Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ async_update_condition_.notify_one();
+ trigger_status = true;
+ }
+ const T return_value = async_update_return_;


I initially thought of using future, but if there is a case that the update takes time and it doesn't have it's current update cycle's return state, then I think it makes sense to continue with its old return state without triggering another cycle, rather than waiting for the future result.

If this above situation occurs, then the controller manager will print a missing update cycle message, so the user knows that he has to lower the frequency of his controller as the computation is taking longer than what he initially estimated

saikishor · 2024-04-20T08:01:35Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ * If the async update method is still running, it will return the last return value of the async
+ * function.
+ *
+ * \note In the case of controllers, The controller manager is responsible


This design process would be cool. I'm not sure about triggering only when the data is ready.

For instance, having a chainable controller A -> B -> HW. A is async and running at a lower rate like 200 Hz and controller B is async and running at 500 Hz closing the loop locally with the actual values (So, we will need to trigger the controller B anyway as it needs to close the loop with the same reference values) and then command to the HW on each loop, if the HW is async then it gets the references on it's cycle (Some hardware like elmo respectively they expect a reference sent constantly at a certain frequency, in that case this mechanism would also be a problem right?

Right now, I couldn't think of a way to make both HW and controllers work together for both cases. It would be really cool as we have a data dependency

What do you think?

codecov · 2024-04-21T22:20:01Z

Codecov Report

Attention: Patch coverage is 82.51748% with 25 lines in your changes missing coverage. Please review.

Project coverage is 87.96%. Comparing base (1ed61ef) to head (56e3229).
Report is 1 commits behind head on master.

❗ Current head 56e3229 differs from pull request most recent head 253f4b1

Please upload reports for the commit 253f4b1 to get more accurate results.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1489      +/-   ##
==========================================
+ Coverage   87.70%   87.96%   +0.26%     
==========================================
  Files         102      104       +2     
  Lines        8704     8809     +105     
  Branches      780      790      +10     
==========================================
+ Hits         7634     7749     +115     
+ Misses        790      771      -19     
- Partials      280      289       +9

Flag	Coverage Δ
unittests	`87.96% <82.51%> (+0.26%)`	⬆️

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

Files	Coverage Δ
...controller_interface/controller_interface_base.hpp	`87.50% <ø> (ø)`
...ler_interface/test/test_async_function_handler.cpp	`100.00% <100.00%> (ø)`
...ler_interface/test/test_async_function_handler.hpp	`100.00% <100.00%> (ø)`
.../include/controller_manager/controller_manager.hpp	`100.00% <ø> (ø)`
controller_manager/src/controller_manager.cpp	`75.21% <57.14%> (+0.65%)`	⬆️
...roller_interface/src/controller_interface_base.cpp	`74.24% <41.17%> (-12.04%)`	⬇️
...de/controller_interface/async_function_handler.hpp	`78.18% <78.18%> (ø)`

VX792 · 2024-04-22T13:52:17Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ async_update_condition_.notify_one();
+ trigger_status = true;
+ }
+ const T return_value = async_update_return_;


That's fair.

Maybe it's possible to compute a timeout using the time & period for the future's wait_for() / wait_until() fn but probably it's needless complexity.

VX792 · 2024-04-22T13:56:02Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ * If the async update method is still running, it will return the last return value of the async
+ * function.
+ *
+ * \note In the case of controllers, The controller manager is responsible


I see, that makes sense.

Also it's a difficult question because afaik there is no definition on what the behaviour of an async HW and async controller combination should be, so maybe at least a warning could be given if the user tries to use this combination, because otherwise there's no information if a certain configuration is invalid or not.

What do you think about that?

controller_interface/include/controller_interface/async_function_handler.hpp

VX792 · 2024-04-22T18:59:49Z

controller_interface/include/controller_interface/async_function_handler.hpp

+ {
+ break;
+ }
+ async_update_return_ = async_function_(current_update_time_, current_update_period_);


Actually I'm not sure even that will solve it, because if you take this code from the JTC for example:

for (size_t index = 0; index < dof_; ++index) { trajectory_point_interface[index] = joint_interface[index].get().get_value(); // get_state() from #1240 }

and you're in the async update cycle, while the HWIF from the CM thread does

for (size_t index = 0; index < dof_; ++index) { hw_states_[index] = some_value // set_state() with the updated version }

at the same time, then it doesn't matter if the access itself is thread safe, the async JTC will do the assignment and calculations based on a mixture of old and new state interfaces.

For async HW, maybe it could be possible to disable plain getters / setters and instead use a grouped set_states and get_states function? That way you could wait until all values are set by the JTC, and update the states only after that! (e.g. lock until all states are read, or all states are set)

As a quick example something like this could easily be extended for multiple types:

std::map<const char*, double> interface_name_and_value = { {"Chocolate", 12.0}, {"Coffee", 14.0}, {"Cake", 15.0}}; template<typename T> struct my_pair { my_pair() = delete; my_pair(const char* first, T second) : first_(first), second_(second) {} const char* first_; T second_; }; template <typename T, typename ...Pairs> void fun(const my_pair<T>& first_pair, const Pairs&... pairs) { interface_name_and_value[first_pair.first_] = first_pair.second_; if constexpr (sizeof...(pairs) > 0) { fun(pairs...); } } // Usage: // const my_pair p1 = {"Chocolate", 18.0}; // const my_pair p2 = {"Cake", 15.2}; // const my_pair p3 = {"Coffee", 198.2}; // lock(); // fun(p1, p2, p3); // unlock()

Plain folds are not be enough if the data is stored in some data structure though, but I think this could work with index sequences / recursive templates.

What do you think about this? The code might not be very readable, but I think this could ensure data consistency later.

controller_interface/include/controller_interface/async_function_handler.hpp

saikishor · 2024-04-24T07:34:10Z

@VX792 Will you be in today's WG meeting?

VX792 · 2024-04-24T12:03:27Z

@VX792 Will you be in today's WG meeting?

I'm not sure yet,but I think I can make it.

saikishor · 2024-04-24T12:26:02Z

I'm not sure yet, but I think I can make it.

@VX792 Sure. It's just so we can discuss this or the async topic in general. Thank you :)

…purious wakeups

…cycle

Co-authored-by: Jordan Palacios <[email protected]>

Co-authored-by: atzaros <[email protected]>

…ng notify

… between threads

Co-authored-by: atzaros <[email protected]>

…FunctionHandler

…rom it

github-actions bot requested review from bmagyar, DasRoteSkelett, destogl, erickisos, jaron-l, olivier-stasse and peterdavidfagan April 18, 2024 13:56

saikishor added this to the Pre-Jazzy (Feb.'24) milestone Apr 18, 2024

VX792 reviewed Apr 18, 2024

View reviewed changes

saikishor commented Apr 20, 2024

View reviewed changes

VX792 reviewed Apr 22, 2024

View reviewed changes

saikishor added 15 commits May 5, 2024 11:21

Add first version of the async controllers

0cf96cb

removed extra notify one

cfad3d3

added async_update_ready_ to use with conditional variable to avoid s…

cd62f78

…purious wakeups

call notify_one after setting the async_update_ready

c64058f

add precommit changes

f8f9c42

add missing this in the conditional variable callback

06a575e

change atomic bool to normal bool

3637274

add wait_for_update_to_finish method

7ab1b7e

add notify_one in different places to wait properly to finish update …

3435636

…cycle

added unique lock and check with try_lock for triggering calls

b6a3a95

use try_to_lock for better checking of owning the lock or not

47b4e11

add current update time and period to be able to update them

473940e

update documentation of the async method

b232349

added some comments about the issue with the get_state method

ebc1e33

use atomic_bool

f6d043a

saikishor and others added 15 commits May 5, 2024 11:23

rename is_preempted to is_stopped for the context of the threads

3b763a3

change EXPECT_THROW to ASSERT_THROW

ae0b49a

Update the documentation of the init return entity

405af89

Fix the doc of stop_async_update_cycle

1e37f29

Co-authored-by: Jordan Palacios <[email protected]>

add minor information

913c9f9

add missing functional include

f0a8ca3

check if lock is owned or not before in the condition

40e663e

Co-authored-by: atzaros <[email protected]>

Use separated arguments for better clarity

40b58b1

Move the lock within a scope to avoid exclusive unlock within the thread

fc034c9

notify other waiting threads before stopping

44ee144

Scope the lock to avoid manual unlock in the trigger_async_update method

eb1e74a

Update async_update_stop_ within the scope of the lock to avoid missi…

0398d7b

…ng notify

Add new conditional variable to have unexpected behavior when working…

90985ea

… between threads

Changes for the starting thread upon activation

df8b378

remove unused notify at the end of the thread

0d04cd3

saikishor force-pushed the async_controllers branch from 40efb15 to 0d04cd3 Compare May 5, 2024 09:38

saikishor mentioned this pull request May 5, 2024

Working async controllers and components [not synchronized] #1041

Merged

VX792 approved these changes May 6, 2024

View reviewed changes

destogl and others added 7 commits May 8, 2024 19:14

Merge branch 'master' into async_controllers

c24402e

Merge branch 'master' into async_controllers

7e197cf

simplify the logic inside the thread

9d31eb4

Co-authored-by: atzaros <[email protected]>

add pre-commit formatting changes

bee14fe

remove the AsyncControllerThread integration to replace it with Async…

1984168

…FunctionHandler

Merge branch 'master' into async_controllers

980bf8a

Merge branch 'master' into async_controllers

56e3229

saikishor mentioned this pull request Jun 11, 2024

Add Async Function Handler ros-controls/realtime_tools#168

Open

saikishor added 3 commits June 12, 2024 13:36

move the async function handler to the realtime_tools and integrate f…

4474281

…rom it

start the async thread when configuring the controller

52fb844

remove the get_state bind for API change

253f4b1

saikishor changed the title ~~Async Function Handler for Controllers and Hardware Components~~ Async Function Handler for Controllers Jun 13, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Async Function Handler for Controllers #1489

Async Function Handler for Controllers #1489

saikishor commented Apr 18, 2024 •

edited

Loading

VX792 left a comment

VX792 Apr 18, 2024

saikishor Apr 19, 2024

saikishor Apr 22, 2024

VX792 Apr 18, 2024

saikishor Apr 19, 2024

VX792 Apr 22, 2024

saikishor Apr 22, 2024

VX792 Apr 18, 2024

saikishor Apr 19, 2024

VX792 Apr 18, 2024

saikishor Apr 20, 2024

VX792 Apr 22, 2024

saikishor Apr 22, 2024

saikishor Apr 22, 2024

VX792 Apr 18, 2024

saikishor Apr 20, 2024

VX792 Apr 22, 2024 •

edited

Loading

saikishor Apr 22, 2024

saikishor left a comment

saikishor Apr 19, 2024

saikishor Apr 19, 2024

saikishor Apr 19, 2024

saikishor Apr 20, 2024

saikishor Apr 20, 2024

codecov bot commented Apr 21, 2024 •

edited

Loading

VX792 Apr 22, 2024 •

edited

Loading

VX792 Apr 22, 2024

VX792 Apr 22, 2024

saikishor commented Apr 24, 2024

VX792 commented Apr 24, 2024

saikishor commented Apr 24, 2024

Async Function Handler for Controllers #1489

Are you sure you want to change the base?

Async Function Handler for Controllers #1489

Conversation

saikishor commented Apr 18, 2024 • edited Loading

VX792 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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

VX792 Apr 22, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

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

codecov bot commented Apr 21, 2024 • edited Loading

Codecov Report

VX792 Apr 22, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

saikishor commented Apr 24, 2024

VX792 commented Apr 24, 2024

saikishor commented Apr 24, 2024

saikishor commented Apr 18, 2024 •

edited

Loading

VX792 Apr 22, 2024 •

edited

Loading

codecov bot commented Apr 21, 2024 •

edited

Loading

VX792 Apr 22, 2024 •

edited

Loading