1414
1515"""Module for the LifecycleNode action."""
1616
17- import functools
18- import threading
19- from typing import cast
2017from typing import List
2118from typing import Optional
2219
2825import lifecycle_msgs .msg
2926import lifecycle_msgs .srv
3027
28+ from .lifecycle_transition import LifecycleTransition
3129from .node import Node
32- from ..events .lifecycle import ChangeState
33- from ..events .lifecycle import StateTransition
3430
35- from ..ros_adapters import get_ros_node
31+ from ..utilities import LifecycleEventManager
3632
3733
3834class LifecycleNode (Node ):
@@ -43,6 +39,7 @@ def __init__(
4339 * ,
4440 name : SomeSubstitutionsType ,
4541 namespace : SomeSubstitutionsType ,
42+ autostart : bool = False ,
4643 ** kwargs
4744 ) -> None :
4845 """
@@ -71,70 +68,25 @@ def __init__(
7168 :param name: The name of the lifecycle node.
7269 Although it defaults to None it is a required parameter and the default will be removed
7370 in a future release.
71+ :param namespace: The ROS namespace for this Node
72+ :param autostart: Whether or not to automatically transition to the 'active' state.
7473 """
7574 super ().__init__ (name = name , namespace = namespace , ** kwargs )
7675 self .__logger = launch .logging .get_logger (__name__ )
77- self .__rclpy_subscription = None
78- self .__current_state = \
79- ChangeState .valid_states [lifecycle_msgs .msg .State .PRIMARY_STATE_UNKNOWN ]
80-
81- def _on_transition_event (self , context , msg ):
82- try :
83- event = StateTransition (action = self , msg = msg )
84- self .__current_state = ChangeState .valid_states [msg .goal_state .id ]
85- context .asyncio_loop .call_soon_threadsafe (lambda : context .emit_event_sync (event ))
86- except Exception as exc :
87- self .__logger .error (
88- "Exception in handling of 'lifecycle.msg.TransitionEvent': {}" .format (exc ))
89-
90- def _call_change_state (self , request , context : launch .LaunchContext ):
91- while not self .__rclpy_change_state_client .wait_for_service (timeout_sec = 1.0 ):
92- if context .is_shutdown :
93- self .__logger .warning (
94- "Abandoning wait for the '{}' service, due to shutdown." .format (
95- self .__rclpy_change_state_client .srv_name ),
96- )
97- return
98-
99- # Asynchronously wait so that we can periodically check for shutdown.
100- event = threading .Event ()
101-
102- def unblock (future ):
103- nonlocal event
104- event .set ()
105-
106- response_future = self .__rclpy_change_state_client .call_async (request )
107- response_future .add_done_callback (unblock )
108-
109- while not event .wait (1.0 ):
110- if context .is_shutdown :
111- self .__logger .warning (
112- "Abandoning wait for the '{}' service response, due to shutdown." .format (
113- self .__rclpy_change_state_client .srv_name ),
114- )
115- response_future .cancel ()
116- return
117-
118- if response_future .exception () is not None :
119- raise response_future .exception ()
120- response = response_future .result ()
121-
122- if not response .success :
123- self .__logger .error (
124- "Failed to make transition '{}' for LifecycleNode '{}'" .format (
125- ChangeState .valid_transitions [request .transition .id ],
126- self .node_name ,
127- )
128- )
129-
130- def _on_change_state_event (self , context : launch .LaunchContext ) -> None :
131- typed_event = cast (ChangeState , context .locals .event )
132- if not typed_event .lifecycle_node_matcher (self ):
133- return None
134- request = lifecycle_msgs .srv .ChangeState .Request ()
135- request .transition .id = typed_event .transition_id
136- context .add_completion_future (
137- context .asyncio_loop .run_in_executor (None , self ._call_change_state , request , context ))
76+ self .__autostart = autostart
77+ self .__lifecycle_event_manager = None
78+
79+ @property
80+ def node_autostart (self ):
81+ """Getter for autostart."""
82+ return self .__autostart
83+
84+ @property
85+ def is_lifecycle_node (self ):
86+ return True
87+
88+ def get_lifecycle_event_manager (self ):
89+ return self .__lifecycle_event_manager
13890
13991 def execute (self , context : launch .LaunchContext ) -> Optional [List [Action ]]:
14092 """
@@ -145,21 +97,30 @@ def execute(self, context: launch.LaunchContext) -> Optional[List[Action]]:
14597 self ._perform_substitutions (context ) # ensure self.node_name is expanded
14698 if '<node_name_unspecified>' in self .node_name :
14799 raise RuntimeError ('node_name unexpectedly incomplete for lifecycle node' )
148- node = get_ros_node (context )
149- # Create a subscription to monitor the state changes of the subprocess.
150- self .__rclpy_subscription = node .create_subscription (
151- lifecycle_msgs .msg .TransitionEvent ,
152- '{}/transition_event' .format (self .node_name ),
153- functools .partial (self ._on_transition_event , context ),
154- 10 )
155- # Create a service client to change state on demand.
156- self .__rclpy_change_state_client = node .create_client (
157- lifecycle_msgs .srv .ChangeState ,
158- '{}/change_state' .format (self .node_name ))
159- # Register an event handler to change states on a ChangeState lifecycle event.
160- context .register_event_handler (launch .EventHandler (
161- matcher = lambda event : isinstance (event , ChangeState ),
162- entities = [launch .actions .OpaqueFunction (function = self ._on_change_state_event )],
163- ))
100+
101+ self .__lifecycle_event_manager = LifecycleEventManager (self )
102+ self .__lifecycle_event_manager .setup_lifecycle_manager (context )
103+
104+ # If autostart is enabled, transition to the 'active' state.
105+ autostart_actions = None
106+ if self .node_autostart :
107+ autostart_actions = [
108+ LifecycleTransition (
109+ lifecycle_node_names = [self .node_name ],
110+ transition_ids = [lifecycle_msgs .msg .Transition .TRANSITION_CONFIGURE ]
111+ ),
112+ LifecycleTransition (
113+ lifecycle_node_names = [self .node_name ],
114+ transition_ids = [lifecycle_msgs .msg .Transition .TRANSITION_ACTIVATE ]
115+ ),
116+ ]
117+
164118 # Delegate execution to Node and ExecuteProcess.
165- return super ().execute (context )
119+ node_actions = super ().execute (context ) # type: Optional[List[Action]]
120+ if node_actions is not None and autostart_actions is not None :
121+ return node_actions + autostart_actions
122+ if node_actions is not None :
123+ return node_actions
124+ if autostart_actions is not None :
125+ return autostart_actions
126+ return None
0 commit comments