Link in rviz teleports between original and transformed position with tf2

Question

Here's a video of what I'm referring to. I'm using rviz to display a transformation using a tf2 broadcaster, but link1 keeps popping back and forth between its specified location from the URDF, and the location that the tf2 message sent. How can I get it to stay where it transformed to?

Additionally, since link2 is the child of link1, why doesn't link2 move along with link1?

Here's the python code:

import rospy
import tf2_ros
import geometry_msgs.msg
import math

if __name__ == '__main__':
    rospy.init_node('dynamic_tf2_broadcaster')
    br = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

    t.header.frame_id = "body"
    t.child_frame_id = "link1"

    rate = rospy.Rate(100.0)
    while not rospy.is_shutdown():
        x = rospy.Time.now().to_sec() * math.pi

        t.header.stamp = rospy.Time.now()
        t.transform.translation.x = 10 * math.sin(x)
        t.transform.translation.y = 10 * math.cos(x)
        t.transform.translation.z = 0.0
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0

        br.sendTransform(t)
        rate.sleep()

And here's the rviz launch file:

<launch>
<arg name="gui" default="True" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find basic_bot)/description/urdf/robot.xacro'"/>
<param name="use_gui" value="$(arg gui)"/>

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find basic_bot)/rviz/robot.rviz" required="true" />

</launch>

---

Originally posted by Victoria Bentell on ROS Answers with karma: 3 on 2019-12-17

Post score: 0

Answer 1

How can I get it to stay where it transformed to?

I haven't checked, but is the link you're broadcasting the transform for part of your URDF? If it is, both the robot_state_publisher and your own script will be broadcasting transforms for it, leading to the behaviour you describe.

---

Edit:

Yes, it is part of the URDF

Ok. That would make robot_state_publisher broadcast transforms for it.

and I removed the robot_state_publisher node from the launch file and initialized the positions of both links directly in the python file. Is this common practice, or is there a more standard way? I ask because although I could now represent the rotations of the links using quaternions, the model is no longer listening to joint_state_publisher.

re: common practice: I'm not sure what you're asking exactly.

A TF frame can have multiple broadcasters, but if those broadcasters do not agree on what the transform should be, the frame will appear to "jump" between different locations, as you experienced.

This makes sense to me: there are essentially multiple "oracles" trying to convince us that frame X is at location Y and location Z and location W. In most cases, this is impossible, and it also leads to visualisation strangeness, as the frame is jumping between these locations.

By having the link in the URDF, robot_state_publisher will assume it is responsible for publishing the transforms related to the link. If you then start broadcasting the same transform(s), problems occur.

I could now represent the rotations of the links using quaternions, the model is no longer listening to joint_state_publisher.

And this makes sense: robot_state_publisher listens to the joint_states topic to receive input for its forward-kinematics (FK). That FK is used to broadcast the transforms for the links in the URDF.

You've removed the links from the URDF -> robot_state_publisher is not longer responsible, so doesn't listen for any joint states for the joints connected to those links -> no transforms -> no "response" when manipulating the sliders in the joint_state_publisher.

---

Originally posted by gvdhoorn with karma: 86574 on 2019-12-17

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Victoria Bentell on 2019-12-17:
Yes, it is part of the URDF, and I removed the robot_state_publisher node from the launch file and initialized the positions of both links directly in the python file. Is this common practice, or is there a more standard way? I ask because although I could now represent the rotations of the links using quaternions, the model is no longer listening to joint_state_publisher.

Comment by Victoria Bentell on 2019-12-17:
It seems to me like I have to choose between using robot_state_publisher in my launch file (where I'm allowed to move the robot with joint_state_publisher) and using a TransformBroadcaster (where each link can move using quaternions, but can also detach and joint angles are lost). So as far as the "common practice" comment goes, when working with a single robot, is it best to control it with robot_state_publisher or TransformBroadcaster?

Comment by gvdhoorn on 2019-12-17:
You're presenting this as an either/or, but that's not the case.

It's possible to get both, but you'll have to make sure to parent your transforms to the ones robot_state_publisher is publishing, listen to JointState messages yourself and do FK. Then you could have both.

robot_state_publisher does fairly simple FK. If you need something more complex, or you don't have JointState messages but some other way to derive the transforms for your robot, you could use a broadcaster.

where I'm allowed to move the robot with joint_state_publisher

pedantic, but: you're not moving the robot. You're making joint_state_publisher (RSP) publish different values for certain joints in its JointState message, which is then picked up by robot_state_publisher and via FK these changes are forwarded to the transforms. This is then used (by RViz for example) to update the 3D visualisation of a robot model. I feel this is important as RSP is not directly "moving the robot".