Python rclpy.ok() Examples

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

    node = rclpy.create_node('minimal_publisher')

    publisher = node.create_publisher(String, 'topic', 10)

    msg = String()

    i = 0
    while rclpy.ok():
        msg.data = 'Hello World: %d' % i
        i += 1
        node.get_logger().info('Publishing: "%s"' % msg.data)
        publisher.publish(msg)
        sleep(0.5)  # seconds

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_node()
    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 talker_main():
	rclpy.init(args=None)
	node = rclpy.create_node('ros2_talker_node')
	pub = node.create_publisher(String, '/chatter')
	msg = String()
	i = 0
	while rclpy.ok():
		msg.data = 'Hello World: %d' % i
		i += 1
		node.get_logger().info('Publishing: "%s"' % msg.data)
		pub.publish(msg)
		sleep(0.5) 

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 test_number_callbacks(period):
    context = rclpy.context.Context()
    rclpy.init(context=context)
    try:
        node = rclpy.create_node('test_timer_number_callbacks', context=context)
        try:
            executor = SingleThreadedExecutor(context=context)
            try:
                executor.add_node(node)
                # The first spin_once() takes  long enough for 1ms timer tests to fail
                executor.spin_once(timeout_sec=0)

                callbacks = []
                timer = node.create_timer(period, lambda: callbacks.append(len(callbacks)))
                try:
                    begin_time = time.time()

                    while rclpy.ok(context=context) and time.time() - begin_time < 4.5 * period:
                        executor.spin_once(timeout_sec=period / 10)

                    assert len(callbacks) == 4
                finally:
                    node.destroy_timer(timer)
            finally:
                executor.shutdown()
        finally:
            node.destroy_node()
    finally:
        rclpy.shutdown(context=context) 

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 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 requester(service_pkg, service_name, namespace):
    import rclpy

    # Import the service
    module = importlib.import_module(service_pkg + '.srv')
    srv_mod = getattr(module, service_name)

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

    srv_fixtures = [[req, resp]]
    service_name = 'test/service/' + service_name

    rclpy.init(args=[])
    try:
        node = rclpy.create_node('requester', namespace=namespace)
        try:
            # wait for the service to be available
            client = node.create_client(srv_mod, service_name)
            tries = 15
            while rclpy.ok() and not client.wait_for_service(timeout_sec=1.0) and tries > 0:
                print('service not available, waiting again...')
                tries -= 1
            assert tries > 0, 'service still not available, aborting test'

            print('requester: beginning request')
            # Make one call to that service
            for req, resp in srv_fixtures:
                future = client.call_async(req)
                rclpy.spin_until_future_complete(node, future)
                assert repr(future.result()) == repr(resp), \
                    'unexpected response %r\n\nwas expecting %r' % (future.result(), resp)
                print('received reply #%d of %d' % (
                    srv_fixtures.index([req, resp]) + 1, len(srv_fixtures)))
        finally:
            node.destroy_node()
    finally:
        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 _rostopic_hz(node, topic, window_size=DEFAULT_WINDOW_SIZE, filter_expr=None, use_wtime=False):
    """
    Periodically print the publishing rate of a topic to console until shutdown.

    :param topic: topic name, ``list`` of ``str``
    :param window_size: number of messages to average over, -1 for infinite, ``int``
    :param filter_expr: Python filter expression that is called with m, the message instance
    """
    # 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 = ROSTopicHz(node, window_size, filter_expr=filter_expr, use_wtime=use_wtime)
    node.create_subscription(
        msg_class,
        topic,
        functools.partial(rt.callback_hz, topic=topic),
        qos_profile_sensor_data)

    while rclpy.ok():
        rclpy.spin_once(node)
        rt.print_hz(topic)

    node.destroy_node()
    rclpy.shutdown() 

def get_msg_class(node, topic, blocking=False, include_hidden_topics=False):
    msg_class = _get_msg_class(node, topic, include_hidden_topics)
    if msg_class:
        return msg_class
    elif blocking:
        print('WARNING: topic [%s] does not appear to be published yet' % topic)
        while rclpy.ok():
            msg_class = _get_msg_class(node, topic, include_hidden_topics)
            if msg_class:
                return msg_class
            else:
                sleep(0.1)
    else:
        print('WARNING: topic [%s] does not appear to be published yet' % topic)
    return None 

def __getattr__(self, name):
        if not rclpy.ok():
            raise RuntimeError('!rclpy.ok()')

        return getattr(self.node, name) 

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 requester(service_type, service_name, values, period):
    # TODO(wjwwood) this logic should come from a rosidl related package
    try:
        parts = service_type.split('/')
        if len(parts) == 2:
            parts = [parts[0], 'srv', parts[1]]
        package_name = parts[0]
        module = importlib.import_module('.'.join(parts[:-1]))
        srv_name = parts[-1]
        srv_module = getattr(module, srv_name)
        if not package_name or not srv_module:
            raise ValueError()
    except ValueError:
        raise RuntimeError('The passed service type is invalid')

    values_dictionary = yaml.safe_load(values)

    rclpy.init()

    node = rclpy.create_node(NODE_NAME_PREFIX + '_requester_%s_%s' % (package_name, srv_name))

    cli = node.create_client(srv_module, service_name)

    request = srv_module.Request()

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

    if not cli.service_is_ready():
        print('waiting for service to become available...')
        cli.wait_for_service()

    while True:
        print('requester: making request: %r\n' % request)
        last_call = time.time()
        future = cli.call_async(request)
        rclpy.spin_until_future_complete(node, future)
        if future.result() is not None:
            print('response:\n%r\n' % future.result())
        else:
            raise RuntimeError('Exception while calling service: %r' % future.exception())
        if period is None or not rclpy.ok():
            break
        time_until_next_period = (last_call + period) - time.time()
        if time_until_next_period > 0:
            time.sleep(time_until_next_period)

    node.destroy_node()
    rclpy.shutdown() 

def talker(message_pkg, message_name, number_of_cycles, namespace):
    import rclpy

    time.sleep(0.1)

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

    rclpy.init(args=[])

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

    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)

    chatter_pub = node.create_publisher(
        msg_mod, 'test/message/' + message_name, qos_profile)

    cycle_count = 0
    print('talker: beginning loop')
    msgs = []
    try:
        msgs = [msg_mod(header=Header(stamp=Time(sec=1, nanosec=0)))]
    except Exception as e:
        msgs = [msg_mod()]

    while rclpy.ok() and cycle_count < number_of_cycles:
        msg_count = 0
        for msg in msgs:
            chatter_pub.publish(msg)
            msg_count += 1
            print('publishing message #%d' % msg_count)
            time.sleep(0.1)
        cycle_count += 1
        time.sleep(0.1)
    node.destroy_node()
    rclpy.shutdown() 

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 test_cancel_reset(period):
    context = rclpy.context.Context()
    rclpy.init(context=context)
    try:
        node = rclpy.create_node('test_timer_cancel_reset', context=context)
        try:
            executor = SingleThreadedExecutor(context=context)
            try:
                executor.add_node(node)
                # The first spin_once() takes  long enough for 1ms timer tests to fail
                executor.spin_once(timeout_sec=0)

                callbacks = []
                timer = node.create_timer(period, lambda: callbacks.append(len(callbacks)))
                try:
                    # Make sure callbacks can be received
                    assert not timer.is_canceled()
                    begin_time = time.time()
                    while rclpy.ok(context=context) and time.time() - begin_time < 2.5 * period:
                        executor.spin_once(timeout_sec=period / 10)
                    assert len(callbacks) == 2

                    # Cancel timer and make sure no callbacks are received
                    assert not timer.is_canceled()
                    timer.cancel()
                    assert timer.is_canceled()
                    callbacks = []
                    begin_time = time.time()
                    while rclpy.ok(context=context) and time.time() - begin_time < 2.5 * period:
                        executor.spin_once(timeout_sec=period / 10)
                    assert [] == callbacks

                    # Reenable timer and make sure callbacks are received again
                    timer.reset()
                    assert not timer.is_canceled()
                    begin_time = time.time()
                    while rclpy.ok(context=context) and time.time() - begin_time < 2.5 * period:
                        executor.spin_once(timeout_sec=period / 10)
                    assert len(callbacks) == 2
                finally:
                    node.destroy_timer(timer)
            finally:
                executor.shutdown()
        finally:
            node.destroy_node()
    finally:
        rclpy.shutdown(context=context) 

def main(self, *, args, summary_table=None):
        if summary_table is None:
            summary_table = SummaryTable()
        with DirectNode(args, node_name=NODE_NAME_PREFIX + '_node') as node:
            publisher = HelloPublisher(node, args.topic, summary_table)
            subscriber = HelloSubscriber(node, args.topic, summary_table)
            sender = HelloMulticastUDPSender(summary_table, ttl=args.ttl)
            receiver = HelloMulticastUDPReceiver(summary_table)
            receiver_thread = threading.Thread(target=receiver.recv)
            receiver_thread.start()

            executor = SingleThreadedExecutor()
            executor.add_node(node.node)

            executor_thread = threading.Thread(target=executor.spin)
            executor_thread.start()

            try:
                clock = node.get_clock()
                prev_time = clock.now()
                print_period = Duration(seconds=args.print_period)
                emit_rate = node.create_rate(frequency=1.0 / args.emit_period, clock=clock)
                while rclpy.ok():
                    current_time = clock.now()
                    if (current_time - prev_time) > print_period:
                        summary_table.format_print_summary(args.topic, args.print_period)
                        summary_table.reset()
                        prev_time = current_time
                    publisher.publish()
                    sender.send()
                    emit_rate.sleep()
                    if args.once:
                        summary_table.format_print_summary(args.topic, args.print_period)
                        break
            except KeyboardInterrupt:
                pass
            finally:
                executor.shutdown()
                executor_thread.join()
                receiver.shutdown()
                receiver_thread.join()
                sender.shutdown()
                subscriber.destroy()
                publisher.destroy()