XBot seems to publish on ros topics the loop after

[torydebra]

Hi,

I am following the code in the ros_from_rt example https://github.com/ADVRHumanoids/xbot2_examples/blob/master/src/ros_from_rt/ros_from_rt.cpp

My goal is to publish the joint states (as in the example) such that the robot_state_publisher of ROS will subscribe to this to generate tf transforms for rviz.

In the example, no time stamp is set in the message (js_msg->msg().header.stamp). This cause some annoying warnings :

[ WARN] [1612979792.350331981, 30.005000000]: Received JointState is 30.004000 seconds old.
[ WARN] [1612979802.359745531, 40.005000000]: Received JointState is 40.005000 seconds old.
[ WARN] [1612979812.369717078, 50.008000000]: Received JointState is 50.008000 seconds old.
...

I tried filling the timestamp with js_msg->msg().header.stamp = ros::Time::now and similarly with std::chrono following the clock example, but this cause even more warnings, about tf old data:

[ WARN] [1612980396.222657083, 5.494000000]: TF_OLD_DATA ignoring data from the past for frame teleop_link1 at time 5,489 according to authority unknown_publisher

If you see the warn, the differences in time is 5.494000000 - 5,489 = 0.005, which is the rate of the xbot thread which I set on the config file nrt_main: {sched: other, prio: 0 , period: 0.005}

I tried changing the period, and I see that again the time differences follows the thread period. So it seems that xbot create a message at loop i but publish it at loop i+1 ...

Does anyone have experience with this?

[torydebra]

I have tried on the PC i have at IIT and things work well here :D

May this issue be caused by ubuntu/ros versions?

• on the system with the issue there is ubuntu 18+ros melodic + xbot for 18,
• in the one which works, ubuntu 16 + ros kinetic (but gazebo 7) + xbot for 16

I will investigate further

[torydebra]

According to @alaurenzi, the issue title is true, the message is published the loop after due to the real-time magics below xbot2.

Indeed,

rostopic delay /xbot_control_main/joint_states
    subscribed to [/xbot_control_main/joint_states]
    WARNING: may be using simulated time
    average delay: 0.005
       min: 0.001s max: 0.010s std dev: 0.00086s window: 189

when the thread period is 0.005

It seems that ros kinetic does never comply for TF_OLD_DATA : I tried to delay even of some seconds the thread and no warnings are print. This seems too strange so I may be doing something wrong. I will check on Ubuntu 18 soon

[torydebra]

I may have found the issue.

I discovered that also /xbotcore node directly sent transformation to tf:

So this cause conflicts and probably rviz complains because doubled tf msgs with different stamps arrive at each loop.

Is it expected for xbotcore to publish on tf? Is something inside the ros_io / ros_ctrl plugin? Now, I can not check this double publishing on Ubuntu 16, but it seems that the version of xbot for Ubuntu 16 does not publish to /tf directly...

[torydebra]

By the way, the message published on tf causes me some troubles, because the transformation between link world and _baselink (speaking about teleop urdf) is not published. The clue is probably here: https://github.com/ADVRHumanoids/xbot2_wip/blob/c9bb2359cdb9998f982d784bc0534d0a53cbcf2c/ros/src/robot_state_publisher.cpp#L32-L36

For a not floating base robot, it would be possible to publish a tf also if the parent is the world.

The troubles which this causes me are related to relative transforms: I want to publish a marker on a specific robot link. Probably the visualization relies on relative transform from world and that link, so without world-baselink transform the marker is discarded.

For the moment, I solved using the robot state publisher and remapping the topic from where rviz reads the transform, to eliminate the conflict explained in the previous message

[alaurenzi]

RosIO publishes tfs by default (see here). It can be configured NOT to do so with the following config entry

ros_io:
  type: ros_io
  thread: nrt_main
  params:
    publish_tf: {value: false, type: bool}

[alaurenzi]

Concerning world frame, it is responsibility of the user to (optionally) locate a robot inside the world frame. The robot itself does not have access to such information.

[torydebra]

Ok thanks !