Update pose_republisher_node.cpp

[ksatyaki]

tf2::Transform receiver_world_pose = last_robot_pose_ * receiver_pose; should be enough when transforming the receiver pose (in robot frame) into the world frame using the robot pose (in world frame).

Similarly for emitter pose.

Description

The function inline tf2::Transform static_link_wrt_global_frame(tf2::Transform static_link, tf2::Transform base_frame) in irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/utils.hpp is not necessary and might be wrong.

This is a simple change that addresses this mistake.

Type of change

• Bug fix (non-breaking change which fixes an issue)
• New feature (non-breaking change which adds functionality)

How Has This Been Tested?

Changing the orientation of the ir opcode sensors with respect to the base frame results in wrong transforms due to the mistake in transforms mentioned above. This fix makes sure that the transforms are correct. I have tested this by adding a dummy base_link frame that is 180 off the original base_link frame.

Checklist

• I have performed a self-review of my own code
• I have commented my code, particularly in hard-to-understand areas
• I have made corresponding changes to the documentation

[justinIRBT]

Can you include pictures of the TF frames from RVIZ before and after the change. Thanks!

[ksatyaki]

It is hard to visualize the frames since they are internal topics. (the tf is in the /_internal namespace and the frame names do not exist in the main tf tree).

However, I can show you how the tf tree looks before and after the fix. This part is all good. But, the pose republisher doesn't use the main tf tree, but the internal.

Echo of the /_internal/sim_ground_truth_ir_emitter_pose before fix:
Some parts removed since they are not important, i.e., covariance.

header:
  stamp:
    sec: 385
    nanosec: 200000000
  frame_id: standard_dock
child_frame_id: standard_dock/halo_link
pose:
  pose:
    position:
      x: 0.00017236211175518748
      y: 0.9169999315455973
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.7073856728485651
      w: 0.7068277794827978
  covariance: ...

Echo of the /_internal/sim_ground_truth_ir_receiver_pose before fix:

header:
stamp:
  sec: 514
  nanosec: 530000000
frame_id: turtlebot4
child_frame_id: turtlebot4/ir_omni
pose:
pose:
  position:
    x: 0.0013592019491204864
    y: 0.8478367673378463
    z: -0.00447447899004243
  orientation:
    x: 0.0
    y: 0.0
    z: -0.705137580109443
    w: 0.7090705135015831

It is clear that these are oriented in the same direction. I think that is because of the mistake in the utils::static_link_wrt_global_frame function.

Even without this error, it makes sense to replace this function with an appropriate multiplication of the tf2::Transform objects as suggested. The create3 ignition node uses the relative (x,y) positions of these frames with respect to each other (it makes sure that both are close to (0,0)). So, even if they have an offset with respect to the z-coordinate, it will work, and does work. :-)