ROS2 Eloquent TF listener (tf_echo) values freeze/resume on a cycle unexpectedly

edit

• Ubuntu 18.04
  • ROS2 Eloquent
  • TF
  • C++

My pipeline:

I have a robot which uses sensors to detect the rotation, extension of its joints. It has revolute, prismatic, and fixed joints in a URDF file. Yes, I realize that the link names may be confusing, but its not really relevant.

me@my_computer:~/ROS2_workspace/src/
return LaunchDescription([my_package/urdf_dir check_urdf my_robot.urdf   
robotname is: MyRobot
---------- Successfully Parsed XML --------------- root Link: base_link has 1 child(ren)
child(1):  0
    child(1):  1
        child(1):  3
            child(1):  4
                child(1):  5
                    child(1):  6
                        child(1):  7
                            child(1):  9
                                child(1):  10

I am gathering data from sensors and publishing that data with my node on a custom Msg interface.

  void publish_sensor_data
  {
return LaunchDescription([
    auto sensors_msg = std::make_unique<SensorsMsg>();
    /* magic */
    pub_for_sensors_->publish(std::move(sensors_msg));
  }

I then have a second node which takes my custom Msg and converts it to Msg::JointState

void sensors_callback(const SensorsMsg::SharedPtr sensors_msg){
  auto joint_state_msg = std::make_shared<sensor_msgs::msg::JointState>();
  joint_state_msg->name.push_back("revolute_joint_1");
  joint_state_msg->name.push_back("revolute_joint_2");
  joint_state_msg->name.push_back("prismatic_joint_1");
  joint_state_msg->name.push_back("revolute_joint_3");
  joint_state_msg->name.push_back("revolute_joint_4");
  joint_state_msg->name.push_back("revolute_joint_5");
  joint_state_msg->name.push_back("revolute_joint_6");
  joint_state_msg->name.push_back("prismatic_joint_2");
  joint_state_msg->name.push_back("fixed_joint");

  joint_state_msg->position.push_back( deg_to_rad(sensors_msg->angle));
  joint_state_msg->position.push_back( deg_to_rad(sensors_msg->angle));
  joint_state_msg->position.push_back( in_to_m( sensors_msg->extend));
  joint_state_msg->position.push_back( deg_to_rad(sensors_msg->angle));
  joint_state_msg->position.push_back( deg_to_rad(sensors_msg->angle));
  joint_state_msg->position.push_back( deg_to_rad(sensors_msg->angle));
  joint_state_msg->position.push_back( deg_to_rad(sensors_msg->angle));
  joint_state_msg->position.push_back( in_to_m( sensors_msg->extend));
  joint_state_msg->position.push_back( deg_to_rad(0.) );

  joint_state_msg->header.stamp = this->now();
  joint_state_pub_->publish(std::move(*joint_state_msg));  
}

I make use of robot_state_publisher to convert my Msg::JointState to send my transforms over the TF server. This is done through a launch file

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

urdf = os.path.join(get_package_share_directory('my_package'),'urdf_dir', 'my_robot.urdf')

config_common = os.path.join(get_package_share_directory('my_package'), 'yaml_dir', 'robot_params.yaml')                        

return LaunchDescription([
    Node(package='my_package', node_name='sensor_publisher', node_executable='sensor_publisher', name="sensor_publisher", output='screen', arguments=[config_common]),
    Node(package='my_package', node_executable='sensors_to_jointstates', output='screen', parameters=[config_common] ),
    Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', arguments=[urdf, config_common]),
    Node(package='rviz2', node_executable='rviz2', output='screen' )
])

My problem:

Although I can use ros2 topic echo /joint_states (image below, left) to see that my sensor data is coming in at the rate I expect (20 hz), subsequently turned into joint states, and finally turned into TFMessages, the values that are received using ros2 run tf2_ros tf2_echo base_link 10 20 (image below, right) will have somewhat regular freezes, where the header timestemp will hold the same value for the sec and nanosec, and the sensor values wont change either. This will clear up after a short moment (~2s), but will reappear shortly (~2s) after

There are a couple things to note about the image

1. Both terminals are running at the same time, but the tf_echo command on the right has a timestamp far ahead of the ros2 topic echo on the left
2. Both terminals/nodes respective sensor values react instantly/synchronously when the physical sensor changes position (not pictured), as long as the TF is in its active phase and regardless of the timestamp difference ...