diff --git a/rosbridge_server/scripts/rosbridge_websocket.py b/rosbridge_server/scripts/rosbridge_websocket.py index e3ad40a6c..089ce95ce 100755 --- a/rosbridge_server/scripts/rosbridge_websocket.py +++ b/rosbridge_server/scripts/rosbridge_websocket.py @@ -31,11 +31,14 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +import asyncio +import signal import sys +import threading import time import rclpy +from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile from rosbridge_library.capabilities.advertise import Advertise @@ -46,26 +49,18 @@ from rosbridge_library.capabilities.unadvertise_service import UnadvertiseService from std_msgs.msg import Int32 from tornado.httpserver import HTTPServer -from tornado.ioloop import IOLoop, PeriodicCallback from tornado.netutil import bind_sockets from tornado.web import Application from rosbridge_server import ClientManager, RosbridgeWebSocket -def start_hook(): - IOLoop.instance().start() - - -def shutdown_hook(): - IOLoop.instance().stop() - - class RosbridgeWebsocketNode(Node): def __init__(self): super().__init__("rosbridge_websocket") RosbridgeWebSocket.node_handle = self + RosbridgeWebSocket.event_loop = asyncio.get_event_loop() ################################################## # Parameter handling # @@ -332,31 +327,42 @@ def protocol_parameter_handling(self): CallService.services_glob = RosbridgeWebSocket.services_glob -def main(args=None): - if args is None: - args = sys.argv +async def async_main(): + rclpy.init(args=sys.argv, signal_handler_options=rclpy.signals.SignalHandlerOptions.NO) - rclpy.init(args=args) node = RosbridgeWebsocketNode() - executor = rclpy.executors.SingleThreadedExecutor() + executor = SingleThreadedExecutor() executor.add_node(node) - def spin_ros(): - executor.spin_once(timeout_sec=0.01) - if not rclpy.ok(): - shutdown_hook() - - spin_callback = PeriodicCallback(spin_ros, 1) - spin_callback.start() - try: - start_hook() - node.destroy_node() - rclpy.shutdown() - except KeyboardInterrupt: + spin_thread = threading.Thread(target=executor.spin) + spin_thread.start() + + loop = asyncio.get_running_loop() + stop_event = asyncio.Event() + signal_handled = False + + def handle_signal(): + nonlocal signal_handled + if signal_handled: + return print("Exiting due to SIGINT") - finally: - shutdown_hook() # shutdown hook to stop the server + stop_event.set() + executor.shutdown() + signal_handled = True + + for sig in (signal.SIGINT, signal.SIGTERM): + loop.add_signal_handler(sig, handle_signal) + + await stop_event.wait() + spin_thread.join() + + node.destroy_node() + rclpy.shutdown() + + +def main(): + asyncio.run(async_main()) if __name__ == "__main__": diff --git a/rosbridge_server/src/rosbridge_server/websocket_handler.py b/rosbridge_server/src/rosbridge_server/websocket_handler.py index 82db43257..fc4f21eec 100755 --- a/rosbridge_server/src/rosbridge_server/websocket_handler.py +++ b/rosbridge_server/src/rosbridge_server/websocket_handler.py @@ -30,21 +30,19 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +import asyncio import sys import threading import traceback import uuid from collections import deque -from functools import partial, wraps +from functools import wraps from rosbridge_library.rosbridge_protocol import RosbridgeProtocol from rosbridge_library.util import bson -from tornado.ioloop import IOLoop from tornado.iostream import StreamClosedError from tornado.websocket import WebSocketClosedError, WebSocketHandler -_io_loop = IOLoop.instance() - def _log_exception(): """Log the most recent exception to ROS.""" @@ -124,6 +122,7 @@ class RosbridgeWebSocket(WebSocketHandler): unregister_timeout = 10.0 # seconds bson_only_mode = False node_handle = None + event_loop = None @log_exceptions def open(self): @@ -174,6 +173,8 @@ def on_close(self): self.incoming_queue.finish() def send_message(self, message, compression="none"): + cls = self.__class__ + if isinstance(message, bson.BSON): binary = True elif compression in ["cbor", "cbor-raw"]: @@ -181,7 +182,7 @@ def send_message(self, message, compression="none"): else: binary = False - _io_loop.add_callback(partial(self.prewrite_message, message, binary)) + asyncio.run_coroutine_threadsafe(self.prewrite_message(message, binary), cls.event_loop) async def prewrite_message(self, message, binary): cls = self.__class__