ZED Lifecycle

The ZED node was designed using the concepts of managed life cycle nodes introduced with ROS2.

A managed life cycle node executes according to a known life cycle state machine, so it can be controlled by any tool created for managing this kind of node.

A robot using the ZED life cycle node can know when the ZED is ready to publish to data topics, and it can start to act only when the ZED node has effectively started to work and can provide reliable information.

This tutorial will introduce you to the basic concepts of the life cycle state machine, then demonstrate how to apply these concepts to control the ZED node.

The life cycle state machine

The node can be in one of the four primary states:

  • UNCONFIGURED: The ZED is ready to be configured
  • INACTIVE: The ZED is configured, but not active
  • ACTIVE: The ZED node is active and the data streams are published
  • FINALIZED: The ZED is closed and the data structured cleaned up

While switching from one state to another, the node transitions through one of the five intermediate states:

  • configuring: Unconfigured -> Inactive
  • activating: Inactive -> Active
  • deactivating: Active -> Inactive
  • cleaningup: Inactive -> Unconfigured
  • errorprocessing
  • shuttingdown

To switch the state, seven transitions are available. Five of them can be called directly by the user:

  • create: Automatically called at the beginning of the node life cycle
  • configure: Called to configure the ZED - [ID 1]
  • cleanup: Called to clean all the data structures - [ID 2]
  • activate: Called to start the data streams publishing after the camera has been configured - [ID 3]
  • deactivate: Called to stop the data streams publishing - [ID 4]
  • shutdown: Called to close and clean up the node - [ID 5]
  • destroy: Automatically called to exit the node

Unconfigured state

This is the life cycle state of the ZED node as soon as it is started. The node returns to this state after an error is handled correctly or after a cleanup transition.

Valid out transitions:

  • shutdown: Sends the node to the Finalized state
  • configure: Sets ZED configuring to the Inactive state

Inactive state

When the ZED node is in this state, the configuration process has been correctly completed, the camera is connected, and the elaboration is ready to be started.

If using an SVO file instead of a physical device, the node has verified that the SVO path is correct and it is ready to process it.

The topics are not yet published and the incoming requests are not processed, unless they are topics of type lifecycle_msgs/TransitionEvent.

Valid out transitions:

  • shutdown: Sends the node to the Finalized state
  • cleanup: The data structures are cleaned and the node is sent back to Unconfigured
  • activate: ZED activation to Active

Active state

The ZED node is in its main state. The data are acquired using the ZED SDK and are published to the subscribers.

Valid out transitions:

  • shutdown: the node enters in Finalized
  • deactivate: the node is stopped to go back to Inactive

Finalized state

The ZED node enters this state after a shutdown or after a fatal error. Only the destroy transition can exit this state, and the node must be re-spawned to get it back in action.

Manual control

Now you will learn how to write a Python launch that starts a ZED node and a Robot State Publisher node. Then you will learn how to manually control the ZED node swithing states via command line.

The launch script generates a LaunchDescription object that is used by the ROS2 launch system to instantiate the ZED node and the Robot State Publisher node.

The ZED node is configured according to the “YAML” files available in the config folder (see the ZED node guide for more details). The Robot State Publisher node publishes the static frame of the ZED cameras described in the zed.urdf or in the zedm.urdf, both placed in the urdf folder.

The Python launch script is available in the launch folder: zed_unmanaged.launch.py.

The Python launch script

Let’s start analyzing the Python launch script in details:

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.actions import LifecycleNode

def generate_launch_description():

    # use: 'zed' for "ZED" camera - 'zedm' for "ZED-M" camera
    camera_model = 'zedm' 

    # URDF file to be loaded by Robot State Publisher
    urdf = os.path.join(
        get_package_share_directory('stereolabs_zed'), 
            'urdf', camera_model + '.urdf'
    )
    
    # ZED Configurations to be loaded by ZED Node
    config_common = os.path.join(
        get_package_share_directory( 'stereolabs_zed' ), 
        'config', 'common.yaml'
    )

    config_camera = os.path.join(
        get_package_share_directory('stereolabs_zed'), 
        'config', camera_model + '.yaml'
    )

    # Set LOG format
    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}] - {message}'

    return LaunchDescription( [
        # Robot State Publisher
        Node(
            node_namespace='zed',
            node_name='zed_state_publisher',
            package='robot_state_publisher',
            node_executable='robot_state_publisher',
            output='screen',
            arguments=[urdf],
        ),

        # ZED
        LifecycleNode(
            node_namespace='zed',        # must match the namespace in config -> YAML
            node_name='zed_node',        # must match the node name in config -> YAML
            package='stereolabs_zed',
            node_executable='zed_wrapper_node',
            output='screen',
            parameters=[
                config_common,  # Common parameters
                config_camera,  # Camera related parameters
            ],
        ),
    ])

First, import the necessary libraries:

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.actions import LifecycleNode

Next, start generating the Launch Description object in the generate_launch_description function:

def generate_launch_description():

Note: you must manually set the camera model of your ZED camera. This information will be used to load the correct URDF and Parameter files:

    # use: 'zed' for "ZED" camera - 'zedm' for "ZED-M" camera
    camera_model = 'zedm'

Now you can correctly generate the path to the URDF file using the camera_model variable:

    # URDF file to be loaded by Robot State Publisher
    urdf = os.path.join(
        get_package_share_directory('stereolabs_zed'), 
            'urdf', camera_model + '.urdf'
    )

The function get_package_share_directory returns the full path to the share directory, where the stereolabs_zed package has been installed during the compilation process. The share folder contains an urdf subfolder with the files zed.urdf and zedm.urdf.

In the same way we generate the correct path for configuration files:

    # ZED Configurations to be loaded by ZED Node
    config_common = os.path.join(
        get_package_share_directory( 'stereolabs_zed' ), 
        'config', 'common.yaml'
    )

    config_camera = os.path.join(
        get_package_share_directory('stereolabs_zed'), 
        'config', camera_model + '.yaml'
    )

For the URDF file, the share folders contain a config subfolder with the files common.yaml, zed.yaml, and zedm.yaml. The content of the configuration files is fully described in the ZED node guide.

Set an environment variable to change the format of the log information displayed on the console:

    # Set LOG format
    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}] - {message}'

With this simple command, the log will contain information about the timestamp, the name of the node that is logging, and the severity of the message and its content.

Finally, instantiate the LaunchDescription object that will provide information about the two nodes that we want to start to the launch system:

return LaunchDescription( [
        # Robot State Publisher
        Node(
            node_namespace='zed',
            node_name='zed_state_publisher',
            package='robot_state_publisher',
            node_executable='robot_state_publisher',
            output='screen',
            arguments=[urdf],
        ),

        # ZED
        LifecycleNode(
            node_namespace='zed',        # must match the namespace in config -> YAML
            node_name='zed_node',        # must match the node name in config -> YAML
            package='stereolabs_zed',
            node_executable='zed_wrapper_node',
            output='screen',
            parameters=[
                config_common,  # Common parameters
                config_camera,  # Camera related parameters
            ],
        ),
    ])

In the zed namespace, you will execute two nodes named zed_state_publisher and zed_node.

The zed_state_publisher is inizialized using the urdf argument.

The zed_node is initialized using the config_common and the config_camera parameter files.

Manual node management

Now that you have the zed_unmanaged.launch.py Python launch script, you can start the ZED node.

Open a text console and enter

$ ros2 launch stereolabs_zed zed_unmanaged.launch.py

The zed_state_publisher SUDDENLY starts and publishes the static TF information found in the URDF file.

The zed_node instead enters the UNCONFIGURED state waiting for a configure transition request.

1538576510.206046443: [zed.zed_node] [INFO] - ZED Camera Component created
1538576510.206062324: [zed.zed_node] [INFO] - ZED namespace: '/zed'
1538576510.206067046: [zed.zed_node] [INFO] - ZED node: 'zed_node'
1538576510.206069638: [zed.zed_node] [INFO] - Waiting for `CONFIGURE` request...

We can send the CONFIGURE request in two ways:

  1. Using a ROS2 service to call the change_state provided by every lifecycle nodes
  2. Using the CLI command ros2 lifecycle set

The first method was the only method available before ROS2 Bouncy and it is quite complicated:

$ ros2 service call /zed/zed_node/change_state lifecycle_msgs/ChangeState "{node_name: zed.zed_node, transition: {id: X}}"

This “structured” command line calls the /zed/zed_node/change_state service of type lifecycle_msgs/ChangeState with the params node_name and transition. For it to work, you must replace X with the ID number of the five callable transitions listed above.

The second method, available from ROS2 Bouncy, is more intuitive and straightforward:

$ ros2 lifecycle set /zed/zed_node X

Where X can be the ID number of one of the five callable transition listed above, or more simply the full name of the transition itself.

Now you know how to call the configure transition to let the node switch to the `INACTIVE’ state.

Open a new text console and issue the service mode with transiction ID 1 to the transition process:

$ ros2 service call /zed/zed_node/change_state lifecycle_msgs/ChangeState "{node_name: zed.zed_node, transition: {id: 1}}"

You will get the following reply:

requester: making request: lifecycle_msgs.srv.ChangeState_Request(node_name='zed.zed_node', transition=lifecycle_msgs.msg.Transition(id=1, label=''))

response:
lifecycle_msgs.srv.ChangeState_Response(success=True)

This is a standard reply to a ROS2 service call, and it confirms that the request was valid. The ZED node starts to configure emitting all the configuration messages on the console where it has been started:

1538636563.597361783: [zed.zed_node] [INFO] - *** State transition: configuring ***
[...]
1538636564.673601227: [zed.zed_node] [INFO] - ZED configured
1538636564.673604685: [zed.zed_node] [INFO] - Waiting for `ACTIVATE` request...

Barring errors, the ZED node is now configured, and you can get a list of all the topics that can be subscribed to:

$ ros2 topic list
/joint_states
/parameter_events
/tf
/tf_static
/zed/parameter_events
/zed/zed_node/confidence/confidence_image
/zed/zed_node/confidence/confidence_image/camera_info
/zed/zed_node/confidence/confidence_map
/zed/zed_node/depth/depth_registered
/zed/zed_node/depth/depth_registered/camera_info
/zed/zed_node/disparity/disparity_image
/zed/zed_node/imu/data
/zed/zed_node/imu/data_raw
/zed/zed_node/left/image_raw_color
/zed/zed_node/left/image_raw_color/camera_info
/zed/zed_node/left/image_rect_color
/zed/zed_node/left/image_rect_color/camera_info
/zed/zed_node/point_cloud/cloud_registered
/zed/zed_node/rgb/image_raw_color
/zed/zed_node/rgb/image_raw_color/camera_info
/zed/zed_node/rgb/image_rect_color
/zed/zed_node/rgb/image_rect_color/camera_info
/zed/zed_node/right/image_raw_color
/zed/zed_node/right/image_raw_color/camera_info
/zed/zed_node/right/image_rect_color
/zed/zed_node/right/image_rect_color/camera_info
/zed/zed_node/transition_event
/zed_description

Subscribing to one of the /zed/zed_node/XXX topic will not cause a reply immediately since the node will start to elaborate and to publish data on the topics only after it receives an ACTIVATE request.

From now on, you will use the lifecycle command to request a node transition:

$ ros2 lifecycle set /zed/zed_node activate

The reply is more direct:

Transitioning successful

Now the ZED node will start transitioning to the ACTIVE state

1538637193.879887829: [zed.zed_node] [INFO] - *** State transition: activating ***
1538637193.880174212: [zed.zed_node] [INFO] - ZED thread started
1538637193.880381009: [zed.zed_node] [INFO] - No subscribers

You can stop the emission of the [INFO] - No subscribers log simply by subscribing to one of the available topics listed above, e.g. rgb/image_rect_color:

$ ros2 topic echo /zed/zed_node/rgb/image_rect_color
header:
  stamp:
    sec: 1538638311
    nanosec: 652546549
  frame_id: zed_left_camera_optical_frame
height: 720
width: 1280
encoding: bgra8
is_bigendian: 0
step: 5120
data: [41, 31, 36, 255, 43, 32, 35, 255, 46, 33, 35, 255, 48, 36, 37, 255, 45, 38, 42, 255, 42, 39, 46, 255, 46, 41, 48, 255, 58, 44, 51, 255, 60, 42, 47, 255, 50, 31, 37, 255, 39, 27, 31, 255, 33, 26, 28, 255, 34, 27, 29, 255, 36, 28, 30, 255, 37, 28, 30, 255, 38, 30, 31, 255, 47, 37, 37, 255, 53, 42, 43, 255, 52, 40, 41, 255, 47, 37, 36, 255, 43, 35, 31, 255, 47, 39, 35, 255, 53, 50, 43, 255, 61, 60, 53, 255, 72, 70, 63, 255, 80, 75, 70, 255, 83, 75, 70, 255, 71, 63, 58, 255, 53, 49, 44, 255, 37, 37, 32, 255, 31, 31, 25, 255, 36, 31, 26, 255, '...']

[...]

Press Ctrl+C to stop the topic from printing and request a deactivate transition for the ZED node:

$ ros2 lifecycle set /zed/zed_node deactivate
Transitioning successful

The node will stop to elaborate and it will switch back to the INACTIVE state:

1538638416.577877574: [zed.zed_node] [INFO] - *** State transition: deactivating ***
1538638416.608430982: [zed.zed_node] [INFO] - ZED thread finished

You can request the current state of the node at any time using the get option of the lifecycle command:

$ ros2 lifecycle get /zed/zed_node
inactive [2]

The ZED node is now in the INACTIVE state, which has the ID of 2.

You can also get a list of the available transitions from the current state using the --transitions option:

$ ros2 lifecycle get /zed/zed_node --transitions
inactive [2]
-> cleanup [2]
-> activate [3]

You can shut down the ZED node pressing Ctrl+C on the console where it has been started.

Automatic control

In the last session, you learned how to manually control the state transitions of a ZED node. It is very useful when you are debugging your system or when you want to use a ZED camera by hand, but it is not really usable in a robotics application.

Now you will learn how to write a Python launch script that acts as a Lyfecycle node manager. It will monitor the state of the node, request transitions, and launch other nodes when appropriate.

The Python launch script is available in the launch folder: zed.launch.py.

The Python launch script

Let’s start analyzing the Python launch script in detail:

import os

import launch

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import EmitEvent
from launch.actions import LogInfo
from launch.actions import RegisterEventHandler
from launch_ros.actions import Node
from launch_ros.actions import LifecycleNode
from launch_ros.events.lifecycle import ChangeState
from launch_ros.event_handlers import OnStateTransition

import lifecycle_msgs.msg

def generate_launch_description():

    # use: 'zed' for "ZED" camera - 'zedm' for "ZED-M" camera
    camera_model = 'zedm' 

    # URDF file to be loaded by Robot State Publisher
    urdf = os.path.join(get_package_share_directory('stereolabs_zed'), 'urdf', camera_model + '.urdf')
    
    # ZED Configurations to be loaded by ZED Node
    config_common = os.path.join(get_package_share_directory('stereolabs_zed'), 'config', 'common.yaml')

    config_camera = os.path.join(get_package_share_directory('stereolabs_zed'), 'config', camera_model + '.yaml')

    # Set LOG format
    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message}'

    # Launch Description
    ld = launch.LaunchDescription()

    # Prepare the ZED node
    zed_node = LifecycleNode(
        node_namespace = 'zed',        # must match the namespace in config -> YAML
        node_name = 'zed_node',        # must match the node name in config -> YAML
        package = 'stereolabs_zed',
        node_executable = 'zed_wrapper_node',
        output = 'screen',
        parameters = [
            config_common,  # Common parameters
            config_camera,  # Camera related parameters
        ]
    )

    # Prepare the Robot State Publisher node
    rsp_node = Node(
        node_name = 'zed_state_publisher',
        package = 'robot_state_publisher',
        node_executable = 'robot_state_publisher',
        output = 'screen',
        arguments = [urdf, 'robot_description:=zed_description']
    )

    # Make the ZED node take the 'configure' transition
    zed_configure_trans_event = EmitEvent(
        event=ChangeState(
            lifecycle_node_matcher = launch.events.process.matches_action(zed_node),
            transition_id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
        )
    )

    # Make the ZED node take the 'activate' transition
    zed_activate_trans_event = EmitEvent(
        event = ChangeState(
            lifecycle_node_matcher = launch.events.process.matches_action(zed_node),
            transition_id = lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
         )
    )

    # Shutdown event
    shutdown_event = EmitEvent( event = launch.events.Shutdown() )

    # When the ZED node reaches the 'inactive' state from 'unconfigured', make it take the 'activate' transition and start the Robot State Publisher
    zed_inactive_from_unconfigured_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            start_state = 'configuring',
            goal_state = 'inactive',
            entities = [
                # Log
                LogInfo( msg = "'ZED' reached the 'INACTIVE' state, start the 'Robot State Publisher' node and 'activating'." ),
                # Robot State Publisher
                rsp_node,
                # Change State event ( inactive -> active )
                zed_activate_trans_event,
            ],
        )
    )

    # When the ZED node reaches the 'inactive' state from 'active', it has been deactivated and it will wait for a manual activation
    zed_inactive_from_active_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            start_state = 'deactivating',
            goal_state = 'inactive',
            entities = [
                # Log
                LogInfo( msg = "'ZED' reached the 'INACTIVE' state from 'ACTIVE' state. Waiting for manual activation..." )
            ],
        )
    )

    # When the ZED node reaches the 'active' state, log a message.
    zed_active_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            goal_state = 'active',
            entities = [
                # Log
                LogInfo( msg = "'ZED' reached the 'ACTIVE' state" ),
            ],
        )
    )

    # When the ZED node reaches the 'finalized' state, log a message and exit.
    zed_finalized_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            goal_state = 'finalized',
            entities = [
                # Log
                LogInfo( msg = "'ZED' reached the 'FINALIZED' state. Killing the node" ),
                shutdown_event, 
            ],
        )
    )

    # Add the actions to the launch description.
    # The order they are added reflects the order in which they will be executed.
    ld.add_action( zed_inactive_from_unconfigured_state_handler )
    ld.add_action( zed_inactive_from_active_state_handler )
    ld.add_action( zed_active_state_handler )
    ld.add_action( zed_finalized_state_handler )
    ld.add_action( zed_node )
    ld.add_action( zed_configure_trans_event)

    return ld

This script appears more complicated than the script that simply start two nodes without controlling them.

The number of imported libraries is bigger:

import os

import launch

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import EmitEvent
from launch.actions import LogInfo
from launch.actions import RegisterEventHandler
from launch_ros.actions import Node
from launch_ros.actions import LifecycleNode
from launch_ros.events.lifecycle import ChangeState
from launch_ros.event_handlers import OnStateTransition

import lifecycle_msgs.msg

In analyzing the import declarations, take note of ChangeState, OnStateTransition, and lifecycle_msgs.msg, as they are relevant to what we’re about to explore.

Again, the script must declare a generate_launch_description function, which returns a LaunchDescription object to be passed to the ROS2 launch system:

def generate_launch_description():

The part relative to the configuration, the path generation for the configuration files, and the logger formatting are all identical to what we have seen previously:

# use: 'zed' for "ZED" camera - 'zedm' for "ZED-M" camera
    camera_model = 'zedm' 

    # URDF file to be loaded by Robot State Publisher
    urdf = os.path.join(
        get_package_share_directory('stereolabs_zed'), 
            'urdf', camera_model + '.urdf'
    )
    
    # ZED Configurations to be loaded by ZED Node
    config_common = os.path.join(
        get_package_share_directory( 'stereolabs_zed' ), 
        'config', 'common.yaml'
    )

    config_camera = os.path.join(
        get_package_share_directory('stereolabs_zed'), 
        'config', camera_model + '.yaml'
    )

    # Set LOG format
    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message}'

The first difference you can note is the declaration of the LaunchDescription object:

    # Launch Description
    ld = launch.LaunchDescription()

Since the script is more complicated, we declare a variable ld that will be initialized at the end of the script, when all the structures will be created.

The structures that we are going to create are launch actions. An action is the intention to do something, as described in the ROS2 code.

The first action is the *ZED node`*:

    # Prepare the ZED node
    zed_node = LifecycleNode(
        node_namespace = 'zed',        # must match the namespace in config -> YAML
        node_name = 'zed_node',        # must match the node name in config -> YAML
        package = 'stereolabs_zed',
        node_executable = 'zed_wrapper_node',
        output = 'screen',
        parameters = [
            config_common,  # Common parameters
            config_camera,  # Camera related parameters
        ]
    )

The node will live in the zed namespace, with the name zed_node, and will be initialized with the config_common and the config_camera parameters that we created above.

Note:: the node is not yet started. Only the action to start it is initialized.

The second action is the Robot State Publisher node, that has the same scope that we described above:

    # Prepare the Robot State Publisher node
    rsp_node = Node(
        node_name = 'zed_state_publisher',
        package = 'robot_state_publisher',
        node_executable = 'robot_state_publisher',
        output = 'screen',
        arguments = [urdf, 'robot_description:=zed_description']
    )

The third action is an event emission:

    # Make the ZED node take the 'configure' transition
    zed_configure_trans_event = EmitEvent(
        event=ChangeState(
            lifecycle_node_matcher = launch.events.process.matches_action(zed_node),
            transition_id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
        )
    )

This action, when executed, will request a ChangeState of type TRANSITION_CONFIGURE to the zed_node.

The fourth action is another event emission:

    # Make the ZED node take the 'activate' transition
    zed_activate_trans_event = EmitEvent(
        event = ChangeState(
            lifecycle_node_matcher = launch.events.process.matches_action(zed_node),
            transition_id = lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
         )
    )

This action, when executed, will request a ChangeState of type TRANSITION_ACTIVATE to the zed_node.

The fifth action is the last event emission:

# Shutdown event
    shutdown_event = EmitEvent( event = launch.events.Shutdown() )

This action, when executed, will send a ‘SIGINT’ signal to all the running nodes.

The sixth action is an event handler:

# When the ZED node reaches the 'inactive' state from 'unconfigured', make it take the 'activate' transition and start the Robot State Publisher
    zed_inactive_from_unconfigured_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            start_state = 'configuring',
            goal_state = 'inactive',
            entities = [
                # Log
                LogInfo( msg = "'ZED' reached the 'INACTIVE' state, start the 'Robot State Publisher' node and 'activating'." ),
                # Robot State Publisher
                rsp_node,
                # Change State event ( inactive -> active )
                zed_activate_trans_event,
            ],
        )
    )

This action will wait for a state transition of the zed_node from the configuring intermediate state to the inactive state. When the event occurs, the list of actions indicated in the parameter entities will be executed.

Let’s analize in detail what happens: someone has requested a configure transition to the zed_node and now it is in the configuring intermediate state, ready to switch to active state. If the configuration process is successful, we can start the “Robot State Publisher” (rsp_node) node for publishing the static TF of the ZED camera and we can activate the camera itself emitting the zed_activate_trans_event event.

In the list we found also a new action, LogInfo, that is used to publish a log information message.

The seventh action is a new event handler:

    # When the ZED node reaches the 'inactive' state from 'active', it has been deactivated and it will wait for a manual activation
    zed_inactive_from_active_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            start_state = 'deactivating',
            goal_state = 'inactive',
            entities = [
                # Log
                LogInfo( msg = "'ZED' reached the 'INACTIVE' state from 'ACTIVE' state. Waiting for manual activation..." )
            ],
        )
    )

This action is executed when the zed_node transitions from the deactivating intermediate state to the inactive state. It does nothing but publish a log message.

This event happens when someone externally emits a deactivate request for the ZED node, so the node will transition to the inactive state waiting for a shutdown or for a new activate.

Please note that the goal_state of this transition is inactive like the previous. As you can see in the state transition graph at the beginning of the tutorial, the inactive state can be reached from two main states. The start_state value allows the launch system to determine which action to execute.

The eighth action is another event handler:

    # When the ZED node reaches the 'active' state, log a message.
    zed_active_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            goal_state = 'active',
            entities = [
                # Log
                LogInfo( msg = "'ZED' reached the 'ACTIVE' state" ),
            ],
        )
    )

This action is executed when the zed_node transitions from the activating intermediate state to the active state, and again does nothing but publish a log message. In this case, the start_state parameter is not used since the active state can only be reached from the activating intermediate state.

The last action handles the transition to the finalized state:

# When the ZED node reaches the 'finalized' state, log a message and exit.
    zed_finalized_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            goal_state = 'finalized',
            entities = [
                # Log
                LogInfo( msg = "'ZED' reached the 'FINALIZED' state. Killing the node..." ),
                shutdown_event,
            ],
        )
    )

This action is executed when the zed_node transitions to the finalized state. This can happen for a problem communicating with the ZED camera, because the user pressed Ctrl+C or because the node was playing an SVO and it reached the end. The action will output a log message and finally kill the nodes.

Finally, you can put all this together in the launch descripton ld and send it to the ROS2 launch system:

    # Add the actions to the launch description.
    # The order they are added reflects the order in which they will be executed.
    ld.add_action( zed_inactive_from_unconfigured_state_handler )
    ld.add_action( zed_inactive_from_active_state_handler )
    ld.add_action( zed_active_state_handler )
    ld.add_action( zed_finalized_state_handler )
    ld.add_action( zed_node )
    ld.add_action( zed_configure_trans_event)

    return ld

As said in the comment line, the actions are executed in the same order as they are added to the launch descriptor ld, so the three event handlers are first registered, then the zed_node is executed and finally the configure transition is requested to the zed_node.

Running the node

To execute the ZED node in the automatic control mode, open a text console and enter the command:

$ ros2 launch stereolabs_zed zed.launch.py

Barring an error, you will get the ZED node ready as the state transitions will be automatically handled, bringing the node to the active state asking for someone to subscribe to its topics:

$ ros2 launch stereolabs_zed zed.launch.py 
[INFO] [launch]: process[zed_wrapper_node-1]: started with pid [13718]

[...]

1538662028.328568187: [zed.zed_node] [INFO]	Waiting for `CONFIGURE` request...
1538662028.555893616: [zed.zed_node] [INFO]	*** State transition: configuring ***

[...]

1538662029.387776812: [zed.zed_node] [INFO]	ZED Opened

[...]

1538662029.387960475: [zed.zed_node] [INFO]	ZED configured
1538662029.387964091: [zed.zed_node] [INFO]	Waiting for `ACTIVATE` request...
[INFO] [launch.user]: 'ZED' reached the 'INACTIVE' state, start the 'Robot State Publisher' node and 'activating'.
[INFO] [launch]: process[robot_state_publisher-2]: started with pid [13753]
[INFO] [launch.user]: 'ZED' reached the 'ACTIVE' state
1538662029.395259693: [zed.zed_node] [INFO]	*** State transition: activating ***
1538662029.395495397: [zed.zed_node] [INFO]	ZED thread started
[...]
Initialize urdf model from file: /home/user_name/ros2_ws/install/stereolabs_zed/share/stereolabs_zed/urdf/zedm.urdf
Parsing robot urdf xml string.
Link zed_camera_center had 2 children
Link zed_left_camera_frame had 1 children
Link zed_left_camera_optical_frame had 0 children
Link zed_right_camera_frame had 1 children
Link zed_right_camera_optical_frame had 0 children
got segment base_link
got segment zed_camera_center
got segment zed_left_camera_frame
got segment zed_left_camera_optical_frame
got segment zed_right_camera_frame
got segment zed_right_camera_optical_frame
Adding fixed segment from base_link to zed_camera_center
Adding fixed segment from zed_camera_center to zed_left_camera_frame
Adding fixed segment from zed_left_camera_frame to zed_left_camera_optical_frame
Adding fixed segment from zed_camera_center to zed_right_camera_frame
Adding fixed segment from zed_right_camera_frame to zed_right_camera_optical_frame
1538662034.522555007: [zed.zed_node] [INFO]	No subscribers
1538662039.655471173: [zed.zed_node] [INFO]	No subscribers

[...]

Node monitoring

One of the most interesting aspects of a managed lifecycle node is that it automatically publishes a topic with the information about its state transitions.

In the case of the ZED node, you can subscribe to the topic /zed/zed_node/transition_event to monitor the status of the node in real time. This topic is available as soon as the node starts, differently from the other topics that can be subscribed only after that the ZED node entered the configured state.

Open a new text console and launch the unmanaged script:

$ ros2 launch stereolabs_zed zed_unmanaged.launch.py

Open a second console and subscribe to the topic transition_event, to echo any published message:

$ ros2 topic echo /zed/zed_node/transition_event

finally open a third text console and start to request transitions as you learned above to see what happens:

ros2 lifecycle set /zed/zed_node configure
Transitioning successful

On the second console, you’ll receive the transition messages:

timestamp: 0
transition:
  id: 0
  label: ''
start_state:
  id: 1
  label: unconfigured
goal_state:
  id: 10
  label: configuring

timestamp: 0
transition:
  id: 0
  label: ''
start_state:
  id: 10
  label: configuring
goal_state:
  id: 2
  label: inactive

Two messages has been received: the first for the transition from unconfigured to configuring, the second for the transition from configuring to inactive.

Now you can try to activate the ZED:

$ ros2 lifecycle set /zed/zed_node activate

You’ll receive these two new messages on the second console:

timestamp: 0
transition:
  id: 0
  label: ''
start_state:
  id: 2
  label: inactive
goal_state:
  id: 13
  label: activating

timestamp: 0
transition:
  id: 0
  label: ''
start_state:
  id: 13
  label: activating
goal_state:
  id: 3
  label: active

The first message has been published for the transition from inactive to activating; The second for the transition from activating to active.

As you may have noticed, each message reports the start_state and the goal_state, each with the relative label and the id. You can understand that it can be simple to write you own diagnostic node that subscribes to the topic /zed/zed_node/transition_event and gets information about the current status of the ZED node.

A lifecycle monitor in C++

Now you will learn how to write a simple C++ node that subscribes to the topic /zed/zed_node/transition_event to monitor the ZED node status.

Introduction

Use this command to connect the ZED camera to the ROS network using the unmanaged launch script:

$ ros2 launch stereolabs_zed zed_unmanaged.launch.py

Running the tutorial

If you properly followed the installation guide, the executable of this tutorial has been compiled and you can run the subscriber node opening a second text console and writing this command:

$ ros2 run stereolabs_zed_tutorial_lifecycle zed_lifecycle_tutorial

The monitor node starts, waiting for transition messages from the ZED node:

[INFO] [zed_lifecycle_sub]: Waiting for transition messages on topic `/zed/zed_node/transition_event`... 

Open a third console and start to request transitions like you did for the previous tutorial:

$ ros2 lifecycle set /zed/zed_node configure
Transitioning successful

On the second text console, you can see how the monitor node receives the transition information, writing log messages on screen:

[INFO] [zed_lifecycle_sub]: Transition received: 
[INFO] [zed_lifecycle_sub]: 	'unconfigured [1]' -> 'configuring [10]'
[INFO] [zed_lifecycle_sub]: Transition received: 
[INFO] [zed_lifecycle_sub]: 	'configuring [10]' -> 'inactive [2]'

As in the previous tutorial, the CONFIGURE request generates two transitions.

Before starting to examinate the C++ code, you can play with the node requesting other transitions by CLI commands and seeing how the node reacts:

Request:

$ ros2 lifecycle set /zed/zed_node activate
Transitioning successful

Transitions:

[INFO] [zed_lifecycle_sub]: Transition received: 
[INFO] [zed_lifecycle_sub]: 	'inactive [2]' -> 'activating [13]'
[INFO] [zed_lifecycle_sub]: Transition received: 
[INFO] [zed_lifecycle_sub]: 	'activating [13]' -> 'active [3]'

Request:

$ ros2 lifecycle set /zed/zed_node deactivate
Transitioning successful

Transitions:

[INFO] [zed_lifecycle_sub]: Transition received: 
[INFO] [zed_lifecycle_sub]: 	'active [3]' -> 'deactivating [14]'
[INFO] [zed_lifecycle_sub]: Transition received: 
[INFO] [zed_lifecycle_sub]: 	'deactivating [14]' -> 'inactive [2]'

Request:

$ ros2 lifecycle set /zed/zed_node cleanup
Transitioning successful

Transitions:

[INFO] [zed_lifecycle_sub]: Transition received: 
[INFO] [zed_lifecycle_sub]: 	'active [3]' -> 'deactivating [14]'
[INFO] [zed_lifecycle_sub]: Transition received: 
[INFO] [zed_lifecycle_sub]: 	'deactivating [14]' -> 'inactive [2]'

The code

The source code of the subscriber node zed_lifecycle_sub_tutorial.cpp:

#include <string>

#include "rclcpp/rclcpp.hpp"
#include "lifecycle_msgs/msg/transition_event.hpp"

class MinimalLifecycleSubscriber : public rclcpp::Node {
  public:
    MinimalLifecycleSubscriber()
        : Node("zed_lifecycle_sub") {

        std::string topic = "/zed/zed_node/transition_event";

        mSub = create_subscription<lifecycle_msgs::msg::TransitionEvent>(
                   topic,
        [this](lifecycle_msgs::msg::TransitionEvent::UniquePtr msgPtr) {
            lifecycle_msgs::msg::TransitionEvent msg = *msgPtr.get();

            RCLCPP_INFO(get_logger(), "Transition received: ");

            RCLCPP_INFO(get_logger(), "\t'%s [%d]' -> '%s [%d]'",
                        msg.start_state.label.c_str(), msg.start_state.id,
                        msg.goal_state.label.c_str(), msg.goal_state.id);
        });

        RCLCPP_INFO(get_logger(), "Waiting for transition messages on topic `%s`... ",
                    topic.c_str());
    }

  private:
    rclcpp::Subscription<lifecycle_msgs::msg::TransitionEvent>::SharedPtr mSub;
};

// The main function
int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MinimalLifecycleSubscriber>());
    rclcpp::shutdown();
    return 0;
}

The tutorial was written using the new concept of Component introduced in ROS2 in order to take advantage of the node composition capatibilities.

A component named MinimalLifecycleSubscriber is created subclassing the ROS2 object rclcpp::Node.

In the constructor, we initialize the Node parent class with the name of our new node zed_depth_sub:

    MinimalLifecycleSubscriber()
        : Node("zed_lifecycle_sub") {

        std::string topic = "/zed/zed_node/transition_event";

        mSub = create_subscription<lifecycle_msgs::msg::TransitionEvent>(
                   topic,
        [this](lifecycle_msgs::msg::TransitionEvent::UniquePtr msgPtr) {
            lifecycle_msgs::msg::TransitionEvent msg = *msgPtr.get();

            RCLCPP_INFO(get_logger(), "Transition received: ");

            RCLCPP_INFO(get_logger(), "\t'%s [%d]' -> '%s [%d]'",
                        msg.start_state.label.c_str(), msg.start_state.id,
                        msg.goal_state.label.c_str(), msg.goal_state.id);
        });

        RCLCPP_INFO(get_logger(), "Waiting for transition messages on topic `%s`... ",
                    topic.c_str());
    }

The constructor mainly defines mSub, a std::SharedPtr to the private variable object that creates a subscriber in the node. The subscriber checks for topics of type /zed/zed_node/transition_event and executes a lambda expression every time it receives one.

ROS 2 uses the new costructs available with C++11 and C++14. This tutorial has been designed to illustrate how lambda expression can be used instead of callback functions.

The lambda expression has the pointer this as capture, lifecycle_msgs::msg::TransitionEvent::UniquePtr msgPtr as parameter and returns nothing. From the msgPtr variable, we extract the message object msg and we process it to get the start_state and the goal_state of the transition to print the information on screen.

Conclusions

With this tutorial, you learned how a managed life cycle node works in ROS2, how to control it, and how to monitor its behaviors. The zed.launch.py python launch script can be a useful way to design the launch script for you robot. For example, it could start the obstacle detection and the motor control nodes only when you are sure that all the sensors are connected and working (their node is in the activated state), like we did for the “Robot State Publisher” node and the ZED node.