def _function(self, context: launch.LaunchContext): try: rclpy.init(args=context.argv) 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') context.extend_globals({ 'ros_startup_action': self, 'launch_ros_node': self.__launch_ros_node }) context.register_event_handler( launch.event_handlers.OnShutdown(on_shutdown=self._shutdown, )) self.__rclpy_spin_thread = threading.Thread(target=self._run) self.__rclpy_spin_thread.start()
def get_ros_adapter(context: launch.LaunchContext): """ Get the ROS adapter managed by the given launch context. If no adapter is found, one will be created. This function is reentrant but concurrent calls on the same `context` are not safe. """ if not hasattr(context.locals, 'ros_adapter'): ros_adapter = ROSAdapter() context.extend_globals({'ros_adapter': ros_adapter}) context.register_event_handler( launch.event_handlers.OnShutdown( on_shutdown=lambda *args, **kwargs: ros_adapter.shutdown())) return context.locals.ros_adapter