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

Full functionality of chainable controllers #667

Merged
merged 31 commits into from
Jun 30, 2022

Conversation

destogl
Copy link
Member

@destogl destogl commented Mar 8, 2022

This PR adds full changes for the functionality of chainable controllers.
This is an integration PR, and it is not intended to be merged, but to show the full scope of changes and provide simple testing possibility of functionality.

The PR is contains (and it is already divided) into following smaller PRs/functionalities:

@destogl destogl requested a review from bmagyar March 8, 2022 15:34
@destogl destogl self-assigned this Mar 8, 2022
@mergify
Copy link
Contributor

mergify bot commented Mar 14, 2022

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

@mergify
Copy link
Contributor

mergify bot commented Mar 17, 2022

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

*
* \returns list of CommandInterfaces that other controller can use as their outputs.
*/
virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() = 0;
Copy link
Member

Choose a reason for hiding this comment

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

Hello @destogl,

There is some good work in this PR. I was thinking that it could be also nice to have the StateInterfaces being exported from the controller, along with the CommandInterfaces. The issue with the CommandInterfaces is that there could be only one resource that can use it, however, there could be cases that multiple controllers might be interested in the information of a specific controller. For instance, a state estimator, many other controllers could use this information in parallel.

Copy link
Member Author

Choose a reason for hiding this comment

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

Hi @saikishor,

this is exactly something I am thinking about in the last days. Nevertheless, this feature will and have to come completely separated when we have finished chaining of command interfaces :)
My proposal, let's make this features finished, and then it will be much easier to get almost the same functionality with the state interfaces. Although there will be some complexities there to make this properly.

Copy link
Member

Choose a reason for hiding this comment

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

@destogl I have few more questions regarding the implementation; This implementation with the CommandInterfaces if I understand right, it's when your controller is exposing some interfaces so other controllers can command it right?, like a position controller expecting joint commands from the higher level controllers?. Please correct me if I am wrong.

Regarding the naming convention of the interfaces, IMHO using hardware_interfaces to have the communication done between the controllers is strange, because hardware_interfaces as the name signifies should be bound to the hardware, and for the controllers, we could have something like controller_interfaces or software_interfaces. What do you think about this?. My proposal is to move the part of the handle and other stuff to a new package and then reuse them in the current hardware_interfaces package and in the controller_interfaces or software_interfaces. If this is something that is not interesting atleast add a naming alias in the hardware_interface package. This makes easier for the developer to discriminate between them.

Copy link
Member Author

Choose a reason for hiding this comment

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

@destogl I have few more questions regarding the implementation; This implementation with the CommandInterfaces if I understand right, it's when your controller is exposing some interfaces so other controllers can command it right?, like a position controller expecting joint commands from the higher level controllers?. Please correct me if I am wrong.

This is correct!

Regarding the naming convention of the interfaces, IMHO using hardware_interfaces to have the communication done between the controllers is strange, because hardware_interfaces as the name signifies should be bound to the hardware, and for the controllers, we could have something like controller_interfaces or software_interfaces. What do you think about this?. My proposal is to move the part of the handle and other stuff to a new package and then reuse them in the current hardware_interfaces package and in the controller_interfaces or software_interfaces. If this is something that is not interesting atleast add a naming alias in the hardware_interface package. This makes easier for the developer to discriminate between them.

We are actually not using "hardware_interfaces". There is hardware interface package that where handles are defined. They are defined there because Thea are generally used by the hardware to provide access from controllers to it. Using the same type for handles for commanding hardware and providing inputs for controllers makes sense from functionality perspective because it is the same. Also this simplifies internal management of all interfaces which is also the same.

We could move this to another package, but I am not sure this would simplify things. It would result in an additional package with very little code and additional dependencies for all other packages.

I am also not sure this would add any clarity because developers does not have to do much with this types. They are used once or twice.

Can you try to make a concrete proposal how this changes would look like? Then we can find a common solution. I understand the issues you see, but I am not sure how to solve it without adding more complexity to the code base.

Copy link
Contributor

Choose a reason for hiding this comment

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

I have an example: Typically I want to control the Center of Mass of my system. This is not really a hardware_interface but this is a reference quantity that I need to be controlled. For this I want the controller to take as an input the reference and the estimated value.

Don't you think we could simply make :
typedef CoMControllerInterface hardware_interface::CommandInterface
and use the new defined type to make the controller interface clearer ?

Copy link
Member

Choose a reason for hiding this comment

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

I have an example: Typically I want to control the Center of Mass of my system. This is not really a hardware_interface but this is a reference quantity that I need to be controlled. For this I want the controller to take as an input the reference and the estimated value.

Yes, this is one kind of use case I was also thinking too. I do get the idea behind the Command Interfaces.

Don't you think we could simply make :
typedef CoMControllerInterface hardware_interface::CommandInterface
and use the new defined type to make the controller interface clearer ?

@olivier-stasse yes, ofcourse it makes it clear, but if we have 10 different types of references/inputs to your system, we don't want to define 10 different typedef lines. I don't think typedefs is a good idea in this case. My intention is that something that has symantic meaning would be interesting.

Copy link
Member

@saikishor saikishor Apr 6, 2022

Choose a reason for hiding this comment

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

Hello @destogl,

Using the same type for handles for commanding hardware and providing inputs for controllers makes sense from functionality perspective because it is the same. Also this simplifies internal management of all interfaces which is also the same.

I completely agree with the idea behind it and also from the functionality perspective.

We could move this to another package, but I am not sure this would simplify things. It would result in an additional package with very little code and additional dependencies for all other packages.

This is something we should think of. May be we can come up with a solution that requires less changes.

I am also not sure this would add any clarity because developers does not have to do much with this types. They are used once or twice.

Well, maintaining semantic meaning is something important for a software like ROS. I think we should strive to maintain semantic meanings whenever we can.

Can you try to make a concrete proposal how this changes would look like? Then we can find a common solution. I understand the issues you see, but I am not sure how to solve it without adding more complexity to the code base.

Sure, I can try to come back to you with a proposal.

Thank you.

Copy link
Contributor

@livanov93 livanov93 left a comment

Choose a reason for hiding this comment

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

Great job with tests!

Copy link
Contributor

@olivier-stasse olivier-stasse left a comment

Choose a reason for hiding this comment

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

To be honest this would need a deep reading and testing phase to give you a useful feedback.
It looks good to me, apart one thing that I may have misunderstood: how do you check the dependency of the controllers with each other to have the proper order of call ?

I would expect to see this in the resource manager. My comprehension is that a controller holding the CommandInterface of another Controller should have precedence. But I failed to see where it is...

{
RCLCPP_ERROR(
get_node()->get_logger(),
"Can not change controller's chained mode because it is no in '%s' state. "
Copy link
Contributor

Choose a reason for hiding this comment

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

no -> now ?

Copy link
Member Author

Choose a reason for hiding this comment

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

Thanks! It should be “not”.

Regarding other question, there is no sorting of controller right now. But if you load them in proper order, the approach will work. Of course, this is not something we can leave like that, and we have do some sorting.

Copy link
Contributor

@olivier-stasse olivier-stasse Apr 7, 2022

Choose a reason for hiding this comment

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

Thanks for the answer. Then to move forward, and maybe with some proper warning about the order, I believe that we should move forward to integrate it.

@mergify
Copy link
Contributor

mergify bot commented May 13, 2022

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

Update structure of chainable controller to allow mode chaning only in 'UNCONFIGURED' state.

Switching to chained_mode is only forbidden if controller is active.

Add default implementation for 'on_set_chained_mode' method.

Avoid compile warning about unused variables.

Added check if all reference interface storage is initialized and that interface prefix is equal to controller's name.

Optimize tests and 'usings'.

Use two internal methods instead of 'update' directly on chained controllers.

Reorder constructor.
@mergify
Copy link
Contributor

mergify bot commented May 17, 2022

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

@mergify
Copy link
Contributor

mergify bot commented May 18, 2022

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

Copy link
Member

@bmagyar bmagyar left a comment

Choose a reason for hiding this comment

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

The two largest changesets are still missing from my review, this is the first pass

@@ -243,12 +254,13 @@ class ControllerManager : public rclcpp::Node
// Per controller update rate support
unsigned int update_loop_counter_ = 0;
unsigned int update_rate_ = 100;
std::vector<std::vector<std::string>> chained_controllers_configuration_;

std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
Copy link
Member

Choose a reason for hiding this comment

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

umm... were moved here for testability? can we have them in a protected section instead perhaps?

Copy link
Member Author

Choose a reason for hiding this comment

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

This is a protected section. Yes, I moved it for simpler testing, otherwise doesn't work, or didn't come with a simple solution to do so.

Copy link
Member

Choose a reason for hiding this comment

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

This is why we moved things to protected and use a "TestableX` class wrapper or friend to write the tests where you can define a getter for these functions and do you assertions

Copy link
Member Author

Choose a reason for hiding this comment

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

Again: those members are protected or am I seeing something wrong?

controller_manager/src/controller_manager.cpp Outdated Show resolved Hide resolved
controller_manager/src/controller_manager.cpp Outdated Show resolved Hide resolved
"Stop the controller manager immediately and restart it.",
interface_name.c_str());
return false;
// TODO(destol): Should we here throw an error? I don't like the idea, but we should
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
// TODO(destol): Should we here throw an error? I don't like the idea, but we should
// TODO(destogl): Should we here throw an error? I don't like the idea, but we should

😆

Copy link
Member Author

Choose a reason for hiding this comment

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

I would rather get the answer to this question so we can delete the comment.

Copy link
Member

Choose a reason for hiding this comment

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

You are right. I think there should be a throw here instead of a boolean return.

The error message says it best:

  "Stop the controller manager immediately and restart it.",

which is somewhat equivalent to HCF

Copy link
Member Author

Choose a reason for hiding this comment

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

Added runtime_error throw

@@ -337,6 +389,8 @@ controller_interface::return_type ControllerManager::unload_controller(
}

RCLCPP_DEBUG(get_logger(), "Cleanup controller");
// TODO(destogl): remove reference interface is chainable; i.e., add a separate method for
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
// TODO(destogl): remove reference interface is chainable; i.e., add a separate method for
// TODO(destogl): remove reference interface if chainable; i.e., add a separate method for

Copy link
Member Author

Choose a reason for hiding this comment

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

Also, should we keep the comment or do not remove interfaces for now?

Copy link
Member

Choose a reason for hiding this comment

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

I think we should remove interfaces already if possible

Copy link
Member Author

Choose a reason for hiding this comment

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

I will do this in the follow-up PR.

controller_manager/src/controller_manager.cpp Outdated Show resolved Hide resolved
RCLCPP_FATAL(
get_logger(),
"The internal 'from' and 'to' chained mode requests are not empty at the "
"switch_controller() call. This should not happen.");
Copy link
Member

Choose a reason for hiding this comment

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

Any hints that could help the user fix the issue? This sounds like an error meant for us, and for us even a baked-in line-number reporting would be just fine, "error happened at line 34183 in file X". Let's try helping users here a bit too

Also... the line number prints may actually be useful in many of these prints we have for internal error reporting...

Copy link
Member Author

Choose a reason for hiding this comment

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

Nah, you can always enable line numbers in your logging configuration. Having them here would just lead for them to be wrong after a short time. It is easier to search for the strings.

TBH, this should really never happen if we have a proper multi-threading management, and we should have tests for it anyway.

Copy link
Member

Choose a reason for hiding this comment

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

but what should we do with these cases then? throw an exception? even the error message says, this should never happen which IMO is where people write assert(false); in older codebases

Copy link
Member Author

Choose a reason for hiding this comment

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

added throwing. It means memory corruption or sync between threads is not working.

Copy link
Member

@bmagyar bmagyar left a comment

Choose a reason for hiding this comment

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

part 2, tests file is missing

continue;
}
// TODO(destogl): performance of this code could be optimized by adding additional lists with
// controllers that the check has failed and has succeeded. The the following would be done
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
// controllers that the check has failed and has succeeded. The the following would be done
// controllers that the check has failed and has succeeded. Then the following would be done

Copy link
Member Author

Choose a reason for hiding this comment

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

done

controller_manager/src/controller_manager.cpp Outdated Show resolved Hide resolved
controller_manager/src/controller_manager.cpp Outdated Show resolved Hide resolved
controller_manager/src/controller_manager.cpp Outdated Show resolved Hide resolved
{
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
// reset all lists
stop_request_.clear();
Copy link
Member

Choose a reason for hiding this comment

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

this pattern of "zero everything" repeats multiple times, one could write a function clear_requests() to do this

Copy link
Member Author

Choose a reason for hiding this comment

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

done.

Copy link
Member

Choose a reason for hiding this comment

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

how about inside handle_conflict()? and line 861 and 919?

controller_manager/src/controller_manager.cpp Show resolved Hide resolved
controller_manager/src/controller_manager.cpp Show resolved Hide resolved
@@ -614,6 +1042,26 @@ controller_interface::return_type ControllerManager::switch_controller(
start_request_.erase(start_list_it);
}

const auto list_interfaces = [this](
Copy link
Member

Choose a reason for hiding this comment

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

I'd expect a list_... function to print something but this is more like and extract_... or get_...?

also, this could probably be a separate function

Copy link
Member

Choose a reason for hiding this comment

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

this remains here, not outdated

Copy link
Member Author

Choose a reason for hiding this comment

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

you are right!


if (ret != controller_interface::return_type::OK)
{
RCLCPP_WARN(
Copy link
Member

Choose a reason for hiding this comment

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

for the record, I found the readability of the code now pretty hard, thanks to these prints... I really don't know what to do about them but it's taking the focus away from the important stuff

controller_manager/src/controller_manager.cpp Show resolved Hide resolved
@mergify
Copy link
Contributor

mergify bot commented Jun 8, 2022

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

Copy link
Member

@bmagyar bmagyar left a comment

Choose a reason for hiding this comment

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

Github's auto-dismiss of "outdated" discussion points is really annoying

@@ -243,12 +254,13 @@ class ControllerManager : public rclcpp::Node
// Per controller update rate support
unsigned int update_loop_counter_ = 0;
unsigned int update_rate_ = 100;
std::vector<std::vector<std::string>> chained_controllers_configuration_;

std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
Copy link
Member

Choose a reason for hiding this comment

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

This is why we moved things to protected and use a "TestableX` class wrapper or friend to write the tests where you can define a getter for these functions and do you assertions

"Stop the controller manager immediately and restart it.",
interface_name.c_str());
return false;
// TODO(destol): Should we here throw an error? I don't like the idea, but we should
Copy link
Member

Choose a reason for hiding this comment

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

You are right. I think there should be a throw here instead of a boolean return.

The error message says it best:

  "Stop the controller manager immediately and restart it.",

which is somewhat equivalent to HCF

@@ -337,6 +389,8 @@ controller_interface::return_type ControllerManager::unload_controller(
}

RCLCPP_DEBUG(get_logger(), "Cleanup controller");
// TODO(destogl): remove reference interface is chainable; i.e., add a separate method for
Copy link
Member

Choose a reason for hiding this comment

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

I think we should remove interfaces already if possible

controller_manager/src/controller_manager.cpp Outdated Show resolved Hide resolved
{
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
// reset all lists
stop_request_.clear();
Copy link
Member

Choose a reason for hiding this comment

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

how about inside handle_conflict()? and line 861 and 919?

controller_manager/src/controller_manager.cpp Show resolved Hide resolved
@@ -614,6 +1042,26 @@ controller_interface::return_type ControllerManager::switch_controller(
start_request_.erase(start_list_it);
}

const auto list_interfaces = [this](
Copy link
Member

Choose a reason for hiding this comment

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

this remains here, not outdated

controller_manager/src/controller_manager.cpp Show resolved Hide resolved
@bmagyar
Copy link
Member

bmagyar commented Jun 30, 2022

Sad part is that there is 0 CI stage that successfully can build the branch.

I tested it locally and things look fine.

@destogl destogl merged commit 70514dd into ros-controls:master Jun 30, 2022
@destogl destogl deleted the chainable-controllers branch June 30, 2022 14:50
destogl added a commit to Schulze18/ros2_control that referenced this pull request Jun 30, 2022
…s-controls#667)

* auto-switching of chained mode in controllers
* interface-matching approach for managing chaining controllers
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.

6 participants