slow updates of links of a robot in RViz [RobotModel]

edit

Hello,

I have written a description package for iCub humanoid robot. It basically consist of a urdf model of the robot to visualize in RViz and also a ros-yarp bridge which publishes the joint positions of all joints on the robot. (for now bridge takes the data from robot simulator.)

The /joint_states topic is published at 50 Hz from the bridge and I am running launch file with /robot_state_publisher as it follows:

<?xml version="1.0"?>
<launch>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description" command="cat $(find icub_description)/urdf/icub.xml" />
  <!-- <param name="publish_frequency" value="200" /> -->

  <node name="icub_tf_publisher" pkg="icub_description" type="icub_tf_publisher" />

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- Launch rviz node for visualization -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find icub_description)/rviz/display_urdf.rviz" />
</launch>

But in this situation, the RobotModel in RViz follows the simulator with almost 1 second delay even robots move only one joint. I have looked every topic (joint_states, tf etc..) and they are all fine publishing the data at desired rate.

Do you have any idea why RobotModel lags ? The package can be found on here.

Edit: According to the tf frame tree most of the tf data is published at 1.250Hz which is very slow. Only base_link is published at 50Hz. So the problem is somehow related to /robot_state_publisher