Ask Your Question
0

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

asked 2020-10-22 18:21:38 -0600

ateator gravatar image

updated 2020-10-22 19:14:33 -0600

  • 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 image description

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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.

mjcarroll gravatar image mjcarroll  ( 2020-10-23 08:21:04 -0600 )edit
1

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

ateator gravatar image ateator  ( 2020-10-23 09:13:58 -0600 )edit
1

I have created a minimal reproducible package and the issue has resolved. I will be doing more research and will update

ateator gravatar image ateator  ( 2020-10-23 20:23:31 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-11-02 13:47:23 -0600

ateator gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-10-22 18:21:38 -0600

Seen: 131 times

Last updated: Nov 02 '20