Transform message in differential mode

[efernandez]

This is important because the relative transformation is not the same if the sensor and target frame are different.

Consider for example the case of an IMU sensor upside down:

• The robot base frame is base_link
• The IMU sensor frame is imu_link
• The imu_link transformation wrt base_link is 180 degrees wrt the y or x axis
• The angular velocity around the z axis has opposite sign in the sensor frame wrt the target frame

This only impacts the Imu2D, Pose2D and Odometry2D sensor models. The changes are similar for each, but with some differences. For this reason I've refrained from trying to factorize things out somehow. TBH it doesn't look trivial and probably not worth it, but at the same time I think there are too many if-else blocks and indentation levels. Any thoughts?

I'd be happy to make the implementation of this logic cleaner/simpler. 😃

For now I advise you to hide the whitespace changes when you inspect the changes.

[efernandez]

What's the error in the CI checks, please? I can't reproduce it offline.

[efernandez]

Still don't know why the checks failed on this one. They passed on #217 , which is on top of this one. Maybe the CI test job should be re-kicked. 🤔

[svwilliams]

The build error is:

C++ exception with description "Could not find required parameter pose_target_frame in namespace /Optimizer/wheel_odometry" thrown in the test body.

from here: https://github.com/locusrobotics/fuse/blob/devel/fuse_optimizers/test/test_optimizer.cpp#L49

This PR makes the "pose_target_frame" parameter required for the Odometry2D sensor. Previously it was not required when in differential mode. The configuration for the above test was not updated to include that required parameter:
https://github.com/locusrobotics/fuse/blob/devel/fuse_optimizers/test/config/struct/common_robot_config.yaml#L28-L33

[efernandez]

Thanks, that makes sense. Fixed.

[svwilliams]

I think there are too many if-else blocks and indentation levels

The only thing that's coming to mind is factoring out some of the if-else blocks into a separate function. For example, in imu_2d.cpp, if all of the differential logic was contained inside a function call, then you would end up with a pleasing:

  if (params_.differential)
  {
    some_new_function(probably, with, lots, of, parameters);
  }
  else
  {
    common::processAbsolutePoseWithCovariance(
      name(),
      device_id_,
      *pose,
      params_.pose_loss,
      params_.orientation_target_frame,
      {},
      params_.orientation_indices,
      tf_buffer_,
      validate,
      *transaction);
  }

Mostly that function call would just be moving/hiding the nesting levels. But it would allow you to exit early if needed.

void some_new_function(probably, with, lots, of, parameters)
{
  if (!common::transformMessage(tf_buffer_, *pose, *transformed_pose))
  {
    ROS_ERROR_STREAM("Cannot transform pose message with stamp "
      << pose->header.stamp << " to orientation target frame "
      <<  params_.orientation_target_frame);
    return;
  }
  if (!previous_pose_)
  {
    previous_pose_ = std::move(transformed_pose);
    return;
  }
  if (params_.use_twist_covariance)
  {
    // Do real work here
  }
  else
  {
    // Do other real work here
  }

To be very clear, I'm not requesting you do this as part of this PR. I'm happy enough as-is. But you did ask.

[efernandez]

Yes, I agree it'd look simpler and cleaner moving things a helper function.

Since both this and #217 are approved, I'll make that refactoring change on another PR. 😃