Frame orientations got wrong in 2.2.1

[peci1]

Hi @fcolas , nice to meet you again at least virtually =) @kubelvla might also want to cheer in :)

What's the rationale of commit 651ee in version 2.2.1?

It broke our poor old Nifti robot, who now thinks that pitch is roll and vice versa... Should we change some configuration? I tried all values of frame_local, but it seems to no longer have any effect.

With version 2.1.0 and 2.2.0, everything works as it used to...

Thanks for any help ;)

Martin Pecka from CTU in Prague

[fcolas]

Hi @peci1 @kubelvla, according to the documentation (and it makes sense) the coordinate system specification (ENU, NED, or NWU) only applies to orientation and linear velocity messages (position messages are in ECEF coordinate frame).
I realized that, when introducing support for this local frame, we've been overzealous and converted coordinates of messages that were, and should stay, in the frame of the IMU.

For your issue, which message type is affected? Are you sure that the IMU frame is correctly oriented in your /tf (it could have been set wrongly to compensate for the wrong conversion in the ROS messages)? Which is the frame_local parameter that works in 2.10 and 2.2.0? Can you paste the output of the configuration (mtdevice.py -i)?

[peci1]

Thanks for the explanation, Francis. I see that you wanted to correct the behavior of the driver according to the specs, but can't there be much more people affected? I assume everybody needed to compensate for the previous "wrong" behavior, and now that it is corrected, all the compensations will give wrong results. Or am I missing a point? What about providing a parameter that would switch between the old and new behavior? This way people could still use this driver without messing with their own code.

Otherwise, here is some debugging info from our robot:

With 2.2.0, the local_frame doesn't matter. With 2.0.0, we use NWU (I didn't have time to test if changing it with this version has any effect, but I assume it does).

I'm not sure about the physical orientation of the IMU, I know they maybe put it there reversed when upgrading the robot. But that should be compensated in the TF tree.

The following was run with the robot standing still on flat ground:

$ rosrun xsens_driver mtdevice.py -i -d /dev/ttyS0
Device: /dev/ttyS0 at 115200 Bd:
  device ID:  0x01500056
  product code: 'MTi-G-28A53G35'
  firmware revision: (2, 6, 1)
  baudrate: 2
  error mode:  0x0001
  option flags:  message unsupported by your device.
  location ID:  0x0000
  transmit delay:  message unsupported by your device.
  synchronization settings:  message unsupported by your device.
  general configuration: {   'Master device ID': 22020182,
    'date': '\x00\x00\x00\x00\x00\x00\x00\x00',
    'device ID': 22020182,
    'length': 115,
    'number of devices': 1,
    'output-mode': 6183,
    'output-settings': 1,
    'period': 1152,
    'skipfactor': 0,
    'time': '\x00\x00\x00\x00\x00\x00\x00\x00'}
  output configuration (mark IV devices):  message unsupported by your device.
  string output type:  message unsupported by your device.
  period: 1152
  alignment rotation sensor:  message unsupported by your device.
  alignment rotation local:  message unsupported by your device.
  output mode:  0x1827
  extended output mode:  0x0000
  output settings:  0x00000001
  GPS coordinates (lat, lon, alt): (45.449825286865234, 12.236778259277344, -4.23799991607666)
  available scenarios: [   (1, 7, 'general'),
    (2, 7, 'automotive'),
    (3, 7, 'aerospace'),
    (9, 7, 'general_nobaro'),
    (10, 7, 'aerospace_nobaro')]
  current scenario ID: 10
  UTC time: (380000000, 0, 0, 0, 0, 2, 39, 0)

$ rostopic echo /imu/data

header: 
  seq: 891
  stamp: 
    secs: 1540564105
    nsecs: 931217908
  frame_id: /imu
orientation: 
  x: -0.827061116695
  y: -0.560816943645
  z: -0.0381120927632
  w: -0.00132798729464
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: 0.0163090433925
  y: 0.00552357593551
  z: 0.00950049236417
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: -0.111239217222
  y: -0.0521704927087
  z: -9.81276226044
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

$ rosrun tf tf_echo /imu /base_link
At time 1540564166.971
- Translation: [0.000, -0.000, 0.150]
- Rotation: in Quaternion [1.000, 0.000, 0.000, -0.000]
            in RPY (radian) [-3.142, 0.000, 0.000]
            in RPY (degree) [-180.000, 0.000, 0.000]

[fcolas]

Well, the thing is that the default leads to no compensation and I expect that most people didn't bother with this parameter.
In any case, it's meant for the mapping between the x, y, and z axes into North, South, East, West, Up and Down. For ground robots, the default is often fine but in aeronautics they might use NED instead of ENU, as far as I understand.

For you problem, the /tf states that the imu is upside down in the robot, with the cable pointing toward the back. From the linear acceleration, it's clearly not the case (it should be pointing upwards since it measures the difference wrt free fall).
You should try with quaternions like (1/sqrt(2), 0, 0, -1/sqrt(2)) (w first and z last, then change sign on z component, or on w or on both). Of course the best solution would be to know how it is placed in the robot.
To be sure, you raise, say, the front of the robot and check that the angular velocity gets a negative y and linear acceleration a positive x (and z, of course).

[peci1]

Ok, we checked the real axes on robot by tilting it and corrected our compensation code for the new axes.

However, we do not fully understand the rationale of publishing data in two different frames. Then you get into trouble like the messages on topic velocity to be in frame ~frame_id for the angular part, and ~frame_local for the linear part, but the frame in the header is set to ~frame_id - so the frame for linear velocity is wrong. If this is still the desired behavior, would it be possible to improve the documentation on ROS wiki to explicitly specify in which frame each part of the data is published, and which data are affected by the ~frame_local parameter?