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

Question

• 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
3. Only the TF Terminal will have its timestamp and values freeze (pictured)
4. The freeze goes on a cycle, to where the timestamp frozen on the screen is typically ~4.0s ahead of the last value stuck on screen. About every 4th cycle, it will skip an extra ~1.0s
5. This freeze can be visualized using rviz2 at the same time (icon pictured above in Ubuntu Launcher)
6. When resuming from frozen to active, the TF values will skip some time in order to match with real time speed
7. All of my nodes have Default QoS with queue size of 50
8. My computer memory and processing cores are fine. No thread is locked

###My Question: How do I get TF to react as expected, without the values and timestamp freezing on a cycle? What should I investigate next? I have looked into a few different things but I would like your opinions so that I can consider everything equally

Tank you for viewing. I look forward to your response

---

Originally posted by ateator on ROS Answers with karma: 37 on 2020-10-22

Post score: 0

---

Original comments

Comment by mjcarroll on 2020-10-23:
What is the behavior on the /tf and /tf_static topic? That would be the missing link between /joint_states and the end result of tf2_echo. Can you echo those topics and see if you are getting gaps/freezes in there?

Also, is there a chance that you can create a reproducable example (that doesn't rely on hardware)? That would go a long way in helping others solve it.

Comment by ateator on 2020-10-23:
The behavior on /tf is as expected, no freezes or anything. I have use_tf_static set to false, so the /tf_static topic does not have any echo output. It seems that the issue is somewhat related to the transform listener. I copied, modified the code inside echoListener code in tf2_ros/tf_echo.cpp (from both Eloquent and Foxy because they have different formats) to work with my project and experienced the same error with my own listener, with both formats.

I will put together some code to reproduce this error in a clean, concise way

Comment by ateator on 2020-10-23:
I have created a minimal reproducible package and the issue has resolved. I will be doing more research and will update

Answer 1

I haven't figured out the exact answer but I believe the problem is in my CMakeLists.txt. I built a new minimal project and the setup shown above works fine.

---

Originally posted by ateator with karma: 37 on 2020-11-02

This answer was ACCEPTED on the original site

Post score: 0