Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plumb rclpy.init context to get_default_launch_description #193

Merged
merged 3 commits into from
Mar 7, 2019
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 11 additions & 6 deletions launch_ros/launch_ros/default_launch_description.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
import launch.events

import rclpy
from rclpy.executors import SingleThreadedExecutor

_logger = logging.getLogger('launch_ros')
_process_log_files = {} # type: Dict[Text, TextIO]
Expand Down Expand Up @@ -68,18 +69,19 @@ def _on_process_output(event: launch.Event, *, file_name: Text, prefix_output: b
class ROSSpecificLaunchStartup(launch.actions.OpaqueFunction):
"""Does ROS specific launch startup."""

def __init__(self):
def __init__(self, rclpy_context=None):
"""Constructor."""
super().__init__(function=self._function)
self.__shutting_down = False
self.__rclpy_context = rclpy_context

def _shutdown(self, event: launch.Event, context: launch.LaunchContext):
self.__shutting_down = True
self.__rclpy_spin_thread.join()
self.__launch_ros_node.destroy_node()

def _run(self):
executor = rclpy.get_global_executor()
executor = SingleThreadedExecutor(context=self.__rclpy_context)
try:
executor.add_node(self.__launch_ros_node)
while not self.__shutting_down:
Expand All @@ -94,12 +96,12 @@ def _run(self):

def _function(self, context: launch.LaunchContext):
try:
rclpy.init(args=context.argv)
rclpy.init(args=context.argv, context=self.__rclpy_context)
wjwwood marked this conversation as resolved.
Show resolved Hide resolved
except RuntimeError as exc:
if 'rcl_init called while already initialized' in str(exc):
pass
raise
self.__launch_ros_node = rclpy.create_node('launch_ros')
self.__launch_ros_node = rclpy.create_node('launch_ros', context=self.__rclpy_context)
context.extend_globals({
'ros_startup_action': self,
'launch_ros_node': self.__launch_ros_node
Expand All @@ -111,17 +113,20 @@ def _function(self, context: launch.LaunchContext):
self.__rclpy_spin_thread.start()


def get_default_launch_description(*, prefix_output_with_name=False):
def get_default_launch_description(*, prefix_output_with_name=False, rclpy_context=None):
"""
Return a LaunchDescription to be included before user descriptions.

:param: prefix_output_with_name if True, each line of output is prefixed
with the name of the process as `[process_name] `, else it is printed
unmodified

:param: rclpy_context Provide a context other than the default rclpy context pass down
to rclpy.init
"""
default_ros_launch_description = launch.LaunchDescription([
# ROS initialization (create node and other stuff).
ROSSpecificLaunchStartup(),
ROSSpecificLaunchStartup(rclpy_context=rclpy_context),
# Handle process starts.
launch.actions.RegisterEventHandler(launch.EventHandler(
matcher=lambda event: isinstance(event, launch.events.process.ProcessStarted),
Expand Down