Python rclpy.spin_once() Examples

def test_create_task_coroutine(self):
        self.assertIsNotNone(self.node.handle)
        executor = SingleThreadedExecutor(context=self.context)
        executor.add_node(self.node)

        async def coroutine():
            return 'Sentinel Result'

        future = executor.create_task(coroutine)
        self.assertFalse(future.done())

        executor.spin_once(timeout_sec=0)
        self.assertTrue(future.done())
        self.assertEqual('Sentinel Result', future.result()) 

def _rostopic_bw(node, topic, window_size=DEFAULT_WINDOW_SIZE):
    """Periodically print the received bandwidth of a topic to console until shutdown."""
    # pause bw until topic is published
    msg_class = get_msg_class(node, topic, blocking=True, include_hidden_topics=True)
    if msg_class is None:
        node.destroy_node()
        return

    rt = ROSTopicBandwidth(node, window_size)
    node.create_subscription(
        msg_class,
        topic,
        rt.callback,
        qos_profile_sensor_data,
        raw=True
    )

    print(f'Subscribed to [{topic}]')
    timer = node.create_timer(1, rt.print_bw)
    while rclpy.ok():
        rclpy.spin_once(node)

    node.destroy_timer(timer)
    node.destroy_node()
    rclpy.shutdown() 

def test_remove_node(self):
        self.assertIsNotNone(self.node.handle)
        executor = SingleThreadedExecutor(context=self.context)

        got_callback = False

        def timer_callback():
            nonlocal got_callback
            got_callback = True

        try:
            tmr = self.node.create_timer(0.1, timer_callback)
            try:
                executor.add_node(self.node)
                executor.remove_node(self.node)
                executor.spin_once(timeout_sec=0.2)
            finally:
                self.node.destroy_timer(tmr)
        finally:
            executor.shutdown()

        assert not got_callback 

def timed_spin(self, duration):
        start_time = time.time()
        while (time.time() - start_time) < duration:
            rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) 

def timed_spin(self, duration):
        start_time = time.time()
        while (time.time() - start_time) < duration:
            rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) 

def main(args=None):
    rclpy.init(args=args)

    minimal_client = MinimalClientAsync()
    minimal_client.send_request()

    while rclpy.ok():
        rclpy.spin_once(minimal_client)
        if minimal_client.future.done():
            try:
                response = minimal_client.future.result()
            except Exception as e:
                minimal_client.get_logger().info(
                    'Service call failed %r' % (e,))
            else:
                minimal_client.get_logger().info(
                    'Result of add_two_ints: for %d + %d = %d' %
                    (minimal_client.req.a, minimal_client.req.b, response.sum))
            break

    minimal_client.destroy_node()
    rclpy.shutdown() 

def test_executor_add_node_wakes_executor(self):
        self.assertIsNotNone(self.node.handle)
        got_callback = False

        def timer_callback():
            nonlocal got_callback
            got_callback = True

        timer_period = 0.1
        tmr = self.node.create_timer(timer_period, timer_callback)

        executor = SingleThreadedExecutor(context=self.context)
        try:
            # spin in background
            t = threading.Thread(target=executor.spin_once, daemon=True)
            t.start()
            # sleep to make sure executor is blocked in rcl_wait
            time.sleep(0.5)

            self.assertTrue(executor.add_node(self.node))
            # Make sure timer has time to trigger
            time.sleep(timer_period)

            self.assertTrue(got_callback)
        finally:
            executor.shutdown()
            self.node.destroy_timer(tmr) 

def test_global_executor_completes_async_task(self):
        self.assertIsNotNone(self.node.handle)

        class TriggerAwait:

            def __init__(self):
                self.do_yield = True

            def __await__(self):
                while self.do_yield:
                    yield
                return

        trigger = TriggerAwait()
        did_callback = False
        did_return = False

        async def timer_callback():
            nonlocal trigger, did_callback, did_return
            did_callback = True
            await trigger
            did_return = True

        timer = self.node.create_timer(0.1, timer_callback)

        executor = SingleThreadedExecutor(context=self.context)
        rclpy.spin_once(self.node, timeout_sec=0.5, executor=executor)
        self.assertTrue(did_callback)

        timer.cancel()
        trigger.do_yield = False
        rclpy.spin_once(self.node, timeout_sec=0, executor=executor)
        self.assertTrue(did_return) 

def test_create_task_during_spin(self):
        self.assertIsNotNone(self.node.handle)
        executor = SingleThreadedExecutor(context=self.context)
        executor.add_node(self.node)

        future = None

        def spin_until_task_done(executor):
            nonlocal future
            while future is None or not future.done():
                try:
                    executor.spin_once()
                finally:
                    executor.shutdown()
                    break

        # Start spinning in a separate thread
        thr = threading.Thread(target=spin_until_task_done, args=(executor, ), daemon=True)
        thr.start()

        # Sleep in this thread to give the executor a chance to reach the loop in
        # '_wait_for_ready_callbacks()'
        time.sleep(1)

        def func():
            return 'Sentinel Result'

        # Create a task
        future = executor.create_task(func)

        thr.join(timeout=0.5)
        # If the join timed out, remove the node to cause the spin thread to stop
        if thr.is_alive():
            executor.remove_node(self.node)

        self.assertTrue(future.done())
        self.assertEqual('Sentinel Result', future.result()) 

def test_create_task_dependent_coroutines(self):
        self.assertIsNotNone(self.node.handle)
        executor = SingleThreadedExecutor(context=self.context)
        executor.add_node(self.node)

        async def coro1():
            return 'Sentinel Result 1'

        future1 = executor.create_task(coro1)

        async def coro2():
            nonlocal future1
            await future1
            return 'Sentinel Result 2'

        future2 = executor.create_task(coro2)

        # Coro2 is newest task, so it gets to await future1 in this spin
        executor.spin_once(timeout_sec=0)
        # Coro1 execs in this spin
        executor.spin_once(timeout_sec=0)
        self.assertTrue(future1.done())
        self.assertEqual('Sentinel Result 1', future1.result())
        self.assertFalse(future2.done())

        # Coro2 passes the await step here (timeout change forces new generator)
        executor.spin_once(timeout_sec=1)
        self.assertTrue(future2.done())
        self.assertEqual('Sentinel Result 2', future2.result()) 

def test_execute_coroutine_timer(self):
        self.assertIsNotNone(self.node.handle)
        executor = SingleThreadedExecutor(context=self.context)
        executor.add_node(self.node)

        called1 = False
        called2 = False

        async def coroutine():
            nonlocal called1
            nonlocal called2
            called1 = True
            await asyncio.sleep(0)
            called2 = True

        tmr = self.node.create_timer(0.1, coroutine)
        try:
            executor.spin_once(timeout_sec=1.23)
            self.assertTrue(called1)
            self.assertFalse(called2)

            called1 = False
            executor.spin_once(timeout_sec=0)
            self.assertFalse(called1)
            self.assertTrue(called2)
        finally:
            self.node.destroy_timer(tmr) 

def test_executor_spin_non_blocking(self):
        self.assertIsNotNone(self.node.handle)
        executor = SingleThreadedExecutor(context=self.context)
        executor.add_node(self.node)
        start = time.monotonic()
        executor.spin_once(timeout_sec=0)
        end = time.monotonic()
        self.assertLess(start - end, 0.001) 

def set_use_sim_time_parameter(self, value):
        self.node.set_parameters(
            [Parameter('use_sim_time', Parameter.Type.BOOL, value)])
        executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
        cycle_count = 0
        while rclpy.ok(context=self.context) and cycle_count < 5:
            use_sim_time_param = self.node.get_parameter('use_sim_time')
            cycle_count += 1
            if use_sim_time_param.type_ == Parameter.Type.BOOL:
                break

            rclpy.spin_once(self.node, timeout_sec=1, executor=executor)
            time.sleep(1)
        return use_sim_time_param.value == value 

def publish_reversed_clock_messages(self):
        clock_pub = self.node.create_publisher(rosgraph_msgs.msg.Clock, CLOCK_TOPIC, 1)
        cycle_count = 0
        time_msg = rosgraph_msgs.msg.Clock()
        while rclpy.ok(context=self.context) and cycle_count < 5:
            time_msg.clock.sec = 6 - cycle_count
            clock_pub.publish(time_msg)
            cycle_count += 1
            executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
            rclpy.spin_once(self.node, timeout_sec=1, executor=executor)
            time.sleep(1) 

def publish_clock_messages(self):
        clock_pub = self.node.create_publisher(rosgraph_msgs.msg.Clock, CLOCK_TOPIC, 1)
        cycle_count = 0
        time_msg = rosgraph_msgs.msg.Clock()
        while rclpy.ok(context=self.context) and cycle_count < 5:
            time_msg.clock.sec = cycle_count
            clock_pub.publish(time_msg)
            cycle_count += 1
            executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
            rclpy.spin_once(self.node, timeout_sec=1, executor=executor)
            # TODO(dhood): use rate once available
            time.sleep(1) 

def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_client_async')

    cli = node.create_client(AddTwoInts, 'add_two_ints')

    req = AddTwoInts.Request()
    req.a = 41
    req.b = 1
    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('service not available, waiting again...')

    future = cli.call_async(req)
    while rclpy.ok():
        rclpy.spin_once(node)
        if future.done():
            try:
                result = future.result()
            except Exception as e:
                node.get_logger().info('Service call failed %r' % (e,))
            else:
                node.get_logger().info(
                    'Result of add_two_ints: for %d + %d = %d' %
                    (req.a, req.b, result.sum))
            break

    node.destroy_node()
    rclpy.shutdown() 

def main(args=None):
    global g_node
    rclpy.init(args=args)

    g_node = rclpy.create_node('minimal_service')

    srv = g_node.create_service(AddTwoInts, 'add_two_ints', add_two_ints_callback)
    while rclpy.ok():
        rclpy.spin_once(g_node)

    # Destroy the service attached to the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    g_node.destroy_service(srv)
    rclpy.shutdown() 

def main(args=None):
    global g_node
    rclpy.init(args=args)

    g_node = rclpy.create_node('minimal_subscriber')

    subscription = g_node.create_subscription(String, 'topic', chatter_callback, 10)
    subscription  # prevent unused variable warning

    while rclpy.ok():
        rclpy.spin_once(g_node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    g_node.destroy_node()
    rclpy.shutdown() 

def replier(service_pkg, service_name, number_of_cycles, namespace):
    import rclpy

    module = importlib.import_module(service_pkg + '.srv')
    srv_mod = getattr(module, service_name)

    rclpy.init(args=[])

    node = rclpy.create_node('replier', namespace=namespace)

    req = srv_mod.Request()
    resp = srv_mod.Response()

    srv_fixtures = [[req, resp]]

    chatter_callback = functools.partial(
        replier_callback, srv_fixtures=srv_fixtures)

    node.create_service(
        srv_mod, 'test/service/' + service_name, chatter_callback)

    spin_count = 0
    print('replier: beginning loop')
    while rclpy.ok() and spin_count < number_of_cycles:
        rclpy.spin_once(node, timeout_sec=2)
        spin_count += 1
        # print('spin_count: ' + str(spin_count))
    node.destroy_node()
    rclpy.shutdown() 

def _rostopic_delay(node, topic, window_size=DEFAULT_WINDOW_SIZE):
    """
    Periodically print the publishing delay of a topic to console until shutdown.

    :param topic: topic name, ``str``
    :param window_size: number of messages to average over, ``unsigned_int``
    :param blocking: pause delay until topic is published, ``bool``
    """
    # pause hz until topic is published
    msg_class = get_msg_class(node, topic, blocking=True, include_hidden_topics=True)

    if msg_class is None:
        node.destroy_node()
        return

    rt = ROSTopicDelay(node, window_size)
    node.create_subscription(
        msg_class,
        topic,
        rt.callback_delay,
        qos_profile_sensor_data)

    timer = node.create_timer(1, rt.print_delay)
    while rclpy.ok():
        rclpy.spin_once(node)

    node.destroy_timer(timer)
    node.destroy_node()
    rclpy.shutdown() 

def __init__(self, args, *, node_name=None):
        timeout_reached = False

        def timer_callback():
            nonlocal timeout_reached
            timeout_reached = True

        argv = getattr(args, 'argv', [])

        rclpy.init(args=argv)

        node_name_suffix = getattr(
            args, 'node_name_suffix', '_%d' % os.getpid())
        start_parameter_services = getattr(
            args, 'start_parameter_services', False)

        if node_name is None:
            node_name = NODE_NAME_PREFIX + node_name_suffix

        self.node = rclpy.create_node(
            node_name,
            start_parameter_services=start_parameter_services)
        timeout = getattr(args, 'spin_time', DEFAULT_TIMEOUT)
        timer = self.node.create_timer(timeout, timer_callback)

        while not timeout_reached:
            rclpy.spin_once(self.node)

        self.node.destroy_timer(timer) 

def call_change_states(*, node, transitions):
    clients = {}
    futures = {}
    # create clients
    for node_name in transitions.keys():
        clients[node_name] = node.create_client(
            ChangeState, f'{node_name}/change_state')

    # wait until all clients have been called
    while True:
        for node_name in [n for n in transitions.keys() if n not in futures]:
            # call as soon as ready
            client = clients[node_name]
            if client.service_is_ready():
                request = ChangeState.Request()
                request.transition = transitions[node_name]
                future = client.call_async(request)
                futures[node_name] = future

        if len(futures) == len(clients):
            break
        rclpy.spin_once(node, timeout_sec=1.0)

    # wait for all responses
    for future in futures.values():
        rclpy.spin_until_future_complete(node, future)

    # return success flag or exception for each node
    results = {}
    for node_name, future in futures.items():
        if future.result() is not None:
            response = future.result()
            results[node_name] = response.success
        else:
            results[node_name] = future.exception()
    return results 

def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node('minimal_client')
    # Node's default callback group is mutually exclusive. This would prevent the client response
    # from being processed until the timer callback finished, but the timer callback in this
    # example is waiting for the client response
    cb_group = ReentrantCallbackGroup()
    cli = node.create_client(AddTwoInts, 'add_two_ints', callback_group=cb_group)
    did_run = False
    did_get_result = False

    async def call_service():
        nonlocal cli, node, did_run, did_get_result
        did_run = True
        try:
            req = AddTwoInts.Request()
            req.a = 41
            req.b = 1
            future = cli.call_async(req)
            try:
                result = await future
            except Exception as e:
                node.get_logger().info('Service call failed %r' % (e,))
            else:
                node.get_logger().info(
                    'Result of add_two_ints: for %d + %d = %d' %
                    (req.a, req.b, result.sum))
        finally:
            did_get_result = True

    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('service not available, waiting again...')

    timer = node.create_timer(0.5, call_service, callback_group=cb_group)

    while rclpy.ok() and not did_run:
        rclpy.spin_once(node)

    if did_run:
        # call timer callback only once
        timer.cancel()

    while rclpy.ok() and not did_get_result:
        rclpy.spin_once(node)

    node.destroy_node()
    rclpy.shutdown() 

def listener(message_pkg, message_name, namespace):
    import rclpy

    module = importlib.import_module(message_pkg + '.msg')
    msg_mod = getattr(module, message_name)

    rclpy.init(args=[])

    node = rclpy.create_node('listener', namespace=namespace)

    received_messages = []
    expected_msgs = []
    try:
        expected_msgs = [(i, repr(msg)) for i, msg in enumerate(
                    [msg_mod(header=Header(stamp=Time(sec=1, nanosec=0)))])]
    except Exception as e:
        expected_msgs = [(i, repr(msg)) for i, msg in enumerate([msg_mod()])]

    chatter_callback = functools.partial(
        listener_cb, received_messages=received_messages, expected_msgs=expected_msgs)

    lifespan = Duration(seconds=0.5)

    qos_profile = QoSProfile(
        depth=10,
        # Guaranteed delivery is needed to send messages to late-joining subscription.
        reliability=QoSReliabilityPolicy.RELIABLE,
        # Store messages on the publisher so that they can be affected by Lifespan.
        durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
        lifespan=lifespan)

    node.create_subscription(
        msg_mod, 'test/message/' + message_name, chatter_callback, qos_profile)

    spin_count = 1
    print('subscriber: beginning loop')
    while (rclpy.ok() and len(received_messages) != len(expected_msgs)):
        rclpy.spin_once(node)
        spin_count += 1
        print('spin_count: ' + str(spin_count))
    node.destroy_node()
    rclpy.shutdown()

    assert len(received_messages) == len(expected_msgs),\
        'Should have received {} {} messages from talker'.format(len(expected_msgs), message_name) 

def publisher(
    node: Node,
    message_type: MsgType,
    topic_name: str,
    values: dict,
    period: float,
    print_nth: int,
    times: int,
    qos_profile: QoSProfile,
    keep_alive: float,
) -> Optional[str]:
    """Initialize a node with a single publisher and run its publish loop (maybe only once)."""
    msg_module = get_message(message_type)
    values_dictionary = yaml.safe_load(values)
    if not isinstance(values_dictionary, dict):
        return 'The passed value needs to be a dictionary in YAML format'

    pub = node.create_publisher(msg_module, topic_name, qos_profile)

    msg = msg_module()
    try:
        set_message_fields(msg, values_dictionary)
    except Exception as e:
        return 'Failed to populate field: {0}'.format(e)

    print('publisher: beginning loop')
    count = 0

    def timer_callback():
        nonlocal count
        count += 1
        if print_nth and count % print_nth == 0:
            print('publishing #%d: %r\n' % (count, msg))
        pub.publish(msg)

    timer = node.create_timer(period, timer_callback)
    while times == 0 or count < times:
        rclpy.spin_once(node)

    # give some time for the messages to reach the wire before exiting
    time.sleep(keep_alive)

    node.destroy_timer(timer) 

def test_execute_coroutine_guard_condition(self):
        self.assertIsNotNone(self.node.handle)
        executor = SingleThreadedExecutor(context=self.context)
        executor.add_node(self.node)

        called1 = False
        called2 = False

        async def coroutine():
            nonlocal called1
            nonlocal called2
            called1 = True
            await asyncio.sleep(0)
            called2 = True

        gc = self.node.create_guard_condition(coroutine)
        try:
            gc.trigger()
            executor.spin_once(timeout_sec=0)
            self.assertTrue(called1)
            self.assertFalse(called2)

            called1 = False
            executor.spin_once(timeout_sec=1)
            self.assertFalse(called1)
            self.assertTrue(called2)
        finally:
            self.node.destroy_guard_condition(gc) 

def main(self, *, args):  # noqa: D102
        with NodeStrategy(args) as node:
            node_names = get_node_names(
                node=node, include_hidden_nodes=args.include_hidden_nodes)

        node_name = get_absolute_node_name(args.node_name)
        if node_name:
            if node_name not in [n.full_name for n in node_names]:
                return 'Node not found'
            node_names = [
                n for n in node_names if node_name == n.full_name]

        with DirectNode(args) as node:
            service_names = get_service_names(
                node=node, include_hidden_services=args.include_hidden_nodes)

            clients = {}
            futures = {}
            # create clients for nodes which have the service
            for node_name in node_names:
                service_name = f'{node_name.full_name}/list_parameters'
                if service_name in service_names:
                    client = node.create_client(ListParameters, service_name)
                    clients[node_name] = client

            # wait until all clients have been called
            while True:
                for node_name in [
                    n for n in clients.keys() if n not in futures
                ]:
                    # call as soon as ready
                    client = clients[node_name]
                    if client.service_is_ready():
                        request = ListParameters.Request()
                        for prefix in args.param_prefixes:
                            request.prefixes.append(prefix)
                        future = client.call_async(request)
                        futures[node_name] = future

                if len(futures) == len(clients):
                    break
                rclpy.spin_once(node, timeout_sec=1.0)

            # wait for all responses
            for future in futures.values():
                rclpy.spin_until_future_complete(node, future, timeout_sec=1.0)

            # print responses
            for node_name in sorted(futures.keys()):
                future = futures[node_name]
                if future.result() is not None:
                    if not args.node_name:
                        print(f'{node_name.full_name}:')
                    response = future.result()
                    for name in sorted(response.result.names):
                        print(f'  {name}')
                else:
                    e = future.exception()
                    print(
                        'Exception while calling service of node '
                        f"'{node_name.full_name}': {e}", file=sys.stderr) 

def _call_get_transitions(node, states, service_name):
    clients = {}
    futures = {}
    # create clients
    for node_name in states.keys():
        clients[node_name] = node.create_client(
            GetAvailableTransitions, f'{node_name}/{service_name}')

    # wait until all clients have been called
    while True:
        for node_name in [n for n in states.keys() if n not in futures]:
            # call as soon as ready
            client = clients[node_name]
            if client.service_is_ready():
                request = GetAvailableTransitions.Request()
                future = client.call_async(request)
                futures[node_name] = future

        if len(futures) == len(clients):
            break
        rclpy.spin_once(node, timeout_sec=1.0)

    # wait for all responses
    for future in futures.values():
        rclpy.spin_until_future_complete(node, future)

    # return transitions from current state or exception for each node
    transitions = {}
    for node_name, future in futures.items():
        if future.result() is not None:
            response = future.result()
            transitions[node_name] = []
            for transition_description in response.available_transitions:
                if (
                    states[node_name] is None or
                    transition_description.start_state == states[node_name]
                ):
                    transitions[node_name].append(
                        transition_description)
        else:
            transitions[node_name] = future.exception()
    return transitions 

def call_get_states(*, node, node_names):
    clients = {}
    futures = {}
    # create clients
    for node_name in node_names:
        clients[node_name] = \
            node.create_client(GetState, f'{node_name}/get_state')

    # wait until all clients have been called
    while True:
        for node_name in [n for n in node_names if n not in futures]:
            # call as soon as ready
            client = clients[node_name]
            if client.service_is_ready():
                request = GetState.Request()
                future = client.call_async(request)
                futures[node_name] = future

        if len(futures) == len(clients):
            break
        rclpy.spin_once(node, timeout_sec=1.0)

    # wait for all responses
    for future in futures.values():
        rclpy.spin_until_future_complete(node, future)

    # return current state or exception for each node
    states = {}
    for node_name, future in futures.items():
        if future.result() is not None:
            response = future.result()
            states[node_name] = response.current_state
        else:
            states[node_name] = future.exception()
    return states