Added GPIO parsing and test

[mahaarbo]

Adds support for non-joint components named gpio in the urdf, following the discussion from #318.

The code can be tested with ros2_control_demos/with_gpio.

How to run

1. Launch the hardware ros2 launch ros2_control_demo_robot rrbot_system_with_sensor_and_gpio.launch.py
2. In a new terminal load the controller ros2 control load_controller --set-state start forward_gpio_controller
3. Publish some command to the GPIO analog output

ros2 topic pub /forward_gpio_controller/commands std_msgs/msg/Float64MultiArray "data:
- 0.1"

4. Watch the GPIO analog input and sensor react in the first terminal
5. Publish a command to reset the GPIO analog output command

ros2 topic pub /forward_gpio_controller/commands std_msgs/msg/Float64MultiArray "data:
- 0.0"

6. Watch the GPIO analog input and sensor react in the first terminal.

Comments

The hardware demo has three GPIO inputs, one sensor, and one GPIO output. The GPIO inputs and sensor readings are affected by the GPIO output, but decay to zero on their own.

The gpio interfaces are basically the same as the joint interfaces, and the thought is that generalizations can be done using something like the semantic components to easily claim multiple GPIO interfaces.

[destogl]

Just a comment for now, because I would like to clarify the structure here.

[olivier-stasse]

LGTM

[mahaarbo]

Added the size and data_type attribute to both the interface element and to the component element. There's a little bit of regex parsing to make it easier to write semantic components. This makes it possible to define:

<joint name="joint" size="2">
  <command_interface name="position"/>
  <state_interface name="position"/>
</joint>
<gpio name="flange_IOS">
  <command_interface name="digital_output" size="2" data_type="bool"/>
  <state_interface name="analog_input" size="0 1 2"/>
</gpio>
<sensor name="ft_" size="ee table">
  <state_interface name="force." size="x y z"/>
  <state_interface name="torque." size="x y z"/>
</sensor>

which makes:

• joint1 and joint2 joint components
• a flange_IOS GPIO component with 2 command interfaces, and 3 state interfaces (zero indexed because someone may want that)
• ft_ee and ft_table sensor components with 6 state interfaces named force.x, force.y, force.z

Can be tested with ros2_control_demos/with_gpio which now initializes the joints and GPIO with the size attribute. The data_type attribute still needs a new PR for making handles with bool and int.

[destogl]

I am not sure about all this changes... Some of them I find good, but some of them become very cryptic and implicit. Here exact comments:

<joint name="joint" size="2">
  <command_interface name="position"/>
  <state_interface name="position"/>
</joint>

I find this good. This could save much of copy-paste code for joints.

<gpio name="flange_IOS">
  <command_interface name="digital_output" size="2" data_type="bool"/>
  <state_interface name="analog_input" size="0 1 2"/>
</gpio>

Generally OK, but why there are two possibilities to specify size. It seems that in the second case the size is not "size" at all. Am I missing something?

<sensor name="ft_" size="ee table">
  <state_interface name="force." size="x y z"/>
  <state_interface name="torque." size="x y z"/>
</sensor>

I don't see much purpose to use this. At least not the first size argument. Why not simply explicitly define two sensors? IMHO this part only reduces readability and clarity.

For the second part this could be OK, but why to have specified ".". Although, I think that a better approach is to use semantic components in hardware interface for this.

[mahaarbo]

I find this good. This could save much of copy-paste code for joints.

Yup, and my thought was that not everyone name their joints joint1, joint2 etc, but rather some other set of suffixes.

Generally OK, but why there are two possibilities to specify size. It seems that in the second case the size is not "size" at all. Am I missing something?

First of all, I think having size on the interfaces themselves can be useful if we want to define a sensor to the end-effector pose for example, which contains 9 doubles. Specifying a size of 9 makes the parser create 9 interfaces. Or if one has a large set of digitally controlled valves attached at the end-effector. But I agree that I may have abused the "size" concept a bit. My idea was that what we ask for when we say size is just to specify a range for the suffix attached to the name of the interface, or to a set of components with different suffixes. So I made the argument either take a number to specify a range of suffixes ranging from 1 to the number specified, or to take a list of whitespace separated strings and apply them as suffixes. Perhaps a better wording would be "suffix_range" rather than "size". Maybe this is encroaching on xacro:macro territory though, and it's a quick fix to remove this functionality.

I don't see much purpose to use this. At least not the first size argument. Why not simply explicitly define two sensors? IMHO this part only reduces readability and clarity.

The idea was modularity with a 'suffix_range" concept, but perhaps the whole suffix range idea should have been made with xacro:macro instead.

For the second part this could be OK, but why to have specified ".". Although, I think that a better approach is to use semantic components in hardware interface for this.

The "." is specified since it's just suffixes. Maybe I've misunderstood the semantic components, they don't need anything to be specified in the URDF? Shouldn't everything in the hardware interface parameters be reflected in the URDF? Does that mean that parameters of a semantic component wouldn't be editable without recompiling the package?

[destogl]

My idea was that what we ask for when we say size is just to specify a range for the suffix attached to the name of the interface, or to a set of components with different suffixes.

I agree in general with your idea, but I am asking myself (and others) if this is too cryptic. Maybe we should use another argument for this...

The "." is specified since it's just suffixed. Maybe I've misunderstood the semantic components, they don't need anything to be specified in the URDF? Shouldn't everything in the hardware interface parameters be reflected in the URDF? Does that mean that parameters of a semantic component wouldn't be editable without recompiling the package?

The idea is the following. Semantic components provide some semantic to components defined in the URDF. (Currently is this only used on the controller side. See, for example, this line and this file). I imagine something similar on the hardware side. Especially, when using "standardized" interface names as in the linked constructor. So we would define semantic components something like:

"Standard" 6D - FTS

<gpio name="ee_fts" type="semantic_component/ForceTorqueSensor">
</gpio>

non-standard (less than 6D FTS) (using the second constructor - parsing similar as in FTS Broadcaster)

<gpio name="ee_fts" type="semantic_component/ForceTorqueSensor">
  <state_interface name="force.x"/>
  <state_interface name="torque.z"/>
</gpio>

Nevertheless, this is out of the scope of this PR.

For this PR, I propose to only add size argument to joint and gpio and type argument to gpio tag. I would not go on interface level to define types. I don't think this I needed for now. Especially because we can not support this in the framework.

[bmagyar]

I agree with Denis that the recent changes has gone a little beyond scope and are generally confusing.

IMO the xml should not direct how exactly those strings are created, it should not be so tightly tied to how we implemented interfaces and their handling.

I would recommend to only keep

<gpio name="flange_IOS">
  <command_interface name="digital_output" size="2" data_type="bool"/>
  <state_interface name="analog_input" size="3"/>
</gpio>

from the proposals in this PR as everything else is out of scope.

Let's discuss what's useful from the rest and how it is useful to incorporate another field instead of size, such as labels or similar.

[mahaarbo]

I would recommend to only keep

<gpio name="flange_IOS">
  <command_interface name="digital_output" size="2" data_type="bool"/>
  <state_interface name="analog_input" size="3"/>
</gpio>

from the proposals in this PR as everything else is out of scope.

This works now, and the size or data_type tags are only processed in the gpio elements. The size string is tested with regex since tinyxml2's QueryIntValue wrongly parses size="0 1 2" as 0. For joints and sensors, the data_type and size fields default to "double" and 1.

[destogl]

The nit-picks only. Can also be merged without those.