WIP: Implementation of components for controlling robots by destogl · Pull Request #101 · ros-controls/ros2_control

destogl · 2020-06-12T09:31:04Z

This PR replaces #80 .

There are a few things to consider:

A package name is work-in-progress. I am not (yet?) comfortable with having hardware word in the name since the components should be completely hardware agnostic and provide the abstraction for controller and controller manager.
Currently, I am planning to add Robot-Component inside RobotHardware. This component should take care of parsing the urdf and gluing sensors and actuators. In the final version, there should be another way around, i.e. Component has reference to the Hardware. Nevertheless, starting as proposed in this PR should reduce number of braking changes and hopefully simplify integration.

hardware_interface/include/hardware_interface/robot_hardware.hpp

hardware_interface/src/robot_hardware.cpp

robot_control_components/CMakeLists.txt

Karsten1987 · 2020-06-12T17:26:57Z

robot_control_components/include/robot_control_components/ros2_control_utils.hpp

+{
+// Classes
+template < typename T >
+class ROS2ControlLoaderPluginlib


I'd propose to leave out the plugin loading functionality as for now until we've figured out an API for the components.

I added loading of Hardware already in Demo repository, and we will need this very soon for Sensors and Actuators.

robot_control_components/include/robot_control_components/sensor.hpp

robot_control_components/include/robot_control_components/simple_component.hpp

robot_control_components/test/test_dummy.cpp

olivier-stasse

Dear @destogl thanks again for this second iteration.
As a general feedback, I understand better the framework. Mimizing the dependency to the middleware is for me a great force of ros_control. Making it also in ros2_control would be super nice.
As far as I understood and IMHO the message targeted for the controllers should not be at the level of the Robot class. But their C++ mapping are definitely needed.

Thanks again !

olivier-stasse · 2020-06-16T15:41:36Z

robot_control_components/include/robot_control_components/robot.hpp

+#include <vector>
+
+#include "control_msgs/msg/dynamic_joint_state.hpp"
+#include "control_msgs/msg/interface_value.hpp"


@destogl I am not sure that we want to put straight at this level the middleware.
In the ROS1 interface the middleware was present only for loading and unloading the controllers not at the robot level.
Right now I do not see the functionnal need to have it here.

IMHO if you want to have a robot compound through a middleware (OpenRTM, Micro-DDS) it should be implemented in another object.

To precise: dynamic_joint_state and interface_value are a convenient way to communicate with the controllers but to be agnostic they could be map to std::map<> and std::vector<> containers in the C++ space of the robot.

I agree to do this if there is a use case for it

olivier-stasse · 2020-06-16T15:43:18Z

robot_control_components/include/robot_control_components/robot.hpp

+#include "control_msgs/msg/dynamic_joint_state.hpp"
+#include "control_msgs/msg/interface_value.hpp"
+
+#include "rclcpp/macros.hpp"


The convenient macros to handle the shared pointers could be exposed to avoid a dependency to rclcpp for this specific object.

olivier-stasse · 2020-06-16T15:51:04Z

robot_control_components/include/robot_control_components/robot.hpp

+#include "control_msgs/msg/interface_value.hpp"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/rclcpp.hpp"


Could you clarify why rclcpp is needed ?

it will be used for logging.

robot_control_components/package.xml

olivier-stasse · 2020-06-16T15:58:21Z

robot_control_components/src/robot.cpp

+{
+  //TODO: Add catch
+  try {
+    ComponentInfo robot_info = parse_robot_from_urdf(urdf_string);


I think that this is very nice idea, but maybe not at the right place.

Where would you put it?

destogl · 2020-06-16T16:39:56Z

As far as I understood and IMHO the message targeted for the controllers should not be at the level of the Robot class. But their C++ mapping are definitely needed.

If you asked me few years ago I would be with you on this one, nevertheless always when I separate this I ended up with doubling code, but still using it in ROS...

Is anybody using ros_control outside ROS? And what are exactly issues including ROS-main headers? I understand your reasoning and we should clarify this for sure. I am also interesting if there is really a use case where you cannot afford to load a few ROS header (some microcontrollers or something).

Karsten1987 · 2020-06-16T16:56:59Z

And what are exactly issues including ROS-main headers?

Having small dependencies is always desirable for various reasons. But I guess we have to distinguish here between a strong dependency to something like a rclcpp::Node and a header for convenience functions.
The biggest argument for me is testability. I don't want to set up a complete ROS network to test my camera image or some controller behavior. Mocking a node is hard, mocking some primitive parameter values is easy.

destogl · 2020-06-16T17:08:29Z

OK, I agree, should we then remove completely dependency to rclcpp? I find it important for logging functions...

destogl · 2020-06-16T19:39:03Z

@Karsten1987 I added interfaces and initial implementations of sensors and actuators. For this simple data, I believe we should make data-management directly in Sensor and Actuator interfaces since it will be shared functionality by many user components.

Is this a good direction? Did you have another concept in mind?

olivier-stasse · 2020-06-16T20:01:19Z

I do agree that logging is important for real-time system. For this very reason, some implementation are carefully crafted to meet real-time constraints.

My only point is that the object Robot should be as lean as possible, and then derived class, or class matching polymorphic use offers various design patterns. (Thanks for the link @Karsten1987)

For more details you can see:
RTRobMultiAxisControl: A Framework for Real-Time Multiaxis and Multirobot Control
H Fischer, M Vulliez, P Laguillaumie, P Vulliez, JP Gazeau
IEEE Transactions on Automation Science and Engineering 16 (3), 1205-1217
for reat time evaluation. It might have changed.

destogl · 2020-06-17T07:51:36Z

@olivier-stasse @Karsten1987

in the meeting, I understood that we should not use ROS2 logging infrastructure. So what are alternative? Do we need to implement some soft of real-time logger?

olivier-stasse · 2020-06-18T12:16:41Z

@destogl For the logging infra-structure this is something which can be illustrate in the ros2_control_demos repository. PAL-robotics has a package called dynamic_introspection I believe.

destogl · 2020-06-22T11:11:10Z

As described in destogl/ros2-control#1 there are some still things to clarify.

The main open questions are:

How resources (actuators) are claimed?
Which type of pointers should be used in read and write functions? (doubles or more complex data)
Do we need separate functions for Accessing actuators data from Hardware and Controllers? E.g. set_command (from controllers), set_current_value (from Hardware)

And TODO:

add constant with common interface types (for now: position, velocity, effort)

Karsten1987 · 2020-06-23T16:42:03Z

I was thinking a bit more about the resource management and came up with the following diagram:

The "Resource Manager" loads the actuators/sensors/systems from the URDF and each component registers their handles. That is, each component can (statically) allocate the data storage it needs and give access to it via the handle.
The resource manager can then "loan" a handle to individual controllers. This loaning of resources can be done quite transparently and ensures that each resource is only accessible via one controller. So we have a 1:1 relationship here. I guess we could have a 1:N relationship for sensor data and have the sensor handle access the data read-only. But I consider this details for now.
Whenever a controller is done (either via a call to shutdown or going out of scope) that loaned handle is returned and made available for other controllers to borrow.

The registered resources are fixed on runtime, making it hard to reload new components but I guess that's okay and usually the hardware is fixed within a setup - opposite to controllers, where we would like to dynamically load/start/stop them.

destogl · 2020-06-25T11:23:14Z

@Karsten1987 thanks for sharing your idea. This sounds sensible to me this means that our "Container" would actually be RessourceManager. In this way we do not need robots to "have" other robots, but they would be composed over RessourceManager? (At the moment I do not see problems with this approach).

I would like that we get more concrete on the point how RessourceManager is started. Is this an objects in ControllerManager? Or how you imagine this?

destogl · 2020-06-25T11:25:56Z

The building is failing on test_robot_hardware package so I will wait for #109 before trying to repair it.

bmagyar

couple of styling comments going forward

hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp

hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp

hardware_interface/include/hardware_interface/utils/ros2_control_utils.hpp

robot_control_components/src/effort_actuator.cpp

robot_control_components/src/position_actuator.cpp

hardware_interface/include/hardware_interface/actuator.hpp

bmagyar · 2020-06-26T06:41:04Z

hardware_interface/src/utils/component_parser.cpp

+
+namespace
+{
+constexpr const auto kRobotTag = "robot";


Is it just me, being the hungarian who doesn't like hungarian notation? We also aren't using it anywhere else on the codebase (thank god) but only for some constants. Do we even need this? I know some people don't like shouty capital-case constants but they are not that bad nor would it be horrific to simply drop the gibberish from these variable names and go with robot_tag in this instance...

@Karsten1987 ?

that's the Google style guide. We've been using it in the ROS2 code base as such.

Should I change something here?

please just stay consistent at least. You've been using constants in three different places, all with their own style :D

v-lopez · 2020-06-26T06:55:32Z

@destogl For the logging infra-structure this is something which can be illustrate in the ros2_control_demos repository. PAL-robotics has a package called dynamic_introspection I believe.

@olivier-stasse It was rewritten as https://github.com/pal-robotics/pal_statistics but, it is still ROS1 only and it's useful for publishing numerical data in a RT safe way, not messages.

So not useful here right now I believe.

destogl · 2020-06-29T13:10:11Z

Before I do some more coding, first 1-2 questions.

@Karsten1987: Can you look at the ResourceManager? Is this along with your proposition?

What would be the best way to manage hardware in ResourceManger? At the current point, we would need to crate to maps, one with hardware and other with components, and then give hardware the reference to the components. This would create some additional interfaces... @Karsten1987 what was your idea to solve this? What is the purpose of SensorInterface is this representation of hardware? (I just want to check that we are thinking int he same direction...)

hardware_interface/include/hardware_interface/actuator.hpp

hardware_interface/include/hardware_interface/component_info.hpp

Karsten1987 · 2020-06-29T23:58:01Z

hardware_interface/include/hardware_interface/component_info.hpp

+  std::string name;
+  std::string type;
+  std::string class_type;
+  std::string joint;
+  std::vector<std::string> interface_names;
+  std::unordered_map<std::string, std::string> parameters;
+  std::vector<ComponentInfo> subcomponents;
+
+  std::string hardware_class_type;
+  std::unordered_map<std::string, std::string> hardware_parameters;


this should be limited to what's currently being used in this PR.
I also strongly advise against having a recursive data structure here. A component info should be a POD, with a flat structure and describing one specific component (either actuator or sensor).

OK. I added than SystemInfo to realize structural dependencies in URDF. is this OK? I extended test URDF and tests to support hardware class types in components.

hardware_interface/include/hardware_interface/resource_manager.hpp

hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp

hardware_interface/include/hardware_interface/utils/ros2_control_utils.hpp

hardware_interface/src/resource_manager.cpp

Karsten1987 · 2020-06-30T00:03:33Z

hardware_interface/src/utils/component_parser.cpp

+
+namespace
+{
+constexpr const auto kRobotTag = "robot";


that's the Google style guide. We've been using it in the ROS2 code base as such.

robot_control_components/src/effort_actuator.cpp

…URDF. Removed recursion from ComponentInfo.

destogl · 2020-07-04T21:51:24Z

@Karsten1987 @bmagyar I hope I addressed all your comments. is build failing because the master is also failing?

Karsten1987 · 2020-07-06T15:48:37Z

Looking at the CI output, it seems like two tests are failing:

  [5.922s] The following tests FAILED:
  [5.922s] 	  3 - test_component_parser (Failed)
  [5.922s] 	  8 - uncrustify (Failed)
  [5.922s] Errors while running CTest

The return code for the test_component_parser is -11 which regularly indicates a segmentation fault. You might want to look into these once more.

destogl · 2020-07-07T11:25:01Z

I was thinking a bit more about the resource management and came up with the following diagram:

I agree with the diagram and have a few questions...

We lost the word "Robot" from here. Will this confuse users?
What about Actuators/ActuatorHandles? I am not sure this will work smooth with transmissions. Maybe we should have reference to Transmissions and Actuators in the RessourceManager?

The resource manager can then "loan" a handle to individual controllers. This loaning of resources can be done quite transparently and ensures that each resource is only accessible via one controller. So we have a 1:1 relationship here. I guess we could have a 1:N relationship for sensor data and have the sensor handle access the data read-only. But I consider this details for now.

I agree.

The registered resources are fixed on runtime, making it hard to reload new components but I guess that's okay and usually the hardware is fixed within a setup - opposite to controllers, where we would like to dynamically load/start/stop them.

This is OK for now. I can only imagine that this registering resources at runtime would be interesting for tool changers. Nevertheless, this could be extended in the future.

jordan-palacios · 2020-07-08T10:54:58Z

What about Actuators/ActuatorHandles? I am not sure this will work smooth with transmissions. Maybe we should have reference to Transmissions and Actuators in the RessourceManager?

Given that robot_hardwarealready holds the Joints and Actuators handles, adding Transmissions there seems like the sensible thing to do.

As @Karsten1987 mentioned, I think focusing on the Joints and Sensor interfaces should be the only scope of the current PR. The ResourceManager and any other feature should come later.

…om/destogl/ros2_control into hardware_control_components-design

destogl · 2020-07-14T19:33:10Z

Hi guys, I renamed Actuator to Joint. I still have problems with some tests and I am not able to find the error. In the test node, everything works fine...

Can somebody help me with the output of the tests? I tried using the following command:

colcon test --packages-select hardware_interface --ctest-args -VV -R test_component_parser --output-on-failure

but there is no additional output. Therefore it is very hard to pin-point the error(s).

olivier-stasse · 2020-07-14T21:37:02Z

@destogl From the logs of the github action run

The output is:

   Starting >>> rclcpp_lifecycle
  --- output: hardware_interface
  [0.363s] CMake Error at CMakeLists.txt:112:
  [0.363s]   Parse error.  Expected "(", got right paren with text ")".
  [0.363s] 
  [0.363s] 
  [0.363s] -- Configuring incomplete, errors occurred!
  ---
  --- stderr: hardware_interface
  CMake Error at CMakeLists.txt:112:
    Parse error.  Expected "(", got right paren with text ")".

it seems to come from a problem with rclcpp_lifecycle.
I was a bit surprise too but it seems that the colcon-mixin is actually fetching the dependencies and then compile all of them. This is quite expensive.

What I do not get it that CMakeLists.txt:112 seems to be a comment in rclcpp_lifecycle CMakeLists.txt:
https://github.com/ros2/rclcpp/blob/e3490a29cd076bff320f6b5f02b013eb93c8d29d/rclcpp_lifecycle/CMakeLists.txt#L112
Not sure that helps.

Karsten1987

are you planning on adding tests to the APIs introduced here? There's quite some new stuff in here which doesn't have any corresponding test.

Karsten1987 · 2020-07-17T23:23:51Z

hardware_interface/include/hardware_interface/component_info.hpp

+  /**
+   * \brief (optional) key-value pairs for components hardware.
+   */
+  std::unordered_map<std::string, std::string> hardware_parameters;


what's the difference here to parameters?

parameters are for component and hardware_parameters are hardware-specific stuff. It could be that in some cases one have the same parameter for the component but uses different hardware/interface.

Karsten1987 · 2020-07-17T23:24:22Z

hardware_interface/include/hardware_interface/component_info.hpp

+/**
+  * \brief constants for types of components.
+  */
+constexpr const auto robotType = "robot";


should this be systemType?

Not really. Those types are for definitions for ResourceManager to get correct type from type property when initializing components.

not sure I understand this.

Karsten1987 · 2020-07-17T23:25:53Z

hardware_interface/include/hardware_interface/component_info.hpp

+  /**
+   * \brief list of subcomponents in the system, i.e., list of sensors and actuators.
+   */
+  std::vector<ComponentInfo> subcomponents;


I still don't understand why we need this. Can a system not be treated as a single black-box component?

According to your figure yes. You are right, this is not the best way, at least it shouldn't be placed inside SystemInfo type. Nevertheless, we need one top "container" which corresponds to the robot tag in URDF, as there could be System, Sensor and Joints defined, and they are like "subcomponents" of this robot.

The SystemInfo here represents exactly this robot level in URDF. IMHO it is better name than RobotInfo since one can define a good deal of different stuff with URDF, not only robots.

I hope you got my idea, and if you have any proposal, I would be happy to discuss it.

fair enough. I am cross-posting here as I just went through #121 but I think we definitely need a piece of URDF here which dictates how the individual components as well as the systems are supposed to be specified.

hardware_interface/include/hardware_interface/robot_hardware.hpp

Karsten1987 · 2020-07-17T23:30:55Z

hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp

+constexpr const auto HW_IF_POSITION = "position";
+constexpr const auto HW_IF_VELOCITY = "velocity";
+constexpr const auto HW_IF_EFFORT = "effort";


why do we need these values? I thought they'd be defined as constants in the control_msgs?

This is not relevant in #121 anymore.

what does that mean for this PR?

hardware_interface/src/robot_hardware.cpp

Karsten1987 · 2020-07-17T23:32:29Z

hardware_interface/src/utils/component_parser.cpp

+
+namespace
+{
+constexpr const auto kRobotTag = "robot";


please just stay consistent at least. You've been using constants in three different places, all with their own style :D

Karsten1987 · 2020-07-17T23:33:30Z

robot_control_components/package.xml

+  <description>The main package for `ros2_control`-concept testing. The package implements the most important classes and tests them with `demo_robot` to enable functionality and test driven development.</description>
+  <maintainer email="denis@stogl.de">Denis Štogl</maintainer>
+  <license>Apache License 2.0</license>
+


Suggested change

destogl · 2020-07-27T17:47:06Z

are you planning on adding tests to the APIs introduced here? There's quite some new stuff in here which doesn't have any corresponding test.

I will add tests in the follow-up PRs starting with #121 with small example.

Karsten1987 · 2020-07-28T20:26:02Z

@destogl what's the state on this PR here?

destogl · 2020-07-28T20:42:19Z

It can be closed. Everything is moved in #121, #122, #123

* joint velocity controller initial class version with tests * Set velocity to zero when deactivating controller * Renaming to joint_group_velocity_controller * Default initialize logger * Improved error message log

destogl mentioned this pull request Jun 12, 2020

WIP: New robot hardware interface design #80

Closed

destogl changed the title ~~Implementation of components for controlling robots~~ WIP: Implementation of components for controlling robots Jun 12, 2020

Karsten1987 reviewed Jun 12, 2020

View reviewed changes

destogl mentioned this pull request Jun 16, 2020

Make Robot class as main Container used from controller_manager #106

Closed

olivier-stasse reviewed Jun 16, 2020

View reviewed changes

This was referenced Jun 16, 2020

Add functionality of Robot-Container #108

Closed

[ros2_control_core] Create virtual components ros-controls/ros2_control_demos#7

Closed

Create *Hardware-structures ros-controls/ros2_control_demos#11

Closed

Karsten1987 force-pushed the hardware_control_components-design branch from 54165d3 to aa885a2 Compare June 16, 2020 20:42

destogl mentioned this pull request Jun 17, 2020

Structure of hardware_interfaces ros-controls/roadmap#10

Closed

3 tasks

destogl mentioned this pull request Jun 22, 2020

move interfaces into hardware_interface package destogl/ros2_control#1

Merged

bmagyar requested changes Jun 25, 2020

View reviewed changes

bmagyar reviewed Jun 26, 2020

View reviewed changes

hardware_interface/include/hardware_interface/actuator.hpp Outdated Show resolved Hide resolved

bmagyar reviewed Jun 26, 2020

View reviewed changes

bmagyar mentioned this pull request Jun 26, 2020

Add ActuatorHandle and Implement string-based interface handle-handling using DynamicJointState message #112

Merged

destogl mentioned this pull request Jun 29, 2020

Change Hardware return type to enum class #114

Merged

Karsten1987 reviewed Jun 30, 2020

View reviewed changes

destogl added 6 commits July 4, 2020 23:07

Added SystemInfo to implement structural dependency of components in …

191b15d

…URDF. Removed recursion from ComponentInfo.

hardware_interface_ret_t -> return_type

cf9ea6c

Added documentation for component_info.hpp

6af1296

Correct repo name in the licence headers

488b90d

Remove default constructor/destructor from cpp

b88faec

correct style

c59575a

destogl requested a review from bmagyar July 4, 2020 21:50

destogl requested a review from Karsten1987 July 4, 2020 21:51

destogl and others added 5 commits July 9, 2020 22:30

Merge branch 'master' into hardware_control_components-design

3d4b282

rename actuator -> joint

cdbb2cc

Correct parser bug with type attribute

5b7919d

Component parser correction

075312a

Merge branch 'hardware_control_components-design' of https://github.c…

9f24c3d

…om/destogl/ros2_control into hardware_control_components-design

Correct lint check

355048d

Correct typo

e0aa6f3

This was referenced Jul 15, 2020

Hardware component interfaces #121

Merged

Robot control components #123

Closed

Add component parser #122

Closed

Karsten1987 reviewed Jul 17, 2020

View reviewed changes

destogl closed this Jul 29, 2020

Conversation

destogl commented Jun 12, 2020

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

olivier-stasse left a comment

Choose a reason for hiding this comment

Uh oh!

olivier-stasse Jun 16, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

olivier-stasse Jun 16, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

destogl commented Jun 16, 2020

Uh oh!

Karsten1987 commented Jun 16, 2020

Uh oh!

destogl commented Jun 16, 2020

Uh oh!

destogl commented Jun 16, 2020

Uh oh!

olivier-stasse commented Jun 16, 2020

Uh oh!

destogl commented Jun 17, 2020

Uh oh!

olivier-stasse commented Jun 18, 2020

Uh oh!

destogl commented Jun 22, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Karsten1987 commented Jun 23, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

destogl commented Jun 25, 2020

Uh oh!

destogl commented Jun 25, 2020

Uh oh!

bmagyar left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bmagyar Jun 26, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

olivier-stasse Jun 16, 2020 •

edited

Loading

olivier-stasse Jun 16, 2020 •

edited

Loading

destogl commented Jun 22, 2020 •

edited

Loading

Karsten1987 commented Jun 23, 2020 •

edited

Loading

bmagyar Jun 26, 2020 •

edited

Loading

olivier-stasse commented Jul 14, 2020 •

edited

Loading