I have been trying to implement a low-level motion controller for my robot in ROS-Gazebo simulation. For this purpose, I need to convert some of my states i.e. CoM from body frame to Inertial (map) frame. However, when I use tf2 lookupTransform to do the transformation, it gives me stale values (repeating values/timestamps) at 500Hz. I have tried to publish my transforms faster by publishing robot_state_publisher and joint_state_publisher publishing at 2000Hz, however, the problem persists.
Any help/suggestions would be highly appreciated.