Rclpy



Resources

Concepts


 Callback Groups

Currently exist two groups, MutuallyExclusive and Reentrant. The former is for making sure that members in the group never run at the same time as each other (even themselves) and the latter for permitting freedom to run concurrently (so long as you have a MultiThreadedExecutor). William has a great table for describing the permutations here.

 Contexts

Amongst other things, contexts hold resources for intra-process communications so nodes in the same process AND context do not need to share resources. This implies that they will not serialise/deserialise nor use the middleware. If in the same process and in different contexts, they will serialise/deserialse, but still not use the middleware. See here for more details.

Snippets

Command Line Args

# Strip ROS args and feed argparse
    command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
    parser.parse_args(command_line_args)

Graph

# Topics and Services - ugh, needs a magic sleep still (otherwise empty)
    rclpy.init(args=None)
    time.sleep(0.1)
    print("Topic Names and Types:\n {}".format(node.get_topic_names_and_types()))
    print("Services Names and Types:\n {}".format(node.get_service_names_and_types()))
    print("Node Names: {}".format(node.get_node_names()))

# Number of subscribers/publishers
    if node.count_publishers(topic_name) > 0:

Names

# Set the node name
    node = rclpy.create_node(node_name="Exchange")
# Publisher with a private name (i.e. appended to the node name)
# IMPORTANT: needs the '/'! See http://design.ros2.org/articles/topic_and_service_names.html
    publisher = node.create_publisher(std_msgs.String, '~/blackboard')
# Anonymous name
    node = rclpy.node.Node('watcher' + "_" + str(os.getpid()))
# Resolved name
    count = 1
    node = rclpy.node.Node('exchange')
    topic = rclpy.expand_topic_name.expand_topic_name(
        topic_name="~/blackboard/watcher_" + str(counter),
        node_name=node.get_name(),
        node_namespace=node.get_namespace()
    )
# Get all the nodes and namespaces a node can see
    print("Node Names & Namespaces\n  {}".format(my_node.get_node_names_and_namespaces()))

Qt

import PyQt5.QtWidgets as qt_widgets
import PyQt5.QtCore as qt_core

# locate generated files
from . import gui

class MainWindow(qt_widgets.QMainWindow):

    gui_close_event = qt_core.pyqtSignal(name="guiCloseEvent")

    def __init__(self):
        super().__init__()

        # Use generated files - bugfree when using promotions & resources
        self.ui = gui.main_window.Ui_MainWindow()
        self.ui.setupUi(self)

    def closeEvent(self, unused_event):
        self.gui_close_event.emit()

class Backend(qt_core.QObject):
    def __init__(self):
        super().__init__()
        self.node = rclpy.create_node("dashboard")
        self.shutdown_requested = False

    def terminate_ros_spinner(self):
        self.shutdown_requested = True

    def spin(self):
        while rclpy.ok() and not self.shutdown_requested:
            rclpy.spin_once(self.node, timeout_sec=0.1)
        self.node.destroy_node()

def main():

    # picks up sys.argv automagically internally
    rclpy.init()
    # enable handling of ctrl-c (from roslaunch as well)
    signal.signal(signal.SIGINT, signal.SIG_DFL)

    # the players
    app = qt_widgets.QApplication(sys.argv)
    main_window = MainWindow()
    backend = Backend()

    # sigslots (specifically to catch 'x' in gui shutdown)
    main_window.gui_close_event.connect(
        backend.terminate_ros_spinner
    )

    # sig interrupt handling
    signal.signal(
        signal.SIGINT,
        lambda unused_signal, unused_frame: main_window.close()
    )

    # ros spinner
    ros_thread = threading.Thread(target=backend.spin)
    ros_thread.start()

    # qt...up
    main_window.show()
    result = app.exec_()

    # shutdown
    ros_thread.join()
    rclpy.shutdown()
    sys.exit(result)

Setup.py


# Install package.xml
    data_files=[
        ('share/' + package_name, ['package.xml']),
    ],
# Install global scripts
    data_files=[
        ('bin', ['scripts/py-trees-blackboard-watcher',
                 'scripts/py-trees-tree-watcher']),
    ],
# Install package specific scripts: entry_points
    entry_points={
         'console_scripts': [
             # These are redirected to lib/<package_name> by setup.cfg
             'py-trees-demo-exchange = py_trees_ros.demos.exchange:main',
             'py-trees-ros-tutorial-tree-one = py_trees_ros.tutorials.one:main',
         ],
     },
# Install package specific scripts: setup.cfg
    [develop]
    script-dir=$base/lib/py_trees_ros
    [install]
    install-scripts=$base/lib/py_trees_ros

Time & Timers

# Get the current time as an rclpy.time.Time object
    rclpy.clock.Clock().now()

# Convert rclpy.time.Time objects to a msg
    rclpy.clock.Clock().now().to_msg()

# Convert a msg to an rclpy.time.Time object
    rclpy.time.Time.from_msg(msg)

# Periodically execute a method
    print_timer = node.create_timer(
        timer_period_sec=1.0,
        callback=periodically_print_something
    )

# Periodically execute a method with arguments
    publisher_timer = node.create_timer(
        timer_period_sec=1.0,
        callback=functools.partial(periodically_publish, exchange=exchange)
    )

# Periodically execute a class's member method
    <TODO>

Guidelines

  • Executors belong in the applications, Nodes in the classes and methods.
  • Avoid blocking spins in classes and methods - a multi-node application won't like you
    • Pass in an executor pre-loaded with other nodes if you have to

Questions

In order of criticality...

  • Action Server Exceptions: hang and only report somewhat unusefully on CTRL-C (rclpy#296)
  • Contexts: is there a situtation when you would not want to use the default global context?
  • Latching: See also https://answers.ros.org/question/305795/ros2-latching/, also https://github.com/ros2/ros2/wiki/About-Quality-of-Service-Settings and https://github.com/ros2/ros2/issues/464 (open: latching needs decision)
    • ros2 topic echo has no way of setting it's own profile to be latched (TRANSIENT_LOCAL)
  • Unicode: strings are intended to support UTF-8 (design/WideStrings.md), but String.msg is only ASCII right now (refer to discussion in ros2cli#176)
  • Oneshot Timers: no implementation (rclpy #186), should be easy to implement, but how does anything know about the callback??? (rclpy/node.py#L302, rclpy/timer.py#l23, rclcpp/timer.hpp)
  • Non-Wall Timers: no implementation (rclpy/timer.py#L21)
  • Non-Wall Sleep: no implementation
  • Rate: no rclpy implementation (rclpy #186)
  • Node Shutdown Hooks: Seems to be missing, no workaround for it right now (rclpy #244)
  • Ros2Topic String Formatting: Ros1 formatted strings to stdout complete with newlines, shell color control sequences, etc.
    • Used this mechanism for writing descriptions/information on latched one-shot publishers (makes it very easy for someone to inspect and understand the runtime).
  • Ros2Service Not Formatting at all: doesn't even format line by line like ros2topic does
  • Coloured Log Levels: on stdout for warning/error using, e.g. node.get_logger().error("...")
  • Home: how to discover where 'home' is? e.g. ROS_HOME || rospkg.get_ros_home()
  • Resolved Names: convenience ROS1 style publisher.resolved_name instead of having to discover via 
rclpy.expand_topic_name.expand_topic_name(
    topic_name,
    node.get_name(),
    node.get_namespace()))


Other (Non-Rclpy)

  • Overlays (sourcing install/setup.bash) no longer remember their dependent underlays? Have to source each and every one manually.....
    • This is the difference between install/setup.bash and install/local_setup.basht (there is a comment in the top of each file)