Giters

ros-drivers / um7

ROS driver for UM7 inertial measurement device.

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

tf ned to enu

rdockterjr opened this issue · comments

commented

I have been having an issue with the quaternions being output by the um7 driver.

Specifically when I have the tf_ned_to_enu flag set to true, the quaternions appear to rotate about the wrong axes when I visualize in rviz.

I believe this is due to the conversion happening around line 220 of main.cpp:

`

  imu_msg.orientation.w =  r.quat.get_scaled(2);

  imu_msg.orientation.x =  r.quat.get_scaled(1);

  imu_msg.orientation.y = -r.quat.get_scaled(3);

  imu_msg.orientation.z =  r.quat.get_scaled(0);

`

Correct me if I am wrong but I believe the correct transformation to get enu coordinates with the IMU facing x forward would be:

`

  imu_msg.orientation.w =  r.quat.get_scaled(0); //w_old

  imu_msg.orientation.x =  r.quat.get_scaled(1); //x_old

  imu_msg.orientation.y =  -r.quat.get_scaled(2); //-y_old

  imu_msg.orientation.z =  -r.quat.get_scaled(3); //-z_old

`

Atleast when I use this transformation the axes in rviz appear to rotate in the correct direction about the correct axes.

Not sure if I am missing something else here but perhaps this was a math error?

Thanks

commented

I implemented rdockterjr's proposed solution and it's giving me the right orientation for REP105.

commented

It's still there.

Thanks for the fix rdockterjr.