0
$\begingroup$

Rosanswers logo

Hi,

I'm trying to write a Python launch file that handles the state transitions of my lifecycle node. In particular I would like to launch a "Robot State Publisher" node when my node reaches the inactive state after that it as been configured.

I followed as example the launch file on Github as indicated here:

https://github.com/ros2/launch/blob/master/launch_ros/examples/lifecycle_pub_sub_launch.py

but I get the following error when I launch it:

[ERROR] [launch.LaunchService]: Caught exception in launch (see debug for traceback): launch file at '/home/walter/devel/ros2_ws/install/stereolabs_zed/share/stereolabs_zed/launch/zed.py' does not contain the required function 'generate_launch_description()'

Is the example updated to the new launch style available in Bouncy?

This is my launch:

"""Launch a lifecycle ZED node and the Robot State Publisher"""

import os
import sys
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))  # noqa
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'launch'))  # noqa

import launch
import launch.actions
import launch.events

from ament_index_python.packages import get_package_share_directory
from launch_ros import get_default_launch_description
import launch_ros.actions
import launch_ros.events
import launch_ros.events.lifecycle

import lifecycle_msgs.msg


def main(argv=sys.argv[1:]):
    """Main."""    

    # use: 'zed' for "ZED" camera - 'zedm' for "ZED mini" 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}'

    # Prepare the ZED node
    zed_node = launch_ros.actions.LifecycleNode( package='stereolabs_zed', 
                                                 node_executable='zed_wrapper_node',
                                                 output='screen', 
                                                 arguments=['__params:='+config_common, # Common parameters
                                                            '__params:='+config_camera  # Camera related parameters
                                                           ]
                                               )
    # Launch Description
    ld = launch.LaunchDescription()

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

    # When the ZED node reaches the 'inactive' state, start the Robot State Publisher and make it take the 'activate' transition.
    register_event_handler_for_zed_reaches_inactive_state = launch.actions.RegisterEventHandler(
        launch_ros.event_handlers.OnStateTransition(
            target_lifecycle_node=zed_node, goal_state='inactive',
            entities=[
                       # Log
                       launch.actions.LogInfo(msg="'ZED' reached the 'inactive' state, start the 'Robot State Publisher' node and 'activating'."),
                       # Robot State Publisher
                       launch_ros.actions.Node( package='robot_state_publisher', 
                                                node_executable='robot_state_publisher',
                                                output='screen', 
                                                arguments=[urdf]
                                              ),
                       # Change State event
                       launch.actions.EmitEvent( event=launch_ros.events.lifecycle.ChangeState(
                         lifecycle_node_matcher=launch.events.process.matches_action(zed_node),
                         transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
                        )),
             ],
        )
    )

    # When the ZED node reaches the 'active' state, log a message.
    register_event_handler_for_talker_reaches_active_state = launch.actions.RegisterEventHandler(
        launch_ros.event_handlers.OnStateTransition(
            target_lifecycle_node=talker_node, goal_state='active',
            entities=[ launch.actions.LogInfo( msg="'ZED' reached the 'active' state" ),],
        )
    )    

    # Add the actions to the launch description.
    # The order they are added reflects the order in which they will be executed.
    ld.add_action(register_event_handler_for_zed_reaches_inactive_state)
    ld.add_action(register_event_handler_for_zed_reaches_active_state)
    ld.add_action(zed_node)
    ld.add_action(emit_event_to_request_that_zed_does_configure_transition)

    print('Starting introspection of launch description...')
    print('')

    print(launch.LaunchIntrospector().format_launch_description(ld))

    print('')
    print('Starting launch of launch description...')
    print('')

    # ls = launch.LaunchService(argv=argv, debug=True)
    ls = launch.LaunchService(argv=argv)
    ls.include_launch_description(get_default_launch_description(prefix_output_with_name=False))
    ls.include_launch_description(ld)
    return ls.run()


if __name__ == '__main__':
    main()

My ROS2 configuration:

  • Ubuntu 16.04

  • ROS2 Bouncy from source

  • Updated to latest master


Originally posted by Myzhar on ROS Answers with karma: 541 on 2018-09-27

Post score: 0


Original comments

Comment by lucasw on 2018-11-08:
Caught exception in launch (see debug for traceback): -> Add -d to ros2 launch before package and launch file name to get the traceback:

ros2 launch -d foo bar_launch.py
$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

If you want to start the launch file with the ros2 launch command, you should define a generate_launch_description function that returns the LaunchDescriptioninstead of a main.

E.g

def generate_launch_description():

    # ...

    # Launch Description
    ld = launch.LaunchDescription()

    # ...
  
    # Add the actions to the launch description.
    # The order they are added reflects the order in which they will be executed.
    ld.add_action(register_event_handler_for_zed_reaches_inactive_state)
    ld.add_action(register_event_handler_for_zed_reaches_active_state)
    ld.add_action(zed_node)
    ld.add_action(emit_event_to_request_that_zed_does_configure_transition)

    return ld

Originally posted by jacobperron with karma: 1870 on 2018-09-27

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Myzhar on 2018-09-27:
Thank you. I missed the information about how to migrate from legacy launch to the new system available in Bouncy. Very useful

$\endgroup$
0
$\begingroup$

Rosanswers logo

This is the final version of the converted launch file. Thank you @jacobperron :

"""Launch a lifecycle ZED node and the Robot State Publisher"""

import os


import launch
from launch import LaunchIntrospector

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 mini" 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,
         )
    )

    # When the ZED node reaches the 'inactive' state, make it take the 'activate' transition and start the Robot State Publisher
    zed_inactive_state_handler = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node = zed_node,
            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 '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" ),
            ],
        )
    )

    # 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_state_handler )
    ld.add_action( zed_active_state_handler )
    ld.add_action( zed_node )
    ld.add_action( zed_configure_trans_event)

    return ld

Originally posted by Myzhar with karma: 541 on 2018-09-28

This answer was NOT ACCEPTED on the original site

Post score: 4


Original comments

Comment by lingjie on 2021-02-01:
Similar to how we start the rsp node when ed node reach inactive state.

Is there a way to stop a running ROS node when the lifecycle node reach a specific state?

Thank you.

Comment by flynneva on 2021-05-25:
thank you for posting this!!!! spent a long time searching for the solution..this was it!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.