Robotics StackExchange | Archived questions

TF_REPEATED_DATA issue

Hello, I'm trying to use

rosrun tf2_tools view_frames.py

but I get spammed with warnings:

Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame wheel_left_link at time 684.167000 according to authority /tb3_1/robot_state_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp

I also tried writing python code with a listener, but I get almost the same warnings, here's the code:

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
import math
import tf2_ros

class move_bot:
    def __init__(self):
        self.pub = rospy.Publisher("tb3_1/cmd_vel",Twist,queue_size=100)
        self.sub = rospy.Subscriber("tb3_1/scan",LaserScan,self.callback)
        self.linx = 1
        self.angz = 0.2


    def control(self):
        move = Twist()

        K_d = 0.005
        #K_beta


        move.linear.x = self.linx
        move.angular.z = self.angz


        #xi_d = (error_d/rho_d)

        #epsilon_d = log((1+(xi_d/M_d))/(1+(xi_d/M_d)))

        #v = Kd*epsilon_d
        #w = K_beta*rho_inv*r_beta*epsilon_beta


        self.pub.publish(move)

    def callback(self,msg):
        min_dist = 1
        print(msg.ranges[0])
        if(msg.ranges[0]<min_dist):
            self.linx = -1
        else:
            self.linx = 1


if __name__ == '__main__':
    rospy.init_node("simple_follow")
    rate = rospy.Rate(1)
    obj = move_bot()

    tfBuffer = tf2_ros.Buffer(rospy.Duration(1200.0))
    listener = tf2_ros.TransformListener(tfBuffer)

    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform("tb3_1", "tb3_1/base_footprint", rospy.Time(0))
        except(tf2_ros.LookupException, \
                tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):  
            rate.sleep()
            continue

        #x = trans[0]
        #print(x)
        #obj.control()

Wondering what could be the problem. Thanks in advance

Edit:

For multiple robots I just took the file from the turtlebot3gazebo pack, this file: https://github.com/ROBOTIS-GIT/turtlebot3simulations/blob/master/turtlebot3gazebo/launch/multiturtlebot3.launch , and just made adjustments so that there are two turtlebots instead of 3, and then just added the roslinkattacher plugin and an aruco marker to the world.

This is the tf tree image description

This is my world file:

<sdf version='1.6'>
  <world name='default'>
    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>

    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>

        <!--
     <include>
      <uri>model://marker</uri>
    </include>
    -->


    <plugin name="ros_link_attacher_plugin" filename="libgazebo_ros_link_attacher.so" />

    <physics type="ode">
      <real_time_update_rate>1000.0</real_time_update_rate>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <ode>
        <solver>
          <type>quick</type>
          <iters>150</iters>
          <precon_iters>0</precon_iters>
          <sor>1.400000</sor>
          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
        </solver>
        <constraints>
          <cfm>0.00001</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
          <contact_surface_layer>0.01000</contact_surface_layer>
        </constraints>
      </ode>
    </physics>

    <!-- Load world -->
    <include>
      <uri>model://turtlebot3_square</uri>
    </include>

    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>true</shadows>
    </scene>

    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose>0.0 0.0 17.0 0 1.5708 0</pose>
        <view_controller>orbit</view_controller>
      </camera>
    </gui>
  </world>
</sdf>

and this is my launch file:

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="first_tb3"  default="tb3_0"/>
  <arg name="second_tb3" default="tb3_1"/>

  <arg name="first_tb3_x_pos" default="0.0"/>
  <arg name="first_tb3_y_pos" default="1.0"/>
  <arg name="first_tb3_z_pos" default=" 0.0"/>
  <arg name="first_tb3_yaw"   default=" 1.57"/>

  <arg name="second_tb3_x_pos" default=" 0.0"/>
  <arg name="second_tb3_y_pos" default="0.0"/>
  <arg name="second_tb3_z_pos" default=" 0.0"/>
  <arg name="second_tb3_yaw"   default=" 1.57"/>



  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/custom_world2.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>  


    <!-- Start marker detector--> 
  <node name="aruco_detect" pkg="aruco_detect" type="aruco_detect">
    <param name="image_transport" value="compressed" />
    <param name="publish_images" value="true" />
    <param name="fiducial_len" value="0.15" />
    <param name="dictionary" value="0" />
    <param name="do_pose_estimation" value="true" />
    <param name="ignore_fiducials" value="" />
    <param name="fiducial_len_override" value="" />
    <remap from="/camera/compressed" to="/tb3_1/camera/rgb/image_raw/compressed"/>
    <remap from="/camera_info" to="/tb3_1/camera/rgb/camera_info"/>
  </node> 


  <group ns = "$(arg first_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg first_tb3)" />
    </node>

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg first_tb3) -x $(arg first_tb3_x_pos) -y $(arg first_tb3_y_pos) -z $(arg first_tb3_z_pos) -Y $(arg first_tb3_yaw) -param robot_description" />
  </group>

  <group ns = "$(arg second_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg second_tb3)" />
    </node>

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg second_tb3) -x $(arg second_tb3_x_pos) -y $(arg second_tb3_y_pos) -z $(arg second_tb3_z_pos) -Y $(arg second_tb3_yaw) -param robot_description" />
  </group>

</launch>

Asked by Roshan on 2021-10-05 07:20:31 UTC

Comments

Answers

The problem is not on the listener side but on the publisher side. The Listener is complaining that it's receiving duplicate transforms from potentially one or more publisher with the same timestamp for the same link. You'll want to look at who's publishing transforms on the /tf topic. Without more information I think that there's two possibilities. The first is that /robot_state_publisher itself is double publishing. It's driven by the incoming JointState messages which may be duplicated. Alternatively there could be another node publishing the transforms which are causing the conflict.

Asked by tfoote on 2021-10-05 11:12:26 UTC

Comments

Hello, thank you for the answer. I checked the publishers on the /tf topic:

Type: tf2_msgs/TFMessage

Publishers: 
 * /tb3_0/robot_state_publisher (http://roshan-Komplett-PC:44071/)
 * /tb3_1/robot_state_publisher (http://roshan-Komplett-PC:36189/)
 * /aruco_detect (http://roshan-Komplett-PC:38331/)
 * /gazebo (http://roshan-Komplett-PC:35495/)

Subscribers: None

I forgot to mention that I am running two turtlebots in the same simulation, but could that be the issue? I don't think so since when using the turtlebot3_gazebo simulation with multiple turtlebots it works fine. So it must be something with aruco_detect, but how do I know what the issue is?

Asked by Roshan on 2021-10-05 12:48:45 UTC

Yes, that will definitely cause an issue if you haven't taken precautions. if you have two robots running at the same time, they need to have different coordinate frames, or you need to remap them to use different /tf topics.

Asked by tfoote on 2021-10-05 14:35:25 UTC

Ok, thank you. I find it kind of weird that this wasn't already done in the gazebo simulation files where they use several turtlebots. I could show the tf tree if that would help, and if you could point in the direction of resources that show how this is done/tutorials that would be amazing.

Asked by Roshan on 2021-10-05 14:54:54 UTC

I would expect that they do have a solution to this problem. I can't speak to it as I don't know what tutorial you're following. In the future if you're following a tutorial you should link to it in your question as part of the background reference. That would have helped me a lot when trying to understand your issue earlier.

Asked by tfoote on 2021-10-05 15:22:29 UTC

Thank you, I'll keep that in mind for next time. For multiple robots I just took the file from the turtlebot3_gazebo pack, this file: https://github.com/ROBOTIS-GIT/turtlebot3_simulations/blob/master/turtlebot3_gazebo/launch/multi_turtlebot3.launch, and just made adjustments so that there are two turtlebots instead of 3, and then just added the ros_link_attacher plugin and an aruco marker to the world

Asked by Roshan on 2021-10-05 15:37:34 UTC

From that tutorial they do have adjustments for multiple TurtleBots. Note that they parameterize the urdf here: https://github.com/ROBOTIS-GIT/turtlebot3_simulations/blob/8241af04a4e56e656d4be24a5c3971e3359bf751/turtlebot3_gazebo/launch/multi_turtlebot3.launch#L32 you should make sure in your adaptation that you're doing the right logic. Again this would be useful to include in your question up front.

Asked by tfoote on 2021-10-05 15:42:50 UTC

I've now added the world file, launch file and tf tree to the question. I have the parameterization but the problem is still there

Asked by Roshan on 2021-10-05 16:03:30 UTC

Thanks for adding those. It looks like the tf_prefix parameter isn't taking effect appropriately. The output frames in your error message do no reflect the prefixes as it looks like they should be evaluated.

Asked by tfoote on 2021-10-05 18:13:18 UTC

Thank you for your help, I have tried reiterating the question a little bit to make it clearer, and maybe show off better what might be the problem here: https://answers.ros.org/question/388044/tf-tree-for-multiple-turtlebots/.

I also found this about prefixes: https://github.com/ros/robot_state_publisher/pull/139. Does this mean it does not work on noetic?

Asked by Roshan on 2021-10-06 04:15:16 UTC

No it should work in Noetic there's a patch here that superceded that one: https://github.com/ros/robot_state_publisher/pull/169

Asked by tfoote on 2021-10-06 05:11:35 UTC