Reverse order of rotation offset (rpyOffset) calculation in p3d and imu plugins

[iche033]

See this comment in sdformat. Ran into an issue with calculation of rpyOffset in urdf to sdf conversion, which seems to suggest that the rpyOffset should be post multiplied in the gazebo ros plugins.

There isn't really much documentation on the xyzOffset and rpyOffset params and what frames they are in. All the example I found online have been using 0s for rpyOffset. The changes here will however affect user's code if they are manually specifying rpyOffset.

Signed-off-by: Ian Chen [email protected]

[azeey]

Would it be possible to condition this based on what version of libsdformat is being used?

[iche033]

added version check in fca594d. It assumes that the release will be made in sdformat 6.3.0 and forward ported though. So we'll need to merge and release the sdformat pull request first

[azeey]

Looks like there's another IMU plugin (GazeboRosImuSensor) that also uses rpyOffset

gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp

Line 86 in fca594d

orientation = offset.Rot()*sensor->Orientation(); //applying offsets to the orientation measurement

but from what I can tell, the rpyOffset that would be used in that sensor is manually set, not automatically generated by the fixed joint reduction code in libsdformat. So it seems okay to leave it as is.

[azeey]

hmm, thinking on this some more, if this runs on currently released versions of libsdformat that are greater than 6.2 (eg. v9.0), it'll do the wrong thing. It might be an ugly hack, but maybe we should have a flag in libsdformat that we check instead of the version?

[scpeters]

I think injecting an SDFormat element into the plugin block would be much simpler than trying to track which versions have the fix, especially once we start forward-porting:

• Fix xyz and rpy offsets in fixed joint reduction gazebosim/sdformat#500 (comment)

[iche033]

updated to use the injected corrected_offsets element

[iche033]

@scpeters are you ok with the latest changes?

[iche033]

but from what I can tell, the rpyOffset that would be used in that sensor is manually set, not automatically generated by the fixed joint reduction code in libsdformat. So it seems okay to leave it as is.

I think for p3d and the imu plugin, the rpyOffset can also be manually set. If not set, fixed joint reduction will generate them. If they are set, fixed joint reduction will read in the manually specified rpyOffset values and apply additional transformation on them.

hmm does that mean we should also not be touching these equations?

[iche033]

as of ef708a3, the equation will only be flipped if it detects that the ignition::corrected_offsets tag was injected by sdformat. So this change should not break existing behavior. We also don't need to wait for an sdformat release before merging this.