Expose pause/resume related services on the Player

[emersonknapp]

Depends on #728
Depends on #704

Exposes "Pause", "Resume", "TogglePaused", and "IsPaused" services for the Player.

[emersonknapp]

I'm opening this up for review because this is the implementation - but we will definitely not merge without tests.

I am manually testing with this simple Python app for keyboard control and it's working great! (need to pip3 install getkey).
I'm wondering, Should we maybe make a rosbag2_examples package for example usage, like this key controller? We could also put usage examples of the rosbag2_py and rosbag2_cpp APIs there, to make it easier to understand by example how to use rosbag2 programmatically. This would probably be better than putting code examples in tutorial docs, because at least we can ensure these compile with updates.

#!/usr/bin/env python3
import threading

from getkey import getkey, keys

import rclpy
from rclpy.node import Node

from rosbag2_interfaces.srv import (
    Pause,
    Resume,
    TogglePaused,
    IsPaused,
)

class PlayController(Node):
    def __init__(self):
        super().__init__('playcontrol')
        self.pause_client = self.create_client(Pause, '/rosbag2_player/pause')
        self.resume_client = self.create_client(Resume, '/rosbag2_player/resume')
        self.toggle_client = self.create_client(TogglePaused, '/rosbag2_player/toggle_paused')
        self.is_paused_client = self.create_client(IsPaused, '/rosbag2_player/is_paused')
        self.timer = self.create_timer(1, self.state_timer)

    def print_state(self, future):
        res = future.result()
        if res is not None:
            self.get_logger().info(f'IsPaused: {res.paused}')
        else:
            self.get_logger().error(f'Exception while calling service: {future.exception()}')

    def trigger_result_cb(self, name):
        def impl(future):
            res = future.result()
            if res is not None:
                self.get_logger().info(f'Response received for: {name}')
            else:
                self.get_logger().error(f'Exception for service: {future.exception()}')

        return impl

    def state_timer(self):
        req = IsPaused.Request()
        future = self.is_paused_client.call_async(req)
        future.add_done_callback(self.print_state)

    def pause(self):
        req = Pause.Request()
        future = self.pause_client.call_async(req)
        future.add_done_callback(self.trigger_result_cb('Pause'))

    def resume(self):
        req = Resume.Request()
        future = self.resume_client.call_async(req)
        future.add_done_callback(self.trigger_result_cb('Resume'))

    def toggle_paused(self):
        req = TogglePaused.Request()
        future = self.toggle_client.call_async(req)
        future.add_done_callback(self.trigger_result_cb('Toggle'))


def spin_fn(node):
    node.get_logger().info('Starting spin thread')
    rclpy.spin(node)
    node.get_logger().info('Spin thread done')


def main():
    rclpy.init()
    node = PlayController()
    spin_thread = threading.Thread(target=spin_fn, args=[node])
    spin_thread.start()

    try:
        while True:
            c = getkey()
            if c == keys.SPACE:
                node.toggle_paused()
            elif c == keys.P:
                node.pause()
            elif c == keys.R:
                node.resume()
            else:
                print(f'Ignored: {c}')
    except KeyboardInterrupt:
        pass

    node.destroy_node()
    rclpy.shutdown()
    spin_thread.join()


if __name__ == "__main__":
    main()

[emersonknapp]

@Karsten1987 this is ready to go, could you give it a look when you have a chance?

[Karsten1987]

lgtm with green CI.

[Karsten1987]

How can clock_ ever be nullptr? On that note, where is clock_ currently initialized?

same below.

[emersonknapp]

Clock is currently intitialized in prepare_clock, which call from play. I would like to initialize it in the constructor (and therefore avoid having it be on the heap) - but we don't have access to the "initial time" until the reader is opened, which only happens when we call play.

It may make sense, to me, to open the reader in the constructor, now that we have all arguments present at that point. What do you think about that approach? We could start the clock paused, then play could be the same function as resume, rather than doing the other setup.

I wouldn't want to make that change here, though.

[Karsten1987]

I am okay if you don't address it here, but I'd like to have it noted in an GH issue or an TODO to keep track of it. I think we should - now with its public header - review the overall API and offload some of the initialization work from play to the constructor.

[emersonknapp]

I agree with all of that

[emersonknapp]

Gist: https://gist.githubusercontent.com/emersonknapp/bd7f6cd9db15a825e6522b7e5f9efad4/raw/2cdbc90e9cf69cd9c850cfca808846fce74cc5bf/ros2.repos
BUILD args: --packages-up-to rosbag2_transport rosbag2 rosbag2_py rosbag2_tests
TEST args: --packages-select rosbag2_transport rosbag2 rosbag2_py rosbag2_tests
Job: ci_launcher
ci_launcher ran: https://ci.ros2.org/job/ci_launcher/8289

• Linux
• Linux-aarch64
• macOS
• Windows

[emersonknapp]

• Linux
• Linux-aarch64
• macOS
• Windows

[emersonknapp]

• Linux
• Linux-aarch64
• macOS
• Windows

[emersonknapp]

Just trying OSX for the mutex concern

[emersonknapp]

Happy OSX, logic fixed up. last (hopefully) run

• Linux
• Linux-aarch64
• macOS
• Windows