ROS2 RViz2 Strange visual TF behavior (disconnected limbs)
I am seeing some strange visual updates in RViz2 with my humanoid. It only started after adding an IMU and using the robot_localization package to provide odom and base_link updates via the IMU (but really just want 3D orientation for now). The robot torso seems to jump to different positions but the arms/legs dont follow until a few fractions of a second later. Since RViz is attached to torso, it appears the arms.legs jump but I believe it's actually the torso that does.
Video of behavior: https://www.youtube.com/watch?v=u-vla...
I am following REP120 for joints.
world => odo => base_link
base_link
=> torso
=> IMU (9DOF BMO-055)
=> l/r arm
=> l/r legs => knee => sole
=> base_footprint
edit: So I've figured out that it is the robot_state_publisher that is publishing the imu_torso joint link in tf_static which seems to be conflicting with the robot_localization. Still not sure how to fix from here.
My 18 servos are split on 3 serial channels, and I am running my joint publisher node 3 times, once for each serial channel. So I have 3 nodes each publishing 1/3 of the total JointState messages and these nodes are not synchronized so perhaps the fact that I have 3 nodes publishing to the same topic is confusing the TF robot_state_publisher? Though like I said, this only started after adding the IMU which is attached to the torso. I sometimes get warnings/errors in the RViz2 tree which seem to be reported by the robot_model/robot_state_publisher.
edit: I recoded this and now send a single combined joint_state message but didnt fix the issue.
Any suggestions? Thanks!
Fixed, but not solved. lol. So robot_localization (RL) package is sending a TF for odom frame @30Hz, so is the robot_state_publisher (RSP) as a fixed joint to the /tf_static endpoint @1Hz. The solution is to get the RSP to stop publishing updates to odom frame but it doesnt seem to support parameters to do this. RSP queries the URDF directly and outputs static TFs for all fixed joints.
Other option would be to split the URDF between world/odom and robot frames and RSP only gets robot URDF as param but I dont know how to do this or if its possible.
For now, I cloned and added an if() statement not to publish "odom" frames from within the publishFixedTransforms() method. This works but isnt elegant.
I feel like there is something else I am not doing right.