Skip to content

Lifecyle node failed to make transition #1166

Open
@kjjpc

Description

@kjjpc

Bug report

  • Operating System:
    • Ubuntu 22.04
  • Installation type:
    • binaries
  • Version or commit hash:
    • ros-jazzy-ros-base=0.11.0-1 of apt package
  • DDS implementation:
    • Fast DDS (Default DDS of jazzy)
  • Client library (if applicable):
    • rclpy

Steps to reproduce issue

When I launch multiple lifecycle nodes using LaunchService, some nodes failed to be activated.
This is a reproduction script.

import os
import signal

from launch import LaunchDescription
from launch.actions import EmitEvent
from launch.actions import RegisterEventHandler
from launch.event_handlers import OnProcessStart
import launch.events
from launch.launch_description_entity import LaunchDescriptionEntity
from launch.launch_service import LaunchService
import launch_ros
from launch_ros.actions import LifecycleNode
from launch_ros.events.lifecycle import ChangeState
from lifecycle_msgs.msg import Transition
from multiprocessing import Process
from typing import List


def up_launch(launch_description_list: List[LaunchDescriptionEntity]):
    ls = LaunchService()
    ls.include_launch_description(LaunchDescription(launch_description_list))
    ls.run()


def generate_launch_description(ns):
    lc_node = LifecycleNode(
        package='lifecycle_py',
        executable='lifecycle_talker',
        name='lc_talker',
        namespace=ns,
        output='screen',
    )
    emit_configure = RegisterEventHandler(
        OnProcessStart(
            target_action=lc_node,
            on_start=[
                EmitEvent(event=ChangeState(
                    lifecycle_node_matcher=launch.events.matches_action(lc_node),
                    transition_id=Transition.TRANSITION_CONFIGURE,
                )),
            ]))

    #  send activate when on_configure finished
    emit_active = RegisterEventHandler(
        launch_ros.event_handlers.OnStateTransition(
            target_lifecycle_node=lc_node,
            goal_state='inactive',
            # start_state='configuring',
            entities=[
                EmitEvent(event=ChangeState(
                    lifecycle_node_matcher=launch.events.matches_action(lc_node),
                    transition_id=Transition.TRANSITION_ACTIVATE,
                )),
            ],
        ))

    return [lc_node, emit_configure, emit_active]


if __name__ == '__main__':
    os.setpgrp()
    try:
        for i in range(12):
            ls = generate_launch_description('room'+str(i))
            launch_proc = Process(target=up_launch,
                                  args=(ls,),
                                  )
            launch_proc.start()
    except KeyboardInterrupt:
        print('group kill')
        os.killpg(0, signal.SIGINT)

Expected behavior

All lifecycle nodes become active.

Actual behavior

Some nodes remains configured occasionally.

Additional information

I investigated the behavior and found that the lifecycle_node action of launch_ros failed to catch transition_event topic.
The lifecycle_node action create subscriber just before calling change_state and the topic subscription registration is not completed before transition_event publish.

One solution is to make transition_event topic reliable and transient local.
Following changes resolve the issue.

Add following code to com_interface.c.

   rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+  publisher_options.qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  publisher_options.qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  publisher_options.qos.depth = 1;
   rcl_ret_t ret = rcl_publisher_init(

Change qos setting in lifecycle_node.py of launch_ros.

         self.__rclpy_subscription = node.create_subscription(
             lifecycle_msgs.msg.TransitionEvent,
             '{}/transition_event'.format(self.node_name),
             functools.partial(self._on_transition_event, context),
-            10)
+            QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE,
+                       durability=DurabilityPolicy.TRANSIENT_LOCAL))

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions