Provide force-torque sensor data through gz_system to controller_manager

[SyllogismRXS]

This PR provides force-torque sensor data to Gazebo's controller_manager in a similar way to how the IMU data is currently provided. I made this change in order to test the ros2_controllers' admittance_controller in Gazebo/Ignition 6.16.0.

[ahcorde]

include <array>

[ahcorde]

include <cstddef>

[Nibanovic]

Rebased to master and tested with gazebo Harmonic on ros2 rolling

Can confirm everything works as expected:

• instantiated force_torque_sensor in URDF
• ran gazebo + ros2_control

state_interfaces set up as expected.

• connected force_torque_sensor_broadcaster controller to those state_interfaces

Echoed wrench topic shows the same values as gazebo sensor topic.

In my opinion, this feature can be merged after rebasing branch to master

[panagelak]

@ahcorde i have tested that this works also in ros2 humble, also supporting the state interfaces that are used by the ur scaled joint trajectory controller would be nice. I think this is a useful feature that needs to be merged on the relevant branches soon : )

[Nibanovic]

@ahcorde i have tested that this works also in ros2 humble, also supporting the state interfaces that are used by the ur scaled joint trajectory controller would be nice. I think this is a useful feature that needs to be merged on the relevant branches soon : )

There is a gpio PR we've been working on, basically enables rudimentary gpio use in gazebo. Exposes state as state + command, and mimics command to state. This may be helpful for your use case?

[panagelak]

@ahcorde i have tested that this works also in ros2 humble, also supporting the state interfaces that are used by the ur scaled joint trajectory controller would be nice. I think this is a useful feature that needs to be merged on the relevant branches soon : )

There is a gpio PR we've been working on, basically enables rudimentary gpio use in gazebo. Exposes state as state + command, and mimics command to state. This may be helpful for your use case?

@Nibanovic hello, thanks for guiding me to this PR and for your quick reply.
(since for some reason i could not raise an issue to your fork, i will bastardize this issue to explain what i found about enabling the ur scaled controllers with ignition gazebo : ) )

So the "ur_controllers/SpeedScalingStateBroadcaster" controller has one 'state interface -> speed_scaling_factor' that the "ur_controllers/ScaledJointTrajectoryController" listens to it and one 'command interface -> target_speed_fraction_cmd' (also async) that is set by the io_and_status_controller, which in turns then changes the speed_scaling_factor.

So in order to make it work for my needs, i hard-coded change the value of the 'speed_scaling_factor' based on the 'target_speed_fraction_cmd' (instead of simply mocking the commands). (After finding state_i,state_j, command_i, command_j locations)

this->dataPtr->gpios_[state_i].states[state_j] = this->dataPtr->gpios_[command_i].commands[command_j];

Some other notes are that the "ur_controllers/GPIOController" needs to system_interface/initialized to be set to 1 to start. However i instead use a custom gpio controller of mine that simulates some dashboard services also like play and pause.

Now as you see this is an example, that mocking the commands does not work, if we want to simulate the controllers without changing their source code.. and it gets pretty complicated pretty fast to generalize that.

Should you provide better mapping options between commands and states?
Dynamically loaded plugins to map commands to states? yeah..

In any case with my hardcoded changes it works for my case : ) so thank you

PS. I also ended up registering command_iterfaces that the gpio controllers wanted directly (similar to how you did states) ( since they were complaining that they weren't existing ) - and were never registered since they were not state_interfaces.