Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update pose_republisher_node.cpp #236

Open
wants to merge 1 commit into
base: humble
Choose a base branch
from

Conversation

ksatyaki
Copy link

@ksatyaki ksatyaki commented Nov 28, 2024

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

`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.
@justinIRBT
Copy link
Collaborator

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

@ksatyaki
Copy link
Author

ksatyaki commented Dec 9, 2024

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.

image
image

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. :-)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants