Allocate memory for components and handles

[Karsten1987]

Third part of #164

implements the resource manager and the allocation for the hardware components and their handles.

sits on top of #223

[Karsten1987]

While working on the tests, I am still not totally convinced that this is the best way of initializing hardware.

To sketch the problem real quick:

in the URDF, we have something like:

...
<joint name="joint3"> // note the 3 here
  <command_interface name="velocity"/>
  <state_interface name="position"/>  // note the "position" here
  <state_interface name="velocity"/>
</joint>
<joint name="joint4">
   <command_interface name="velocity"/>
   <state_interface name="position"/>
   <state_interface name="velocity"/>
</joint>
...

The hardware implementation however has something like this:

std::vector<StateHandle> export_state_handles()
{
   std::vector<StateHandle> handles;
   // implementation is free to put in arbitrary values here
   // "joint3" turns into "joint1", "position" becomes "pos", order between velocity and position is changed.
   // same for "joint4"
   handles.push_back(StateHandle("joint1", "velocity", &velocity_ptr[0]));
   handles.push_back(StateHandle("joint1", "pos", &position_ptr[0])); <-- "position" turns into "pos"
   handles.push_back(StateHandle("joint2", "velocity", &velocity_ptr[0]));
   handles.push_back(StateHandle("joint2", "position", &position_ptr[0]));
}

The implementation is free to export its the data to its best knowledge and I even think that's preferred, because they'd know best what to export and what not.

What I propose here is that we introduce some sort of remapping. That is, even though the hardware implementation exports "joint1/pos", we can remap this via the URDF to "joint3/position".
That entails changes to the URDF once more in which we don't specify any command or state interfaces in the URDF and let the hardware take care of this. However, we introduce a new tag under each joint which remaps any handle.

<joint name="joint3">
  <remap from="pos" to="position"/>
</joint>
<joint name="joint4">
  <remap from="pos" to="position"/>
</joint>

The remapping for the actual joint name can be done implicitly by respecting the order of the handles. This yet brings the risk that we introduce a strict ordering of the joints within the URDF, but I believe that's doable.

With that remapping in place, the following handles would be exported:
joint1/velocity --> joint3/velocity
joint1/pos --> joint3/position
joint2/velocity --> joint4/velocity
joint2/pos --> joint4/position

What do you think? I am happy for any other suggestions.
/cc @bmagyar @destogl

[destogl]

The standard interfaces ("position", "velocity", "effort) I propose to define constants. See #140 (this file)[https://github.com/ros-controls/ros2_control/blob/6b0aca3b4d406147f1b527115c9140f2c018720f/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp].

Using those, we make life easier for the people using standard ros2_control stuff but still enable developers to do their own "proprietary" thing if they want to.

Although using remapping is interesting, it adds unnecessary complexity, especially with the specific order of interface. This could lead to many errors/unwanted behaviors.

[Karsten1987]

Using those, we make life easier for the people using standard ros2_control stuff but still enable developers to do their own "proprietary" thing if they want to.

That makes it easier for the interfaces, not for the actual joint names. I think it's a good idea to have these defaults, but it's still a string comparison in the end, where typos or deviations from the conventions can be made. In this case I believe a remapping makes things easier to reuse controller code.

Although using remapping is interesting, it adds unnecessary complexity, especially with the specific order of interface. This could lead to many errors/unwanted behaviors.

How would you remap the joints then? The idea of having the hardware components is that I can flexibly compose a new hardware setup with these components. That however also means that the joint names are different in each setup.
Imagine a system implementation for a kuka or abb, which has six joints. The internal joint names are - abb_joint1, abb_joint2, ... abb_joint6.
If you combine this system twice now for a dual arm setup, you'll have these joint names twice. How do you remap these to abb_joint1_left, ... abb_joint5_left?

We've quickly touched that with @bmagyar and the only option I see so far is to rely on the joint order specified in the URDF.
That is, the first joint listed in the hardware_info struct is implicitly mapped to abb_joint1. I personally don't like that tho as this implies a lot of insight knowledge upon the developers of these hardware components. Whereas if we implement an easy and central remapping, the implementation can basically deal as if they were a stock implementation.

[bmagyar]

I think that enforcing strict ordering in a hw component should be a design choice for those components, not part of the framework. The remapping idea is nice but I think we should still keep the original setup as it's super clear what's happening. Yes, ultimately it is the hardware that exports those handles but the resource manager and the rest of the framework shouldn't entirely be at the mercy of it. The state_interface and command_interface setup makes the contract between the component and the framework extremely clear. The implementation on the component side is left undefined which is fine, these can essentially be black boxes for all the framework is concerned - as long as it fulfils the contract. We can't have arbitrary 3rd party components mess everything up.

Your proposal enforces each component implementation to have a string-based internal representation which may not be necessary. Similarly, if you have string-based internals, you could support remapping with the existing setup by forwarding the relevant XML snippet to the specific component at hand. We - as in ResourceManager - can parse what we need from the URDF, the component itself can also read it and figure out what to do with it on it's own.

  <ros2_control name="RemapInTheURDF" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/RemapInTheURDFHW</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
        <maps_to>j4_pos</maps_to>
      </command_interface>
      <state_interface name="position">
        <maps_to>j4_pos_cmd</maps_to>
      </state_interface>
    </joint>
  </ros2_control>

For development, debug use and runtime introspection, we could extend the top level interface with a new function that'd print diagnostics in a pre-defined format, something along the lines of:

== component summary== 
state interfaces: ['joint_1/position', 'joint_2/position', 'temp1/celsius']
command interfaces: ['joint_1/position', 'joint_2/current']
internal remapping: # this can be entirely user-defined
['joint_1/position' -> can_data[2], 'joint_2/position' -> can_data[3], 'joint_1/position' -> can_data[0], 'joint_2/position' -> can_data[1] , 'temp1/celsius' -> usb serial messages]

Plus components could be encouraged to document their own configuration parameters for the URDF.

[codecov-io]

Codecov Report

Merging #207 into remodel_component_interfaces will increase coverage by 1.41%.
The diff coverage is 42.56%.

@@                       Coverage Diff                        @@
##           remodel_component_interfaces     #207      +/-   ##
================================================================
+ Coverage                         33.33%   34.74%   +1.41%     
================================================================
  Files                                44       50       +6     
  Lines                              2622     2855     +233     
  Branches                           1675     1783     +108     
================================================================
+ Hits                                874      992     +118     
- Misses                              256      287      +31     
- Partials                           1492     1576      +84     

Flag	Coverage Δ
unittests	34.74% <42.56%> (+1.41%)	⬆️

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

Impacted Files	Coverage Δ
...include/hardware_interface/components/actuator.hpp	100.00% <ø> (ø)
...rdware_interface/components/actuator_interface.hpp	100.00% <ø> (ø)
...e/include/hardware_interface/components/sensor.hpp	100.00% <ø> (ø)
...hardware_interface/components/sensor_interface.hpp	100.00% <ø> (ø)
...e/include/hardware_interface/components/system.hpp	100.00% <ø> (ø)
...hardware_interface/components/system_interface.hpp	100.00% <ø> (ø)
hardware_interface/src/component_parser.cpp	39.78% <ø> (+1.07%)	⬆️
...dware_interface/test/test_component_interfaces.cpp	28.18% <16.66%> (ø)
hardware_interface/test/test_joint_handle.cpp	33.33% <25.00%> (ø)
controller_manager/test/test_resource_manager.cpp	28.20% <28.20%> (ø)
... and 16 more

[Karsten1987]

I opted for a simple (and optional) validation function in order to validate that all specified command and state interfaces in the URDF are indeed existing after all hardware components are loaded.
If a given key is not available, the initialization step fails. If a component exports other, non-specified keys, these are simply being ignored.

this is ready for review in order to keep the diff somehow comprehensible.

[Karsten1987]

In order to push things forward, I'll go ahead and merge this. @bmagyar please feel free to open up a follow up ticket if you're requested changes were not appropriately addressed within here.

[bmagyar]

Thanks for the note, looks all good