diff --git a/launch_ros/launch_ros/actions/explicit_ros_startup.py b/launch_ros/launch_ros/actions/explicit_ros_startup.py deleted file mode 100644 index fa59c083..00000000 --- a/launch_ros/launch_ros/actions/explicit_ros_startup.py +++ /dev/null @@ -1,60 +0,0 @@ -# Copyright 2018-2020 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import List -from typing import Optional - -import launch.actions -import launch_ros.ros_adapters - -import rclpy - - -class ExplicitROSStartup(launch.actions.OpaqueFunction): - """Does ROS specific launch startup explicitly.""" - - def __init__( - self, - *, - argv: Optional[List[str]] = None, - rclpy_context: Optional[rclpy.Context] = None - ): - """ - Create a ExplicitROSStartup opaque function. - - :param: argv List of global arguments for rclpy context initialization. - :param: rclpy_context Provide a context other than the default rclpy context - to pass down to rclpy.init. - The context is expected to have already been initialized by the caller - using rclpy.init(). - """ - super().__init__(function=self._function) - self.__ros_adapter = launch_ros.ros_adapters.ROSAdapter( - argv=argv, context=rclpy_context, autostart=False - ) - - def _shutdown(self, event: launch.Event, context: launch.LaunchContext): - self.__ros_adapter.shutdown() - - def _function(self, context: launch.LaunchContext): - if hasattr(context.locals, 'ros_adapter'): - raise RuntimeError( - 'A ROS adapter is already been managed by this ' - 'launch context. Duplicate explicit ROS startup?' - ) - context.extend_globals({'ros_adapter': self.__ros_adapter}) - context.register_event_handler(launch.event_handlers.OnShutdown( - on_shutdown=self._shutdown - )) - self.__ros_adapter.start() diff --git a/launch_ros/launch_ros/ros_adapters.py b/launch_ros/launch_ros/ros_adapters.py index 02b88028..0872e5f1 100644 --- a/launch_ros/launch_ros/ros_adapters.py +++ b/launch_ros/launch_ros/ros_adapters.py @@ -33,23 +33,18 @@ def __init__( self, *, argv: Optional[List[str]] = None, - context: Optional[rclpy.Context] = None, autostart: bool = True ): """ Construct adapter. :param: argv List of global arguments for rclpy context initialization. - :param: context Provide a context other than the default rclpy context - to pass down to rclpy.init. - The context is expected to have already been initialized by the caller - using rclpy.init(). :param: autostart Whether to start adapter on construction or not. """ self.__argv = argv - self.__context = context - self.__node = None - self.__executor = None + self.__ros_context = None + self.__ros_node = None + self.__ros_executor = None self.__is_running = False if autostart: @@ -59,59 +54,55 @@ def start(self): """Start ROS adapter.""" if self.__is_running: raise RuntimeError('Cannot start a ROS adapter that is already running') - try: - if self.__context is not None: - rclpy.init(args=self.__argv, context=self.__context) - except RuntimeError as exc: - if 'rcl_init called while already initialized' in str(exc): - pass - raise - self.__node = rclpy.create_node( + self.__ros_context = rclpy.Context() + rclpy.init(args=self.__argv, context=self.__ros_context) + self.__ros_node = rclpy.create_node( 'launch_ros_{}'.format(os.getpid()), - context=self.__context + context=self.__ros_context ) + self.__ros_executor = SingleThreadedExecutor(context=self.__ros_context) self.__is_running = True - self.__executor = SingleThreadedExecutor(context=self.__context) - self.__executor_thread = threading.Thread(target=self._run) - self.__executor_thread.start() + + self.__ros_executor_thread = threading.Thread(target=self._run) + self.__ros_executor_thread.start() def _run(self): try: - self.__executor.add_node(self.__node) + self.__ros_executor.add_node(self.__ros_node) while self.__is_running: # TODO(wjwwood): switch this to `spin()` when it considers # asynchronously added subscriptions. # see: https://github.com/ros2/rclpy/issues/188 - self.__executor.spin_once(timeout_sec=1.0) + self.__ros_executor.spin_once(timeout_sec=1.0) except KeyboardInterrupt: pass finally: - self.__executor.remove_node(self.__node) + self.__ros_executor.remove_node(self.__ros_node) def shutdown(self): """Shutdown ROS adapter.""" if not self.__is_running: raise RuntimeError('Cannot shutdown a ROS adapter that is not running') - self.__executor_thread.join() - self.__node.destroy_node() - rclpy.shutdown(context=self.__context) self.__is_running = False + self.__ros_executor_thread.join() + self.__ros_node.destroy_node() + rclpy.shutdown(context=self.__ros_context) @property def argv(self): return self.__argv @property - def context(self): - return self.__context + def ros_context(self): + return self.__ros_context @property - def node(self): - return self.__node + def ros_node(self): + return self.__ros_node @property - def executor(self): - return self.__executor + def ros_executor(self): + return self.__ros_executor def get_ros_adapter(*, context: launch.LaunchContext): @@ -135,4 +126,4 @@ def get_ros_node(*, context: launch.LaunchContext): If no node is found, one will be created. """ - return get_ros_adapter(context).node + return get_ros_adapter(context).ros_node diff --git a/ros2launch/ros2launch/api/api.py b/ros2launch/ros2launch/api/api.py index aa26f568..ec6fa4c2 100644 --- a/ros2launch/ros2launch/api/api.py +++ b/ros2launch/ros2launch/api/api.py @@ -25,8 +25,6 @@ import launch from launch.frontend import Parser from launch.launch_description_sources import get_launch_description_from_any_launch_file -import launch_ros -import launch_ros.actions class MultipleLaunchFilesError(Exception):