Ask Your Question
2

Waiting for TransformListener with ROS2 and Python?

asked 2021-02-01 18:25:17 -0600

dognaught47 gravatar image

I'm trying to build code that listens to transforms between two frames. About 50% of the time that I run the following code I get an error:

"tf2.LookupException: "front_bumper" passed to lookupTransform argument target_frame does not exist."

As far as I can tell, passing in any value for Duration has no effect despite what I believe the C++ documentation suggests.

What is the right way to wait for the transform to show up? Am I better off trying to do this in C++?

Environment is Ubuntu 20.04.1 LTS & ROS2 Foxy

#! /usr/bin/env python
from math import sin, cos, pi
import threading
import rclpy
import time

from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformListener, TransformStamped, Buffer
class StatePublisher(Node):

  def __init__(self):
      rclpy.init()
      super().__init__('state_publisher')

      self.nodeName = self.get_name()
      self.get_logger().info("{0} started".format(self.nodeName))

      tfBuffer = Buffer()
      listener = TransformListener(tfBuffer, self)

      try:
          while rclpy.ok():
              rclpy.spin_once(self)

              now = self.get_clock().now()

              dur = Duration()
              dur.sec = 40
              dur.nsec = 0

              trans = tfBuffer.lookup_transform('front_bumper', 'base_link', now, dur )

              self.get_logger().info("\n")
              self.get_logger().info("Translation:")
              self.get_logger().info( "x:" + str( trans.transform.translation.x ) )
              self.get_logger().info( "y:" + str( trans.transform.translation.y ) )
              self.get_logger().info( "z:" + str( trans.transform.translation.z ) )


              self.get_logger().info("Rotation (in quaternion):")
              self.get_logger().info( "w:" + str( trans.transform.rotation.w ) )
              self.get_logger().info( "x:" + str( trans.transform.rotation.x ) )
              self.get_logger().info( "y:" + str( trans.transform.rotation.y ) )
              self.get_logger().info( "z:" + str( trans.transform.rotation.z ) ) 
              time.sleep(1)
      except KeyboardInterrupt:
          pass





def main():
  node = StatePublisher()

if __name__ == '__main__':
  main()
~
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-02-05 20:09:20 -0600

dognaught47 gravatar image

updated 2021-02-06 11:42:58 -0600

I'm embarrassed to admit I went about this all wrong. Instead I should have just subscribed to the topic publishing the information I cared about instead of trying to get at the resulting frame transform.

First, a little context.

I'm new to robotics and ROS2. I'm building a custom robot as my learning platform. The robot contains a RealSense T265 and T455, both of which are mounted on top of a pan and tilt system. Fixed to the robot base is a ydlidar g2 laser scanner.

When I posted this question, I was experiencing an issue in that rviz2 Camera output showed the laser scan representation following the rgb camera when the pan and tilt was adjusted because there was no transform being applied to the joints that held the frame attached to the RealSense cameras. I was thrown off because I noticed that the T265 tracking camera published orientation to /odom_frame, therefore I figured (incorrectly) that I needed to get the transform from there. This is how I got to posting this question in the first place which was intended as a simplified version of what I was trying to ultimately accomplish.

This is how I started down the path of getting the transform between two frames. My next issue was that I was using the tf2 tutorial as my example to grab the transform. This tutorial seems to be written for ROS1, therefore it doesn't work out of the box on ROS2. This confusion is what led to my original question.

It turns out that it is MUCH simpler to follow the ROS2 topic subscription tutorial and adapt it to subscribe to the /tf topic. There is no need to mess around with TransformListener or Buffer which didn't seem to work as expected anyway. While this solves my particular since I was seeking the transforms between movable joints, this doesn't actually solve the more general problem I originally posted because I don't think /tf shows transforms for fixed joints (something I didn't understand when I wrote the question).

I then realized that the T265 is publishing an Odometry topic which is exactly what I needed to update the joint state of the pan and tilt. Therefore, the solution to my actual issue was to a) subscribe to the Odometry topic from the T265 and then b) publish to the /joint_states topic to update the orientation of the pan and tilt.

edit flag offensive delete link more

Comments

1

Please clarify: what do you mean by

I should have just subscribed to the topic publishing the information I cared about

Using TF to lookup the transform between two frames is perfectly OK and is typically how this should be done.

gvdhoorn gravatar image gvdhoorn  ( 2021-02-06 04:47:28 -0600 )edit

I updated the answer with more context. Hopefully this helps someone else!

dognaught47 gravatar image dognaught47  ( 2021-02-06 11:32:13 -0600 )edit
1

It turns out that it is MUCH simpler to [..] subscribe to the /tf topic.

perhaps, but it's also somewhat of an anti-pattern with TF.

I'd advise against doing that.

Using a regular listener and lookup would also solve this:

this doesn't actually solve the more general problem I originally posted because I don't think /tf shows transforms for fixed joints

a lookup using TF would also work for / across static transforms.

What you're essentially doing now is the manual version of what lookup_transform(..) does.

gvdhoorn gravatar image gvdhoorn  ( 2021-02-06 12:30:39 -0600 )edit
1

Can you explain more why you advise against subscribing to the /tf topic in this case?

There is also the matter of there being no known solution to using lookup_transform() in python on ros2. I suspect that the python API for tf2 isn't operating as intended in this situation. I was able to get that to work in c++ as part of my debugging process but I'd prefer to use python since it's been 20+ years since I really did much in c++!

dognaught47 gravatar image dognaught47  ( 2021-02-06 12:40:58 -0600 )edit
1

Can you explain more why you advise against subscribing to the /tf topic in this case?

While TF works with ROS messages, looking up transforms takes the entire tree into account, so it doesn't work with single messages directly, but builds an in-memory representation of the complete transform tree based on those messages.

If you subscribe to /tf yourself, you now:

  1. have to filter all incoming messages keeping only the one you are interested in
  2. perform all the transforms yourself

things like looking up a transform at a specific point in time are also going to be much more work: you'd have to manually do all the interpolation and make sure the results are correct and deal with the edge cases.

Among other things.

TF does all of that for you.

I've not looked at the specific problem, or the problems with TF in Python in ROS ...(more)

gvdhoorn gravatar image gvdhoorn  ( 2021-02-06 12:48:15 -0600 )edit
1

There's no known fundamental problem with tf2 and python. You can see we have basic unit test coverage here: https://github.com/ros2/geometry2/blo...

On thing that I'll note is that the timeout is never going to help as you are not spinning anything during the timeout. You're explicitly pumping the subscriptions yourself, but that means that it's never going to receive any data while your in the timeout period. This can probably keep your code from ever operating. There's an option on the listener to spin it's own thread to help with this spin_thread. If you really only need one link with the latest data then directly subscribing can be simpler. However as your use case gets more complicated you'll find the tree spanning solutions and ability to transform data and not worry about specific frames of reference to be much more useful.

tfoote gravatar image tfoote  ( 2021-02-06 15:12:51 -0600 )edit

Thank you for the help!!

dognaught47 gravatar image dognaught47  ( 2021-02-08 08:59:38 -0600 )edit

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-02-01 18:18:08 -0600

Seen: 737 times

Last updated: Feb 06