rclpy.spin_until_future_complete() removes node from executor added by rclpy.spin()

asked 2022-03-31 10:18:43 -0500

bjoernolav gravatar image

I'm writing a ROS2 rclpy node (using Galactic and Ubuntu 20.04LTS) that is executed using rclpy.spin, but also uses arclpy.spin_until_future_complete for waiting for a TF transform to become available. I'm having trouble with the node stopping execution, due to the rclpy.spin_until_future_complete unregistering the node in the SingleThreadedExecutor, making it stop.

I'm not sure if this is intentional, or if I'm doing something unappropriate here. Initially I tried just looking up the transform using lookup_transform with a timeout, but this blocks the executor in whole making the TF transform callbacks not being served - therefore I'm attempting to use the wait_for_transform_async function instead.

I'm getting this to work by replacing the rclpy.spin_until_future_complete() with my own using rclpy.spin_once(), but that seems like an unintended workaround.

MWE:

import rclpy
from rclpy.time import Time
from rclpy.node import Node
import geometry_msgs.msg as geomsgs
import tf2_ros


class Blah(Node):
    def __init__(self):
        super().__init__('blahblah')

        # Set up tf
        self.__tf_buffer = tf2_ros.Buffer()
        self.__tf_listener = tf2_ros.TransformListener(self.__tf_buffer, self)

        self.__timer = self.create_timer(
            1., self.run_once)

    def run_once(self):
        self.get_logger().info('Running, list of nodes in executor: {}'.format(self.executor.get_nodes()))

        # Look up transform
        self._lookup_transform('frame1', 'frame2')

    def _lookup_transform(self, target_frame: str, source_frame: str) -> geomsgs.TransformStamped:
        # Wait for transform using future
        t_trans = Time(seconds=0)
        transform_available_future = self.__tf_buffer.wait_for_transform_async(
            target_frame, source_frame, t_trans)

        self.get_logger().info('Executor nodes before spin_until_future_complete: {}'.format(self.executor.get_nodes()))
        rclpy.spin_until_future_complete(
            self, transform_available_future, timeout_sec=1.0)
        self.get_logger().info('Executor nodes after spin_until_future_complete: {}'.format(self.executor.get_nodes()))

        # Get transform
        try:
            trans = self.__tf_buffer.lookup_transform(
                target_frame, source_frame, t_trans)
        except tf2_ros.TransformException as e:
            self.get_logger().fatal('TF lookup exception {}'.format(e))
            raise e
        return trans


def main():
    rclpy.init()
    node = Blah()
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == "__main__":
    main()

Running the node, I get the following output:

[INFO] [1648739520.263017100] [blahblah]: Running, list of nodes in executor: [<package.blah.Blah object at 0x7fc77e2c6a90>]
[INFO] [1648739520.263507200] [blahblah]: Executor nodes before spin_until_future_complete: [<package.blah.Blah object at 0x7fc77e2c6a90>]
[INFO] [1648739520.264210100] [blahblah]: Executor nodes after spin_until_future_complete: []
edit retag flag offensive close merge delete

Comments

Were you able to solve it?

märve gravatar image märve  ( 2022-07-06 08:19:38 -0500 )edit

Not really. I made it work using rclpy.spin_once()instead. It's probably better to use a MultiThreadedExecutor instead, and let the thread block until the transform becomes available. The TF listener uses a seaparate callback group (https://github.com/ros2/geometry2/blo...) - so using this approach will make the TF callback being handled by a separate thread.

bjoernolav gravatar image bjoernolav  ( 2022-07-12 15:06:12 -0500 )edit