Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

issue 815 hardware interface add return value #933

Conversation

flochre
Copy link
Contributor

@flochre flochre commented Feb 6, 2023

In my Opinion this solve the issue #815

  • add skip flag
  • update read_write_status
  • skip controller update

Copy link
Member

@destogl destogl left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for your contribution!

Firstly, sorry for the poorly described issue. Looking more into this, I am not sure anymore what's the best solution here.

Besides that, what you are missing is actually returning “SKIPPING” from the hardware interfaces. Currently, an error is returned. To have better clarity on the issue, we should actually start implementing this proposal about command interfaces only available when active. This would lead to the following implementations:

  • Add URDF parser for those interfaces – make command interfaces being not available when inactive by default
  • Add additional write_when_active method for hardware interfaces that is called in active state.

This would give us much more perspective and idea if this additional return value is needed and how it should be implemented.

Besides that, if you are extending the range of return values, then please consider #884. To make the extension (ABI braking) into one batch.

Comment on lines +1749 to +1758
else if (failed_hardware_names.size() > 0)
{
// Status is ok but some hardware is not ok (SKIPPED)
// Determine controllers to skip
for (const auto & hardware_name : failed_hardware_names)
{
auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
controllers_to_skip.insert(controllers_to_skip.end(), controllers.begin(), controllers.end());
}
}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think this is needed here. Hardware interface is not failed. The issue here is that we are calling read and write from HW interfaces constantly, and in some cases we skip it (when not inactive or active). But then the controllers cannot run at all since they don't have interfaces to connect to – so I doubt that we have to do anything with them here.

@@ -1074,9 +1074,13 @@ HardwareReadWriteStatus ResourceManager::read(
{
if (component.read(time, period) != return_type::OK)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not simply change this?

Suggested change
if (component.read(time, period) != return_type::OK)
if (component.read(time, period) == return_type::ERROR)

@flochre
Copy link
Contributor Author

flochre commented Feb 19, 2023

Hi @destogl !

Thanks for your answer!
Maybe I am solving a different issue here then :)

My attempt here is to try and solve problem I am facing with the best practice from hardware_interface

`hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period)
{

bool hardware_go = main_loop_update_rate_ == 0 ||
desired_hw_update_rate_ == 0 ||
((update_loop_counter_ % desired_hw_update_rate_) == 0);

if (hardware_go){
// hardware comms and operations

}
...

// update counter
++update_loop_counter_;
update_loop_counter_ %= main_loop_update_rate_;
}`

Using this I need to return hardware_interface::return_type::OK or the controller will stop.
And then because I am sending hardware == OK my ros2_controller will be sending "old data" and publishing over and over my old data until new data is there.
By sending SKIPPED I tell the controller there are no new data do not update but also do not stop

from here comes the if (component.read(time, period) != return_type::OK)
If OK keep going if not check if it should stop or just skip.

This PR is an attempt to stop the ros2_controller to publish the data as long as there is no new data available at the rate where the hardware_go flag goes to true and without completely stopping the controller.

It appears that I misunderstood the issue here. Maybe I should open another issue?

@codecov-commenter
Copy link

codecov-commenter commented Apr 8, 2023

Codecov Report

Merging #933 (4e0d021) into master (8141212) will decrease coverage by 0.03%.
The diff coverage is 26.66%.

Additional details and impacted files
@@            Coverage Diff             @@
##           master     #933      +/-   ##
==========================================
- Coverage   31.83%   31.80%   -0.03%     
==========================================
  Files          94       94              
  Lines       10483    10494      +11     
  Branches     7136     7144       +8     
==========================================
+ Hits         3337     3338       +1     
- Misses        801      805       +4     
- Partials     6345     6351       +6     
Flag Coverage Δ
unittests 31.80% <26.66%> (-0.03%) ⬇️

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

Files Coverage Δ
.../include/controller_manager/controller_manager.hpp 18.75% <ø> (ø)
hardware_interface/src/resource_manager.cpp 46.68% <33.33%> (-0.08%) ⬇️
controller_manager/src/controller_manager.cpp 38.40% <25.00%> (-0.16%) ⬇️

... and 1 file with indirect coverage changes

@destogl
Copy link
Member

destogl commented Oct 15, 2023

Closing because doesn't solve the mentioned issues, and it is not a clean approach to solve the issue user has. If there are the problems with hardware so that data is not written, then the controllers have to be stopped.

@destogl destogl closed this Oct 15, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants