Fix imu 2d transform

[efernandez]

• Fix IMU transform

  Also fixes transformed twist covariance not being used in differential
  mode when using the twist covariance.

This is on top of #267, fixing the rostest provided in that PR.

$ make run_tests_fuse_models_rostest_test_imu_2d.test
[ROSTEST]-----------------------------------------------------------------------

[fuse_models.rosunit-imu_2d_test/BaseFrameYawAngularVelocity][passed]

SUMMARY
 * RESULT: SUCCESS
 * TESTS: 1
 * ERRORS: 0
 * FAILURES: 0

I think the IMU data needs to be handled differently than all other pose/twist/odometry measurements when used in differential mode to create relative constraints. Indeed, in https://github.com/cra-ros-pkg/robot_localization/blob/b5345749e1449a5ec3536d3c8cc80f1de65ae9f6/src/ros_filter.cpp#L2563-L2955 the IMU is handled differently when its data has to be transformed to a different frame. However, I've used the implementation in https://github.com/ros-perception/imu_pipeline/blob/ec36441c5c0cf1e63c12731a3a59185d29a00979/imu_transformer/include/imu_transformer/tf2_sensor_msgs.h#L80-L121, which is way simpler and seems to fix the issue.

[efernandez]

Please let me know why the checks have failed, so I can look into it.

[svwilliams]

There are currently multiple target frames supported:
https://github.com/locusrobotics/fuse/blob/devel/fuse_models/include/fuse_models/parameters/imu_2d_params.h#L99-L101

...though I'm not entirely sure why. I would think that both the twist and acceleration would want to use the same target/body frame.

And it's been a long time since I thought about IMUs, but my recollection is that transforming the IMU pose is complicated. I need to review the imu_transformer package in detail. It was not clear to me how the orientation was being transformed.

[efernandez]

Yes, that's something I wanted to discuss in this PR and I forgot to mention it.

Right now there are three different target frames:

• acceleration_target_frame
• orientation_target_frame
• twist_target_frame

However, the implementation in imu_transformer transforms everything in the IMU all at once with a single target frame.

I wonder if it really makes sense to have multiple target frames. 🤔

In that case, we could call the imu_transformer code three times and only take the components we're interested in, with each target frame. However, in that case, it'd be more efficient, and I believe still clean, to steal some of the code in imu_transformer to perform the same transformation for each component separately.

I believe the twist is already done correctly, and probably the acceleration as well. The orientation is definitely incorrect.

Just let me know whether you want to keep the three target frames or not, and I'll make the changes mentioned above?

If we remove the existing target frames and create a singal target_frame, then we need some deprecation warnings, and set the new target_frame with them. In that case, if the different target frames aren't the same, I'm not sure which one should be used. I've been using twist_target_frame in this initial implementation.

[efernandez]

Please let me know why the checks have failed, so I can look into it.

pinging on this, so I can have a look at fixing the PR checks

[ayrton04]

Three of the four checks are from the actual ROS build farm, right? Won't those have the details?

[ayrton04]

https://build.ros.org/job/Mpr__fuse__ubuntu_bionic_amd64/31/consoleFull

I believe you can see that one too.

21:06:57 In file included from /tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:46:0:
21:06:57 /usr/src/googletest/googletest/include/gtest/gtest.h: In instantiation of ‘testing::AssertionResult testing::internal::CmpHelperEQ(const char*, const char*, const T1&, const T2&) [with T1 = int; T2 = long unsigned int]’:
21:06:57 /usr/src/googletest/googletest/include/gtest/gtest.h:1421:23:   required from ‘static testing::AssertionResult testing::internal::EqHelper<lhs_is_null_literal>::Compare(const char*, const char*, const T1&, const T2&) [with T1 = int; T2 = long unsigned int; bool lhs_is_null_literal = false]’
21:06:57 /tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:151:3:   required from here
21:06:57 /usr/src/googletest/googletest/include/gtest/gtest.h:1392:11: error: comparison between signed and unsigned integer expressions [-Werror=sign-compare]
21:06:57    if (lhs == rhs) {
21:06:57        ~~~~^~~~~~

[efernandez]

Three of the four checks are from the actual ROS build farm, right? Won't those have the details?
https://build.ros.org/job/Mpr__fuse__ubuntu_bionic_amd64/31/consoleFull
I believe you can see that one too.

I thought I couldn't access those. Thanks.

I've fixed a couple of compilation issues. Hopefully after that all checks pass. 🤞

[ayrton04]

Done processing /tmp/ws/src/fuse/fuse_models/test/test_graph_ignition.cpp
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:134:  Do not use namespace using-directives.  Use using-declarations instead.  [build/namespaces] [5]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:178:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:180:  } should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:200:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:202:  } should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:226:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:228:  } should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:246:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:246:  } should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:262:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:264:  } should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:289:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:289:  } should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:297:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:297:  } should be on a line by itself  [whitespace/braces] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:245:  Add #include <algorithm> for sort  [build/include_what_you_use] [4]
/tmp/ws/src/fuse/fuse_models/test/test_imu_2d.cpp:231:  Add #include <vector> for vector<>  [build/include_what_you_use] [4]

[efernandez]

I just fixed the roslint issues