Add ros2 control managers #147

destogl · 2020-09-15T10:19:57Z

This PR implements: Updated on 7.10.2020

ResourceManager - initial version to mange "SystemHardware" only (Example 1)
ros2_control_node - generic node to start the ROS2 control robots
ControllerManager - has now ResourceManager and supports components which supports. controller manager has multiple callback groups.

To limit amount of code in this PR, only functionalities needed for Example 1 - Robot will be implemented in this PR.

Solves #141, #142, and #143

To be done:

functionalitiy in the ControllerManagerNewWithManager
Use Forward controller in the Example 1
Registring controllers for components (de-registring should be done in follow up, to keep this PR short as possible)
Remove executor:: namespace (I am on Ubutnu 18 with eloquent, therefore needed....)

How to test: `ros2_control` with components

UPDATE: check the README in ros2_control_demos

…ncy collision. ControllerManager prepared to initialize a controller with components.

…ntation

…rce Manager give aditional error output.

…alize and use controllers. ResourceManager is now smart pointer. Enable undeclared paramters for the node.

destogl · 2020-10-01T09:58:02Z

Wit the last commit I kind of "closed the loop". So now is forward controller also loaded and called in update.

I have a huge issue... It seams that is something wrong with the initializaition of the LifecycleNode in the ControllerInterface L64 (here).
Description of issue:

Node gets the same name as the main node, i.e., ros2_control_node
I get error when try to access the paramters (even in the ros2_control_node namespace)
I do not get any callbacks from the topic subscriber
@Karsten1987 you are probably the most competent here regarding this. Do you have any idea?

There are many very nice additions in recent PRs from @Karsten1987 and I think we have to implement those also here.

Therefore, I just want to emphasize the goal of this PR.
This PR creates minimal implementation for ros2_control to realize the Example 1 from the roadmap. The idea is to have a small test-version from the simple hardware implementation to the controllers. This would give other people (except myself, @Karsten1987 and @bmagyar) to get the idea what we are actually trying to achieve with this huge restructuring of the framework. I think this woul also help other people to get involved into active development and since we have some kind of base.

I am very much aware that not all of the code here is realtime-friendly and that some concepts are sub-optimal. Nevertheless it provide a very concrete base to test different cases and optimize everything.

After we resolve the issue with the LifecycleNode I advocate that we merge this PR as the first step toward components and also include other people who would like to help.

I also added above the notes how to test this implementation on your computer.

… functions

Enable undeclared parameters for the controller's LifecycleNode s. Renable controller_manager tests. Clean and extend `ros2_control_manager`. Use executor from the ros2_control_node.

destogl · 2020-10-02T13:30:56Z

I am glad to anounce that I resolved all the issues I had and this very-first, simple ros2_control implementaiton with components is working!

There is one simple functionality behind it: "Just load the SystemHardware and robot's Joints from the URDF description, start contoller defined in YAML file and read-update-write data between the hardware and controller".

The goal is to demonstrate the idea behind the components.
@AndyZe @v-lopez @jordan-palacios @dignakov this could be interesting for you.

destogl · 2020-10-02T13:31:35Z

The tests are missing!

controller_interface/CMakeLists.txt

controller_interface/include/controller_interface/controller_interface.hpp

controller_interface/src/controller_interface.cpp

controller_manager/CMakeLists.txt

hardware_interface/include/hardware_interface/resource_manager.hpp

hardware_interface/src/resource_manager.cpp

Karsten1987 · 2020-10-05T20:50:52Z

hardware_interface/src/resource_manager.cpp

+      command_interfaces_[joint_info.name] = joint->get_command_interfaces();
+      // TODO(anyone): add checking if Joint has state interfaces at all
+      state_interfaces_[joint_info.name] = joint->get_state_interfaces();
+      joint_components_[joint_info.name] = joint;


why do we need this? I don't really see a strong need to know that mapping. But even if, why not just storing the instance of "hardware_info_list" ?

The controllers are accessing the joints using their names so using the names instead of hardware_info cannot be directly searched.

hardware_interface/test/test_component_interfaces.cpp

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

controller_manager/src/controller_manager.cpp

v-lopez · 2020-10-06T12:17:50Z

hardware_interface/include/hardware_interface/resource_manager.hpp

+  std::map<std::string, std::shared_ptr<
+      hardware_interface::components::Joint>> joint_components_;
+  std::map<std::string, std::vector<std::shared_ptr<
+      hardware_interface::components::Joint>>> joint_components_for_hardware_;
+  std::map<std::string, std::shared_ptr<
+      hardware_interface::components::Sensor>> sensor_components_;
+  std::map<std::string, std::vector<std::shared_ptr<
+      hardware_interface::components::Sensor>>> sensor_components_for_hardware_;
+
+  /**
+   * Map of joints and their command interfaces
+   */
+  std::map<std::string, std::vector<std::string>> command_interfaces_;
+
+  std::map<std::string, std::vector<std::string>> claimed_command_interfaces_;


And if needed, consider using a unordered_map for speed.

…l/ros2_control into add_ros2_control_managers

…ollerManager's update(). Resource manager is using unodered_map

destogl · 2020-10-07T08:57:52Z

I'm looking at this in tandem with: ros-controls/ros2_control_demos#37. I did manage to get everything to run, but I ran into an issue along the way.

Thanks for the testing!

launch.substitutions.substitution_failure.SubstitutionFailure: package 'controller_manager' found at '/home/dmitri/ws_colcon/install/controller_manager', but libexec directory '/home/dmitri/ws_colcon/install/controller_manager/lib/controller_manager' does not exist

We are look into this in #177

launch.substitutions.substitution_failure.SubstitutionFailure: executable 'ros2_control_node' not found on the libexec directory '/home/dmitri/ws_colcon/install/controller_manager/lib/controller_manager'

This should be resolved now. Thanks!

v-lopez · 2020-10-08T10:28:37Z

@destogl I was going to start looking at the tests, ilke you said we need a robot description. How do you think it's best to implement it? I was copying part of https://github.com/ros-controls/ros2_control_demos/tree/add_rrbot_system_position_joints/ros2_control_demo_robot/description into the test_robot_hardware, and use that in the tests.

Any other ideas?

guru-florida · 2020-10-12T05:25:35Z

I am heaving an issue with the logic change from this PR, specifically the loading of controllers configuring themselves in load_controller(name, type) before being ready. JointTrajectoryController, for example, is now being configured before it is getting it's parameters and it then complains about empty joint_names. Previously, there was configure() and activate() method that would be called after the controllers were loaded and parameters set.

I am working on getting @ahcorde's work on gazebo_ros2_control updated. In the plugin's initialization it creates the ControllerManager, then calls the load_controller(name, type) which calls load_controller_impl internally, which calls on_configure() on the controller instance. Inside the on_configure of JointTrajectoryController is where it looks for joints parameters but no yaml has been loaded at this point.

There is no longer a way to configure a controller loaded via pluginlib anymore. How would these loaded controllers initialize with their yaml under the new changes? Perhaps I am totally doing it wrong now, There is a note by @ahcorde that his load-controller-parameters code should be updated when "spawn functionality".

destogl · 2020-10-12T21:06:46Z

@destogl I was going to start looking at the tests, ilke you said we need a robot description. How do you think it's best to implement it? I was copying part of https://github.com/ros-controls/ros2_control_demos/tree/add_rrbot_system_position_joints/ros2_control_demo_robot/description into the test_robot_hardware, and use that in the tests.

Any other ideas?

You can take this description of any other description from the roadmap. @AndyZe also uses them in #167. Therefore, it would probably make sense to put them into test_robot_hardware package as a header file, so we can just use them on multiple places.

destogl · 2020-10-12T21:15:29Z

I am heaving an issue with the logic change from this PR, specifically the loading of controllers configuring themselves in load_controller(name, type) before being ready. JointTrajectoryController, for example, is now being configured before it is getting it's parameters and it then complains about empty joint_names. Previously, there was configure() and activate() method that would be called after the controllers were loaded and parameters set.

We changed the logic in #139. There we decided to go almost the same way, from the logic perspective as in ROS1.

I am working on getting @ahcorde's work on gazebo_ros2_control updated. In the plugin's initialization it creates the ControllerManager, then calls the load_controller(name, type) which calls load_controller_impl internally, which calls on_configure() on the controller instance. Inside the on_configure of JointTrajectoryController is where it looks for joints parameters but no yaml has been loaded at this point.

YAML should actually be loaded before starting any of those function, so before even calling load_controller function. Can you open a new issue regarding this, since it is not really relevant to this PR. There you could also explain you test example/approach.

There is no longer a way to configure a controller loaded via pluginlib anymore. How would these loaded controllers initialize with their yaml under the new changes? Perhaps I am totally doing it wrong now, There is a note by @ahcorde that his load-controller-parameters code should be updated when "spawn functionality".

If you look the manual linked in the first post you can find working example of controller and hardware.

destogl · 2020-10-29T13:35:17Z

Closing in favor of #216, as discussed in Meeting on 21st of October 2020.

destogl added 2 commits September 14, 2020 13:18

Add resource manager.

80d32e3

Add ros2_control_node and main manager for the components Concept

08a9623

destogl marked this pull request as draft September 15, 2020 10:20

destogl self-assigned this Sep 15, 2020

destogl added the enhancement label Sep 15, 2020

destogl mentioned this pull request Sep 23, 2020

Update controller_manager CMakeLists #156

Closed

destogl and others added 5 commits September 26, 2020 18:32

ResourceManager move to hardware_interface package because of depende…

afc2068

…ncy collision. ControllerManager prepared to initialize a controller with components.

ResourceManager: interface check and handles claiming initial impleme…

d36b5c5

…ntation

Joint and Sensor have now virtual memeber to enable overriding. Resou…

74de76c

…rce Manager give aditional error output.

ControllerManagerNew add missing functions. Control Manager can initi…

d88a734

…alize and use controllers. ResourceManager is now smart pointer. Enable undeclared paramters for the node.

Merge branch 'master' into add_ros2_control_managers

8ef26d8

destogl mentioned this pull request Oct 1, 2020

Forward Controller using Interfaces (ResourceManager) ros-controls/ros2_controllers#99

Merged

destogl added 3 commits October 1, 2020 13:12

Correct build.

834343f

Revert executor namespace and add compile definition to disable boost…

a124bb5

… functions

Added LifecycleNode name output

8b2de3f

bmagyar self-requested a review October 2, 2020 06:42

v-lopez mentioned this pull request Oct 2, 2020

Add cli interface #176

Merged

destogl added 2 commits October 2, 2020 14:50

Add the test where LifecycleNode name is checked.

9e0573b

Enable undeclared parameters for the controller's LifecycleNode s. Renable controller_manager tests. Clean and extend `ros2_control_manager`. Use executor from the ros2_control_node.

Apply linters and uncrustify

fb003db

destogl marked this pull request as ready for review October 2, 2020 13:30

destogl requested review from Karsten1987 and v-lopez October 2, 2020 13:32

Use foxy uncrustify

a70dc60

This was referenced Oct 2, 2020

Fix compile errors ros-controls/ros2_control_demos#39

Closed

Demo not work ros-controls/ros2_control_demos#42

Closed

Review how to provide controller parameters #168

Closed

Added joint_limits_interface - Direct port from ROS 1 #135

Closed

destogl and others added 3 commits October 5, 2020 21:33

Merge branch 'master' into add_ros2_control_managers

61d0405

Refractoring and cleaning

8292c4f

Small build correction

775e0e6

Karsten1987 reviewed Oct 5, 2020

View reviewed changes

bmagyar and others added 3 commits October 6, 2020 09:36

Update hardware_interface/src/resource_manager.cpp

6b3ee94

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

Throw execption from the contstructor

94ae85d

Change namespace of ResourceManager

bbe0df0

This was referenced Oct 6, 2020

Realtime safe access to read & write components #174

Closed

ResourceManager should be part of the controller_manager package #183

Closed

Remove BOOST compiler defintions from CMakeLists.txt files in rolling/galactic #184

Closed

v-lopez reviewed Oct 6, 2020

View reviewed changes

destogl added 4 commits October 6, 2020 14:55

Merge branch 'add_ros2_control_managers' of https://github.com/destog…

f86a7d4

…l/ros2_control into add_ros2_control_managers

Revert controller_loader_plugin_interface. Add more checks into Contr…

3887008

…ollerManager's update(). Resource manager is using unodered_map

Include reviews

0eda72a

Rever spin changes because multi-threading does not work otherwise

135278a

destogl mentioned this pull request Oct 7, 2020

User unique_ptr in ResourceManager #186

Closed

Make the rest of the reviews in

8495d0a

destogl mentioned this pull request Oct 14, 2020

Tests for the ResourceManager in #147 #196

Closed

7 tasks

Merge branch 'master' into HEAD

b0f842e

destogl mentioned this pull request Oct 21, 2020

Update ci ros-controls/ros2_controllers#104

Merged

destogl mentioned this pull request Oct 28, 2020

Ros2 control node #216

Closed

destogl closed this Oct 29, 2020

destogl deleted the add_ros2_control_managers branch October 29, 2020 13:37

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add ros2 control managers #147

Add ros2 control managers #147

destogl commented Sep 15, 2020 •

edited

Loading

destogl commented Oct 1, 2020

destogl commented Oct 2, 2020 •

edited

Loading

destogl commented Oct 2, 2020

Karsten1987 Oct 5, 2020

destogl Oct 7, 2020

v-lopez Oct 6, 2020

destogl commented Oct 7, 2020

v-lopez commented Oct 8, 2020

guru-florida commented Oct 12, 2020

destogl commented Oct 12, 2020

destogl commented Oct 12, 2020

destogl commented Oct 29, 2020

Add ros2 control managers #147

Add ros2 control managers #147

Conversation

destogl commented Sep 15, 2020 • edited Loading

How to test: ros2_control with components

destogl commented Oct 1, 2020

destogl commented Oct 2, 2020 • edited Loading

destogl commented Oct 2, 2020

Karsten1987 Oct 5, 2020

Choose a reason for hiding this comment

destogl Oct 7, 2020

Choose a reason for hiding this comment

v-lopez Oct 6, 2020

Choose a reason for hiding this comment

destogl commented Oct 7, 2020

v-lopez commented Oct 8, 2020

guru-florida commented Oct 12, 2020

destogl commented Oct 12, 2020

destogl commented Oct 12, 2020

destogl commented Oct 29, 2020

destogl commented Sep 15, 2020 •

edited

Loading

How to test: `ros2_control` with components

destogl commented Oct 2, 2020 •

edited

Loading