Add Imu and FT state interfaces

[v-lopez]

Add state interfaces for gazebo sensors.

This allows sensors from being read directly from ROS control without going through ROS topics, which can better replicate real hardware where the sensor is read through the same bus as the actuators.

I took the liberty of establishing some standard interface names for IMU: "orientation.x", "linear_acceleration.x" and "angular_velocity.x". And obviously for y, z and w in the case of the orientation quaternion.

Maybe we could put this somewhere more standard , thoughts @bmagyar?

If this looks good, I'll do the same for the FT Sensor.
We've been using this for years in ROS1 in https://github.com/pal-robotics/pal_hardware_gazebo/blob/melodic-devel/src/pal_hardware_gazebo.cpp but we think it will benefit more people here in ROS2.

[v-lopez]

On hold to review the naming of the interfaces as discussed on March 10th WG meeting. Sorry about the noise. But feel free to check the rest of the implementation as well.

Adapting something similar as the example in https://github.com/ros-controls/roadmap/blob/master/design_drafts/hardware_access.md#controller-interface

[v-lopez]

We'll keep the current naming, feel free to give it a look, if it's fine I'll add the FT sensor in the same way.

[v-lopez]

I just added ForceTorque sensor too.

I don't know if I should file an Issue for this: https://github.com/ros-simulation/gazebo_ros2_control/pull/65/files#diff-e14963d1c2ba3a31138bda99525946c355191fe361859b5fb7247fddad6c3a2aR229

A Force Torque sensor parent must be a Joint, but Model::SensorScopedName only looks for sensors in its links. So never finds them.

[bmagyar]

@ahcorde @chapulina could we please get a review on this guys?

[ahcorde]

In general It looks good to me, but I would love to see any example world or documentation about how to use/interact with this.

[jordan-palacios]

In general It looks good to me, but I would love to see any example world or documentation about how to use/interact with this.

The idea is that the (WIP) IMU and FT sensor controllers (or broadcasters) in ros2_controllers can make use of the interfaces provided by the gazebo system plugin.

We will make use of this in our public robot simulations, though we need the support for it in the gazebo system plugin first.

[jordan-palacios]

I forgot to mention that I will continue the work of this PR. 😛

[bmagyar]

@jordan-palacios I think if you added an IMU to the cart demo and an extra statement to the readme printing the relevant values from flexible joint states, that would cover what Alex is asking for

[jordan-palacios]

@jordan-palacios I think if you added an IMU to the cart demo and an extra statement to the readme printing the relevant values from flexible joint states, that would cover what Alex is asking for

Good idea. Will do.

[jordan-palacios]

I have added the IMU to the urdf of the cart_example_position demo.

You can check the reported IMU data with:

ros2 topic echo /dynamic_joint_states

Unfortunately, something must be wrong with the IMU configuration because the data ranges are all wrong.

For example, while idle, the cart reports a linear_acceleration.x of -5.633648437341699e-59. When the motion is triggered this value changes to, at best, to 2.9199594465887168e-31. @ahcorde could you take a look please?

When we fix this I'll add the IMU to the remaining demos and document the usage in the README.

[ahcorde]

@jordan-palacios

In the position mode we are moving the box to the specific position with setPosition(), I'm not really sure that this will generate any kind of interaction with the IMU because there is no "real" movement.

Did you try with velocity ?

[jordan-palacios]

@jordan-palacios

In the position mode we are moving the box to the specific position with setPosition(), I'm not really sure that this will generate any kind of interaction with the IMU because there is no "real" movement.

Did you try with velocity ?

Just tried it (also pushed the changes for the velocity demo). Same issues. IMU moves, but in very low magnitudes.

[v-lopez]

I have submitted a PR for the imu_sensor_broacaster ros-controls/ros2_controllers#195 which is a controller to read IMU, such as the ones added in this PR.

I also added it this controller to the velocity demo, so you can see a ROS topic publishing the values of the IMU obtained through these changes, passed to ROS control and published from there.

[v-lopez]

The IMU controller was merged on ros2_control and this could be merged here as well, you should be able to run the examples and see the imu changes.

[bmagyar]

FYI Foxy and Rolling releases were also made with the imu broadcaster

[ahcorde]

Can we remove this ? or add a comment why this is usefull ?

[v-lopez]

Sorry I'll remove the commented lines, they are not needed.

[v-lopez]

Also the whole imu for the position cart, I didn't understand

[ahcorde]

Same comment

[ahcorde]

do we need these changes in the position.yaml and position launch file ?

[v-lopez]

They are just an example, we only need them if we want the imu on those as well. But it doesn't make sense because afaik the cartpole moves instantaneously, so the IMU doesn't report anything useful.

[v-lopez]

Ping, is there anything missing?

[roncapat]

Is there an example or a template to write an IMU ros2 control interface? The gazebo-ros2-control package is not a great start point if someone has to write a plugin for a real IMU hardware sensor from scratch.