Laser is rotating with odom
I have written my localization code and the biggest challenge I have is to get the correct Map -> Odom transform.
From my understanding, I should first get the TF base_footprint -> Map and then inverse it and send TF as odom -> Map.
But the problem is that when I move or rotate my robot, the laser beams move and rotate together with it instead of being "static".
Here is my code:
# Get map -> laser transform.
map_laser_pos = np.array( (pose[0],pose[1],0) )
map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
# Apply laser -> base_link transform to map -> laser transform
# This gives a map -> base_link transform
laser_base_link_offset = (0.265, 0, 0)
map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T
# Publish transform
self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , "base_footprint", "map")
What am I doing wrong here?
EDIT:
My TF LINK
Is you laser not mounted on your robot? I would expect the sensor frame to be fixed with respect to your robot not the world. Or am I misunderstanding.
@PeteBlackerThe3rd yes the laser is mounted and it has a static TF to the base_link
@PeteBlackerThe3rd Can you please explain your thoughts? Are you saying that my code assumes that the laser is not fixed?
@PeteBlackerThe3rd I have included my TF when I turn the AMCL. When I turn on my Localization, Odom becomes left over and Map directly connects to base_footprint but if I change in my code to publish TF from Map to Odom, my laser gets rotated with the robot
@PeteBlackerThe3rd Yes the laser itself should rotate but not the laser beams! They have to reflect the map environment and map environment should not rotate with robot
@PeteBlackerThe3rd Can you have a look here link. Isn't it odd that Map to Odom has the same frequency as odom to base_footprint?
@stevemartin Hi, we have almost the same issue, did you find any solution to this? If yes, maybe you can share it to me please.