Full functionality of chainable controllers #667

destogl · 2022-03-08T15:34:54Z

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:

Make ControllerManager tests more flexible and reusable for different scenarios. Use more parameterized tests regarding strictness. #661
Use lifecycle nodes in controllers again #538
Add tests for ControllerInterface class. #662
Add ControllerInterfaceBase class with methods for chainable controller #717
Adding base class for chained controllers: ChainedControllersInterface #663
Resource Manager extension to support management of reference interfaces from chained controllers. #664

mergify · 2022-03-14T15:05:06Z

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

mergify · 2022-03-17T21:39:16Z

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

saikishor · 2022-03-29T10:55:21Z

controller_interface/include/controller_interface/chainable_controller_interface.hpp

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


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.

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.

@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.

@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.

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 ?

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.

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.

livanov93

Great job with tests!

olivier-stasse

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...

olivier-stasse · 2022-04-07T05:16:58Z

controller_interface/src/chainable_controller_interface.cpp

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


no -> now ?

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.

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.

controller_interface/include/controller_interface/helpers.hpp

controller_interface/test/test_chainable_controller_interface.hpp

mergify · 2022-05-13T06:56:31Z

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.

…e interfaces.

mergify · 2022-05-17T15:32:26Z

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

mergify · 2022-05-18T11:11:58Z

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

bmagyar

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

controller_manager/include/controller_manager/controller_manager.hpp

bmagyar · 2022-05-24T06:01:47Z

controller_manager/include/controller_manager/controller_manager.hpp

@@ -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_;


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

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.

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

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

controller_manager/src/controller_manager.cpp

bmagyar · 2022-05-24T06:23:57Z

controller_manager/src/controller_manager.cpp

+      "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


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

😆

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

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

Added runtime_error throw

bmagyar · 2022-05-24T06:24:56Z

controller_manager/src/controller_manager.cpp

@@ -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


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

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

I think we should remove interfaces already if possible

I will do this in the follow-up PR.

controller_manager/src/controller_manager.cpp

bmagyar · 2022-05-24T06:26:23Z

controller_manager/src/controller_manager.cpp

+    RCLCPP_FATAL(
+      get_logger(),
+      "The internal 'from' and 'to' chained mode requests are not empty at the "
+      "switch_controller() call. This should not happen.");


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...

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.

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

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

hardware_interface/test/test_components/test_actuator.cpp

bmagyar

part 2, tests file is missing

bmagyar · 2022-05-26T07:05:38Z

controller_manager/src/controller_manager.cpp

+          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


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

controller_manager/src/controller_manager.cpp

bmagyar · 2022-05-26T07:13:52Z

controller_manager/src/controller_manager.cpp

+      {
+        RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
+        // reset all lists
+        stop_request_.clear();


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

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

controller_manager/src/controller_manager.cpp

bmagyar · 2022-05-26T07:19:14Z

controller_manager/src/controller_manager.cpp

@@ -614,6 +1042,26 @@ controller_interface::return_type ControllerManager::switch_controller(
      start_request_.erase(start_list_it);
    }

+    const auto list_interfaces = [this](


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

this remains here, not outdated

you are right!

bmagyar · 2022-05-26T07:20:38Z

controller_manager/src/controller_manager.cpp

+
+    if (ret != controller_interface::return_type::OK)
+    {
+      RCLCPP_WARN(


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

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

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

…ts simpler.

bmagyar

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

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

bmagyar · 2022-06-28T22:05:32Z

controller_manager/include/controller_manager/controller_manager.hpp

@@ -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_;


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

controller_manager/include/controller_manager/controller_manager.hpp

bmagyar · 2022-06-28T22:10:10Z

controller_manager/src/controller_manager.cpp

+      "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


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

bmagyar · 2022-06-28T22:11:35Z

controller_manager/src/controller_manager.cpp

@@ -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


I think we should remove interfaces already if possible

controller_manager/src/controller_manager.cpp

bmagyar · 2022-06-28T22:18:32Z

controller_manager/src/controller_manager.cpp

+      {
+        RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
+        // reset all lists
+        stop_request_.clear();


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

controller_manager/src/controller_manager.cpp

bmagyar · 2022-06-28T22:21:27Z

controller_manager/src/controller_manager.cpp

@@ -614,6 +1042,26 @@ controller_interface::return_type ControllerManager::switch_controller(
      start_request_.erase(start_list_it);
    }

+    const auto list_interfaces = [this](


this remains here, not outdated

controller_manager/src/controller_manager.cpp

bmagyar · 2022-06-30T10:34:30Z

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

I tested it locally and things look fine.

…s-controls#667) * auto-switching of chained mode in controllers * interface-matching approach for managing chaining controllers

destogl requested a review from bmagyar March 8, 2022 15:34

destogl self-assigned this Mar 8, 2022

destogl force-pushed the chainable-controllers branch from a846266 to 126f154 Compare March 16, 2022 12:26

destogl mentioned this pull request Mar 24, 2022

Chained controllers example ros-controls/ros2_control_demos#162

Closed

destogl force-pushed the chainable-controllers branch 2 times, most recently from 2131712 to 93adc2a Compare March 24, 2022 17:09

saikishor reviewed Mar 29, 2022

View reviewed changes

destogl mentioned this pull request Mar 29, 2022

Adding base class for chained controllers: ChainedControllersInterface #663

Merged

livanov93 approved these changes Mar 31, 2022

View reviewed changes

olivier-stasse reviewed Apr 7, 2022

View reviewed changes

olivier-stasse approved these changes Apr 7, 2022

View reviewed changes

destogl force-pushed the chainable-controllers branch from 92ed950 to c8c1088 Compare April 26, 2022 20:22

destogl marked this pull request as ready for review April 26, 2022 20:23

github-actions bot requested review from aprotyas, erickisos, kellyprankin, peterdavidfagan and rosterloh April 26, 2022 20:23

erickisos approved these changes May 9, 2022

View reviewed changes

controller_interface/include/controller_interface/helpers.hpp Show resolved Hide resolved

controller_interface/test/test_chainable_controller_interface.hpp Outdated Show resolved Hide resolved

destogl added 8 commits May 15, 2022 21:12

Add ControllerInterfaceBase class and use it in Controller Manager.

03ae2b2

Make nice user interface to export controllers.

c5d7383

Revert some changes to work on this stage.

862987e

Extend resource manager to manage reference interfaces from controllers.

0b7bd7f

Adjust interface between CM and RM for managing controllers' referenc…

2a4af6b

…e interfaces.

Small fixes in controller manager tests.

44b17e0

Added first tests up to configuration of chainable controllers.

b0cc0a7

destogl added 3 commits May 16, 2022 14:23

Update chainable controller for tests.

ed0d8d6

Add ControllerInterfaceBase class and use it in Controller Manager.

648d039

Make nice user interface to export controllers.

65bcd7c

destogl force-pushed the chainable-controllers branch from c8c1088 to 65bcd7c Compare May 16, 2022 18:58

Fixup tests to the new API.

cad977b

Merge branch 'master' into chainable-controllers

1cb9154

Merge branch 'master' into chainable-controllers

b0ff012

erickisos approved these changes May 23, 2022

View reviewed changes

bmagyar requested changes May 24, 2022

View reviewed changes

bmagyar requested changes May 26, 2022

View reviewed changes

pac48 mentioned this pull request Jun 15, 2022

Chainable Admittance Controller moveit/moveit2#1371

Closed

destogl added 5 commits June 18, 2022 11:39

Format code using clang-format-12 to make resolution of merge conflic…

d652a34

…ts simpler.

Merge branch 'master' into chainable-controllers

8ead608

Make trivial changes according to review comments.

4b81f80

Update switch_chained_mode methods per review.

fe20955

Move checks for chained controllers into separate methods.

702f69f

This was referenced Jun 26, 2022

Problem integrating trajectory controller and chainable interface #735

Closed

Support Chained Controllers in Moveit moveit/moveit2#1395

Closed

Add admittance controller ros-controls/ros2_controllers#370

Merged

bmagyar reviewed Jun 28, 2022

View reviewed changes

destogl mentioned this pull request Jun 29, 2022

Remove reference interfaces of chained controllers if those are unloaded. #743

Open

Second round of reviews.

e6d2f23

bmagyar approved these changes Jun 30, 2022

View reviewed changes

destogl merged commit 70514dd into ros-controls:master Jun 30, 2022

destogl deleted the chainable-controllers branch June 30, 2022 14:50

	// 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

	// 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

	// 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

Full functionality of chainable controllers #667

Full functionality of chainable controllers #667

Conversation

destogl commented Mar 8, 2022 • edited Loading

mergify bot commented Mar 14, 2022

mergify bot commented Mar 17, 2022

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

saikishor Apr 6, 2022 • edited Loading

Choose a reason for hiding this comment

livanov93 left a comment

Choose a reason for hiding this comment

olivier-stasse left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

olivier-stasse Apr 7, 2022 • edited Loading

Choose a reason for hiding this comment

mergify bot commented May 13, 2022

mergify bot commented May 17, 2022

mergify bot commented May 18, 2022

bmagyar 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

bmagyar 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

mergify bot commented Jun 8, 2022

bmagyar 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

bmagyar commented Jun 30, 2022

destogl commented Mar 8, 2022 •

edited

Loading

saikishor Apr 6, 2022 •

edited

Loading

olivier-stasse Apr 7, 2022 •

edited

Loading