ros2 / rclpy

rclpy (ROS Client Library for Python)
Apache License 2.0
308 stars 227 forks source link

Callback Groups Leak Memory #1356

Open jpfeltracco opened 2 months ago

jpfeltracco commented 2 months ago

Bug report

Required Info:

Steps to reproduce issue

from weakref import WeakSet
import rclpy
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup, CallbackGroup
from rclpy.executors import MultiThreadedExecutor

class WeaksetReentrantCallbackGroup(CallbackGroup):
    def __init__(self) -> None:
        super().__init__()
        self.entities: WeakSet = WeakSet()

    def add_entity(self, entity) -> None:
        self.entities.add(entity)

    def has_entity(self, entity) -> bool:
        return entity in self.entities

    def can_execute(self, entity) -> bool:
        return True

    def beginning_execution(self, entity) -> bool:
        return True

    def ending_execution(self, entity) -> None:
        pass

class TimerTestNode(Node):
    def __init__(self):
        super().__init__("timer_test_node")
        self.callback_group = ReentrantCallbackGroup()
        # self.callback_group = WeaksetReentrantCallbackGroup()
        self.mytimers = set()
        self.create_timer(
            1.0, self.create_timers_callback, callback_group=self.callback_group
        )
        self.create_timer(
            1.0, self.log_entities_callback, callback_group=self.callback_group
        )

    def create_oneshot_timer(self):
        future = rclpy.Future()
        timer = self.create_timer(
            0.1,
            lambda: future.set_result("done"),
            callback_group=self.callback_group,
        )
        future.add_done_callback(lambda _: self.timer_done_callback(timer))
        return timer

    def create_timers_callback(self):
        for _ in range(100):
            timer = self.create_oneshot_timer()
            self.mytimers.add(timer)

    def timer_done_callback(self, timer_instance):
        assert self.destroy_timer(timer_instance)
        self.mytimers.remove(timer_instance)

    def log_entities_callback(self):
        num_entities = len(self.callback_group.entities)
        self.get_logger().info(
            f"Number of entities in the callback group: {num_entities}"
        )

def main(args=None):
    rclpy.init(args=args)
    node = TimerTestNode()
    executor = MultiThreadedExecutor()

    executor.add_node(node)
    executor.spin()

if __name__ == "__main__":
    main()

Expected behavior

Weakrefs are cleaned up as timers are destroyed, values stays ~100 or briefly goes up then back down.

Actual behavior

Forever increasing weakref set size.

Additional information

You can see my alternative callback group implementation that uses a weak set to cull dead entries automatically. Swap to it and you see the entity count in the callback group stays reasonable.