Add controller manager services #139

v-lopez · 2020-09-02T14:09:46Z

Added services for:

load controller
unload controller
switch controllers
list controllers
list controller_types
reload controller libraries

To do that, besides adding all the respective code I've had to perform the following modifications to the existing code:

Add the double buffered list from ROS1 control to avoid locking the update thread
Modify the ControllerLoaderInterface to add the base class name to the constructor
update() only the active controllers, not all loaded controllers.

The whole resource management implementation is not done yet, since that part is undergoing changes.
It is usually #ifdef'ed out with the tag TODO_IMPLEMENT_RESOURCE_CHECKING

v-lopez · 2020-09-02T14:37:08Z

controller_manager/src/controller_manager.cpp

+  // TODO(v-lopez) this should only be done if controller_manager is configured.
+  // Probably the whole load_controller part should fail if the controller_manager
+  // is not configured, should it implement a LifecycleNodeInterface
+  controller->get_lifecycle_node()->configure();


@Karsten1987 Could I have your opinion regarding this? I do not fully understand why some classes inherit from Nodes, others from LifeCycleNode and others from LifeCycleNodeInterface, but have a member LifeCycleNode.

And to the best of my Google skills there's not much written about when to use what.

codecov-commenter · 2020-09-02T14:44:47Z

Codecov Report

Merging #139 into master will decrease coverage by 2.64%.
The diff coverage is 29.12%.

@@            Coverage Diff             @@
##           master     #139      +/-   ##
==========================================
- Coverage   36.61%   33.97%   -2.65%     
==========================================
  Files          42       44       +2     
  Lines        1726     2337     +611     
  Branches     1080     1524     +444     
==========================================
+ Hits          632      794     +162     
- Misses        100      174      +74     
- Partials      994     1369     +375

Flag	Coverage Δ
#unittests	`33.97% <29.12%> (-2.65%)`	⬇️

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

Impacted Files	Coverage Δ
...controller_manager/controller_loader_pluginlib.hpp	`100.00% <ø> (ø)`
...r_manager/test/test_controller/test_controller.cpp	`100.00% <ø> (ø)`
...r_manager/test/test_controller/test_controller.hpp	`100.00% <ø> (ø)`
controller_manager/test/test_load_controller.cpp	`11.53% <11.34%> (-7.51%)`	⬇️
...ontroller_manager/test/test_controller_manager.cpp	`8.88% <13.63%> (-19.69%)`	⬇️
...ller_manager/test/test_controller_manager_srvs.cpp	`19.73% <19.73%> (ø)`
controller_manager/src/controller_manager.cpp	`40.17% <39.66%> (-16.73%)`	⬇️
.../include/controller_manager/controller_manager.hpp	`62.50% <57.14%> (-4.17%)`	⬇️
...er_manager/test/controller_manager_test_common.hpp	`60.00% <60.00%> (ø)`
...roller_manager/src/controller_loader_pluginlib.cpp	`83.33% <71.42%> (ø)`
... and 5 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 9a25721...ae10741. Read the comment docs.

controller_manager/include/controller_manager/controller_manager.hpp

v-lopez · 2020-09-16T09:48:57Z

controller_manager/src/controller_manager.cpp

-  for (auto loaded_controller : loaded_controllers_) {
-    auto controller_state = loaded_controller->get_lifecycle_node()->activate();
+  for (auto loaded_controller : controllers_lists_[current_controllers_list_]) {
+    auto controller_state = loaded_controller.c->get_lifecycle_node()->activate();


I believe this code should be completely removed now.

When we activate the controller manager, we should not activate all loaded controllers, we should only activate controllers on the switch_controller function.

But for deactivate, I believe we need to deactivate them.

This creates a unintuitive situation, you deactivate the controller manager, it deactivates the controllers, but you activate it, and it doesn't activate the controllers.

think of deactivating the controller manager as a last resort, something that an emergency stop would call for instance. I agree this part should be gone

What about activating previously active controllers?

Like you said, in the event of an emergency (which happen quite often with research robots) it's going to be a pain in the ass to reactivate controllers manually, it's something that is not needed in ROS1 right now.

Another alternative is not to deactivate the controllers themselves, just stop updating them because the controller manager is disabled.

I think this one is strongly about safety. A velocity controller will keep sending the same velocity (arm of robot base!) without an update, robot arms may be restarted with the old command but from a new joint position (if an arm doesn't know to hold position without sending command actively) and may overreact to reach the new target. I'd err on the side of caution and don't mind people writing one-liners to call the switch functionality every time they had to shut down controls.

All right, removing the activation of controllers.

destogl

To be continued...

controller_manager/include/controller_manager/controller_manager.hpp

controller_manager/include/controller_manager/controller_spec.hpp

controller_manager/src/controller_loader_pluginlib.cpp

controller_manager/src/controller_manager.cpp

destogl · 2020-09-16T10:06:22Z

controller_manager/src/controller_manager.cpp

+  const std::string & controller_name)
+{
+  // get reference to controller list
+  int free_controllers_list = (current_controllers_list_ + 1) % 2;


Can you explain the logic here a bit?

This should be the same logic that was used on ROS1.

The idea is that the update loop is using the list A, so you do all your modifications on the list B.

At the end of the function, you change current_controllers_list_ so the index points to list B, on the next loop the B list will be used, and A is available to be cleared.

yeah I think this could definitely do with more documentation. the logic here is similar to how some STL containers do memory management, moving things to the end of their allocated array and freeing stuff after the end index.

Sure, I'll add it.

This should be the same logic that was used on ROS1.

OK, this could be. I still do not understand this free_controllers_list variable. I will contact you directly, maybe you could help me to grap it.

You can mark this as resolved.

I have refactored the whole thing here: 4c6a609

Hopefully it makes the original code easier to understand.

controller_manager/src/controller_manager.cpp

controller_manager/include/controller_manager/controller_manager.hpp

bmagyar · 2020-09-17T07:05:32Z

controller_manager/include/controller_manager/controller_spec.hpp

@@ -0,0 +1,45 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.


It seems this license header is from the PoC ros2_control, if this is copied from ros1 I think you can re-own it with PAL under the new license. If you want to keep the ros1 license that'd be tricky mixing licenses

bmagyar · 2020-09-17T07:16:23Z

controller_manager/src/controller_manager.cpp

+  const std::string & controller_name)
+{
+  // get reference to controller list
+  int free_controllers_list = (current_controllers_list_ + 1) % 2;


yeah I think this could definitely do with more documentation. the logic here is similar to how some STL containers do memory management, moving things to the end of their allocated array and freeing stuff after the end index.

controller_manager/src/controller_manager.cpp

bmagyar · 2020-09-18T08:01:32Z

controller_manager/src/controller_manager.cpp

-  for (auto loaded_controller : loaded_controllers_) {
-    auto controller_state = loaded_controller->get_lifecycle_node()->activate();
+  for (auto loaded_controller : controllers_lists_[current_controllers_list_]) {
+    auto controller_state = loaded_controller.c->get_lifecycle_node()->activate();


think of deactivating the controller manager as a last resort, something that an emergency stop would call for instance. I agree this part should be gone

controller_manager/test/controller_manager_test_common.hpp

destogl

Second part

controller_manager/include/controller_manager/controller_loader_interface.hpp

controller_manager/include/controller_manager/controller_loader_pluginlib.hpp

controller_manager/include/controller_manager/controller_manager.hpp

controller_manager/src/controller_manager.cpp

destogl · 2020-09-18T11:28:52Z

controller_manager/src/controller_manager.cpp

+  const std::string & controller_name)
+{
+  // get reference to controller list
+  int free_controllers_list = (current_controllers_list_ + 1) % 2;


This should be the same logic that was used on ROS1.

OK, this could be. I still do not understand this free_controllers_list variable. I will contact you directly, maybe you could help me to grap it.

You can mark this as resolved.

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

destogl

Just a few small comments.

It looks very good! Thank you for the great work!

controller_manager/include/controller_manager/controller_manager.hpp

destogl · 2020-09-19T13:00:07Z

controller_manager/src/controller_manager.cpp

+
+int ControllerManager::RTControllerListWrapper::get_other_list(int index) const
+{
+  return (index + 1) % 2;


Ahhhh, I got it now :D

Haha no problem, I'm adding a short description for clarity.

I still think this is overkill to simply flip-flop an index between 0 and 1.

Well I believe it's better than having this random operation duplicated in a few places in the code, the function allows for giving the operation a name and some documentation. I could make it a free function so it's not even a private member of the class.

controller_manager_msgs/srv/LoadController.srv

controller_manager_msgs/srv/ReloadControllerLibraries.srv

controller_manager_msgs/srv/UnloadController.srv

hardware_interface/include/hardware_interface/controller_info.hpp

Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com> Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

Karsten1987

There're quite some comments inline. I am requesting mainly changes for some of the ported ROS1 code which I think should be handled with a few calls to STL. Additionally, I would rethink the double buffered list to consider really only flip-flopping between an active and inactive list.

controller_manager/include/controller_manager/controller_loader_interface.hpp

controller_manager/include/controller_manager/controller_manager.hpp

controller_manager/src/controller_manager.cpp

Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>

controller_manager/include/controller_manager/controller_manager.hpp

controller_manager/src/controller_manager.cpp

Karsten1987 · 2020-09-24T00:18:51Z

controller_manager/src/controller_manager.cpp

-  return loaded_controllers_.back();
+  // Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders)
+  for (const auto & controller_loader : loaders_) {
+    controller_loader->reload();


shouldn't the controller be re-configured as well?

In the old version it wasn't: https://github.com/ros-controls/ros_control/blob/noetic-devel/controller_manager/src/controller_manager.cpp#L663

We could discuss adding it, after reloading there are no loaded controllers. We may want to load (create + configure) them.

I'd err on the side of caution here, we don't know what we are loading and whether the newly loaded ones will be compatible with each other.

controller_manager/src/controller_manager.cpp

get_controller_by_name was confusing as to in which list it might look into, and was returning a non const pointer to a controller that might be being used in the RT thread

Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>

As discussed in ros-controls#139 (review) Also removed a test that was redundant after these changes

controller_manager/src/controller_manager.cpp

bmagyar · 2020-09-29T08:49:01Z

Thanks for following up on the remaining discussions. I'm merging this now, @Karsten1987 @destogl please refer to the issues to continue some of the design discussions.

Karsten1987 · 2020-09-29T17:37:09Z

This PR introduced some regressions on clang.

In file included from /Users/karsten/workspace/ros2/ros2_control_ws/src/ros-controls/ros2_control/controller_manager/src/controller_manager.cpp:24:
/Users/karsten/workspace/ros2/ros2_control_ws/src/ros-controls/ros2_control/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp:39:54: error: 'create' overrides a member function but is not marked 'override' [-Werror,-Winconsistent-missing-override]
  controller_interface::ControllerInterfaceSharedPtr create(const std::string & controller_type);
                                                     ^
/Users/karsten/workspace/ros2/ros2_control_ws/src/ros-controls/ros2_control/controller_manager/include/controller_manager/controller_loader_interface.hpp:40:62: note: overridden virtual function is here
  virtual controller_interface::ControllerInterfaceSharedPtr create(
                                                             ^
In file included from /Users/karsten/workspace/ros2/ros2_control_ws/src/ros-controls/ros2_control/controller_manager/src/controller_manager.cpp:24:
/Users/karsten/workspace/ros2/ros2_control_ws/src/ros-controls/ros2_control/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp:42:8: error: 'is_available' overrides a member function but is not marked 'override' [-Werror,-Winconsistent-missing-override]
  bool is_available(const std::string & controller_type) const;
       ^
/Users/karsten/workspace/ros2/ros2_control_ws/src/ros-controls/ros2_control/controller_manager/include/controller_manager/controller_loader_interface.hpp:44:16: note: overridden virtual function is here
  virtual bool is_available(const std::string & controller_type) const = 0;
               ^
2 errors generated.
make[2]: *** [CMakeFiles/controller_manager.dir/src/controller_manager.cpp.o] Error 1
make[1]: *** [CMakeFiles/controller_manager.dir/all] Error 2
make: *** [all] Error 2
---
Failed   <<< controller_manager [4.42s, exited with code 2]

ros-controls#139)

v-lopez commented Sep 2, 2020

View reviewed changes

v-lopez mentioned this pull request Sep 2, 2020

add ROS services for controller manager #60

Closed

v-lopez force-pushed the add-controller-manager-services branch from 945c3aa to 064bdc3 Compare September 2, 2020 15:35

destogl reviewed Sep 7, 2020

View reviewed changes

controller_manager/include/controller_manager/controller_manager.hpp Show resolved Hide resolved

v-lopez force-pushed the add-controller-manager-services branch 2 times, most recently from 5198371 to ee4a1d5 Compare September 9, 2020 13:09

v-lopez marked this pull request as ready for review September 15, 2020 15:35

bmagyar requested review from destogl and bmagyar September 16, 2020 09:17

v-lopez commented Sep 16, 2020

View reviewed changes

destogl requested changes Sep 16, 2020

View reviewed changes

v-lopez mentioned this pull request Sep 17, 2020

Unify return_type nomenclature in hardware_interface and controller_interface #151

Closed

Victor Lopez added 6 commits September 18, 2020 08:31

Add controller_manager_msgs package

5deeaf6

Add switch and unload controller functionality

f00ecec

Use ControllerSpec for storing controller type and name

91e1767

Add more messages and services

bef08e2

Add list_controllers service

27206ea

Implement load, unload, switch and reload services

47b170f

v-lopez force-pushed the add-controller-manager-services branch from 26ef14d to 47b170f Compare September 18, 2020 06:34

bmagyar requested changes Sep 18, 2020

View reviewed changes

Remove whitespaces

81a8d21

This was referenced Sep 18, 2020

Loading a controller should only be allowed after controller_manager has been configured #152

Closed

Cache running controllers to avoid checking status on update() loop #153

Closed

Victor Lopez added 4 commits September 18, 2020 10:48

Reference issues for todos added during services PR

160a7c7

Refactor rt double buffered list for easier comprehension and safety

4c6a609

Rename isControllerRunning function

2b81d5d

Don't use raw pointers in switch controller and get_controller_by_name

e4e226c

destogl requested changes Sep 18, 2020

View reviewed changes

Apply suggestions from code review

64ff2b2

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

destogl approved these changes Sep 20, 2020

View reviewed changes

v-lopez and others added 4 commits September 21, 2020 10:34

Apply suggestions from code review

3481676

Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com> Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

Documentation for controller_manager RT List wrapper and parameters

62df3aa

controller_manager activate will not activate controllers

bc63827

Fix constness in controller_loader

3650c38

Karsten1987 suggested changes Sep 21, 2020

View reviewed changes

v-lopez and others added 6 commits September 22, 2020 09:47

Apply suggestions from code review

9c05a5e

Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>

Fix remaining camelCase to snake_case

0967122

Update copyright year

2805044

Apply PR suggestions

ef1187f

Use find algorithms for finding controllers

0067623

Deduplicate code

b6ea360

bmagyar mentioned this pull request Sep 23, 2020

Remove ControllerLoaderInterface #160

Closed

Karsten1987 reviewed Sep 24, 2020

View reviewed changes

v-lopez mentioned this pull request Sep 25, 2020

Remove ControllerSpec and simplify add_controller functions in CM #162

Open

Victor Lopez and others added 3 commits September 25, 2020 07:59

Remove get_controller_by_name and use names in stop/start request lists

2fba683

get_controller_by_name was confusing as to in which list it might look into, and was returning a non const pointer to a controller that might be being used in the RT thread

Apply suggestions from code review

dd02801

Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>

Remove activate, deactivate, configure and cleanup

ae10741

As discussed in ros-controls#139 (review) Also removed a test that was redundant after these changes

v-lopez force-pushed the add-controller-manager-services branch from fdb44d8 to ae10741 Compare September 25, 2020 06:46

v-lopez commented Sep 28, 2020

View reviewed changes

controller_manager/src/controller_manager.cpp Outdated Show resolved Hide resolved

bmagyar approved these changes Sep 28, 2020

View reviewed changes

Victor Lopez added 2 commits September 29, 2020 09:29

Add namespace to controller_manager services

9aff325

Allow undeclared parameters on controller_manager

528d1c9

v-lopez force-pushed the add-controller-manager-services branch from e53f5cb to 528d1c9 Compare September 29, 2020 08:01

bmagyar merged commit facb804 into ros-controls:master Sep 29, 2020

Karsten1987 mentioned this pull request Sep 29, 2020

fix regression in controller manager services #169

Merged

destogl mentioned this pull request Oct 12, 2020

Add ros2 control managers #147

Closed

4 tasks

destogl pushed a commit to StoglRobotics-forks/ros2_control that referenced this pull request Aug 11, 2022

Remove unused sensor_msgs dependency (was non-declared in package.xml) (

b192299

ros-controls#139)

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add controller manager services #139

Add controller manager services #139

v-lopez commented Sep 2, 2020 •

edited

v-lopez Sep 2, 2020

codecov-commenter commented Sep 2, 2020 •

edited

v-lopez Sep 16, 2020

bmagyar Sep 18, 2020

v-lopez Sep 18, 2020

bmagyar Sep 19, 2020

v-lopez Sep 21, 2020

destogl left a comment

destogl Sep 16, 2020

v-lopez Sep 16, 2020

bmagyar Sep 17, 2020

v-lopez Sep 18, 2020

destogl Sep 18, 2020

v-lopez Sep 18, 2020

bmagyar Sep 17, 2020

bmagyar Sep 17, 2020

bmagyar Sep 18, 2020

destogl left a comment

destogl Sep 18, 2020

destogl left a comment

destogl Sep 19, 2020

v-lopez Sep 21, 2020

Karsten1987 Sep 21, 2020

v-lopez Sep 22, 2020

Karsten1987 left a comment

Karsten1987 Sep 24, 2020

v-lopez Sep 25, 2020

bmagyar Sep 28, 2020

bmagyar commented Sep 29, 2020

Karsten1987 commented Sep 29, 2020

		@@ -0,0 +1,45 @@
		// Copyright 2017 Open Source Robotics Foundation, Inc.

Add controller manager services #139

Add controller manager services #139

Conversation

v-lopez commented Sep 2, 2020 • edited

Choose a reason for hiding this comment

codecov-commenter commented Sep 2, 2020 • edited

Codecov Report

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

destogl 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

destogl left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

destogl 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

Karsten1987 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

bmagyar commented Sep 29, 2020

Karsten1987 commented Sep 29, 2020

v-lopez commented Sep 2, 2020 •

edited

codecov-commenter commented Sep 2, 2020 •

edited