Fix/coordinate system motive 2.0

[jadhm]

After testing the system on our robot and after visualizing the results on the RVIZ. It appeared that the coordinate system is used by Motive 2.0+ is using the right hand Y up coordinate system. However ROS uses also right hand but Z up coordinate system. The way that was implemented in the package is totally wrong switching the values of the axis Y and Z is not a solution. But introducing a rotation between the axis is. Luckily Motive 2.0+ supports publishing messages with Z up. This could be found in streaming -> UP Axis After setting the UP axis to Z Up. No need to do any thing in the odom/poses messages just stream the data as it is.

[tonybaltovski]

@jadhm I am reluctant accept this because the default is Y up. Could it be enabled by parameter? Thoughts? Regarding the axis switching, if you do the matrix multiplication, it is equal.

[jadhm]

@tonybaltovski Thanks for your reply and I totally see your point. After looking closely on the messages of the motive software and comparing the axis with what ROS uses I cam out with the following : If we leave the default axis in motive as Y UP we should convert it in the mocap package and that should be done by just introducing a (90 degree ) rotation in the X axis :

After substituting the the alpha value we get : y = -z z = y

which what's been done here https://github.com/ros-drivers/mocap_optitrack/blob/5f8ab69d030eb1f02b5422eea3ff051191c52c26/src/rigid_body_publisher.cpp#L69-L80

And to make it more concrete this is an example of a message that been published using motive 2.2.0 in Y UP and then with Z UP coordinates without swiping any axis in mocap packge just the messages as they are Y UP

    position: 
      x: -0.0010439753532409668
      y: 0.10643653571605682
      z: 0.0025311997160315514
    orientation: 
      x: -0.00019001988403033465
      y: -0.00659878458827734
      z: 0.00016971502918750048
      w: -0.9999781847000122

Z UP

    position: 
      x: -0.0011064708232879639
      y: -0.002512845676392317
      z: 0.10646363347768784
    orientation: 
      x: -0.0002279526524944231
      y: -0.00014353691949509084
      z: -0.0066085271537303925
      w: -0.9999781847000122

What I want to say that the way done before any addition seems the way to go so I don't understand the reasoning of this https://github.com/ros-drivers/mocap_optitrack/blob/5f8ab69d030eb1f02b5422eea3ff051191c52c26/src/rigid_body_publisher.cpp#L58-L68

I tested this using motive 2.2.0 and using this transformation :
https://github.com/ros-drivers/mocap_optitrack/blob/5f8ab69d030eb1f02b5422eea3ff051191c52c26/src/rigid_body_publisher.cpp#L69-L80 and it seems to work very well :

You can see how the rotation works perfectly. Would you except a pull request by changing the coordinates of motive 2.0.0 > and make them as they are in https://github.com/ros-drivers/mocap_optitrack/blob/5f8ab69d030eb1f02b5422eea3ff051191c52c26/src/rigid_body_publisher.cpp#L69-L80

[jadhm]

I cannot test it on any versions between 1.7 and 2.0 otherwise I would just keep this transformation for all versions https://github.com/ros-drivers/mocap_optitrack/blob/5f8ab69d030eb1f02b5422eea3ff051191c52c26/src/rigid_body_publisher.cpp#L69-L80

What do you think ?

[tonybaltovski]

@jadhm I am fine with that. Could you add some instructions on how to change the axis with Motive, eventually? I really appreciate your contributions!

[jadhm]

@tonybaltovski Thanks for your quick resonance and thank you for the release. I will add these instructions within this week.

[jadhm]

@tonybaltovski I wanted to add how to switch the axis in motive but I was thinking if that would be helpful because switching the axis will make the coordinates of the mocap wrong ! so what do you think ?