diff --git a/rosapi/scripts/rosapi_node b/rosapi/scripts/rosapi_node index 79892ef54..10d98448a 100755 --- a/rosapi/scripts/rosapi_node +++ b/rosapi/scripts/rosapi_node @@ -309,7 +309,10 @@ def main(args=None): rclpy.init(args=args) node = Rosapi() - rclpy.spin(node) + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info("Exiting due to SIGINT") node.destroy_node() rclpy.shutdown() diff --git a/rosbridge_server/scripts/rosbridge_websocket.py b/rosbridge_server/scripts/rosbridge_websocket.py index 6e6486c93..4859369f7 100755 --- a/rosbridge_server/scripts/rosbridge_websocket.py +++ b/rosbridge_server/scripts/rosbridge_websocket.py @@ -315,7 +315,10 @@ def main(args=None): spin_callback = PeriodicCallback(lambda: rclpy.spin_once(node, timeout_sec=0.01), 1) spin_callback.start() - start_hook() + try: + start_hook() + except KeyboardInterrupt: + node.get_logger().info("Exiting due to SIGINT") node.destroy_node() rclpy.shutdown() diff --git a/rosbridge_server/test/websocket/smoke.test.py b/rosbridge_server/test/websocket/smoke.test.py index 2337213d7..3d6c4de45 100644 --- a/rosbridge_server/test/websocket/smoke.test.py +++ b/rosbridge_server/test/websocket/smoke.test.py @@ -58,6 +58,7 @@ class TestClientProtocol(WebSocketClientProtocol): def __init__(self, *args, **kwargs): self.received = [] self.connected_future = rclpy.task.Future() + self.completed_future = rclpy.task.Future() super().__init__(*args, **kwargs) def onOpen(self): @@ -72,6 +73,13 @@ def sendDict(self, msg_dict, times=1): def onMessage(self, payload, binary): print(f"WebSocket client received message: {payload}") self.received.append(payload) + if len(self.received) == NUM_MSGS: + print("Received all messages on WebSocket subscriber") + self.completed_future.set_result(None) + elif len(self.received) > NUM_MSGS: + raise AssertionError( + f"Received {len(self.received)} WebSocket messages but expected {NUM_MSGS}" + ) class TestWebsocketSmoke(unittest.TestCase): @@ -137,7 +145,19 @@ def callback(): def test_smoke(self): ros_received = [] - sub_a = self.node.create_subscription(String, A_TOPIC, ros_received.append, NUM_MSGS) + sub_completed_future = rclpy.task.Future() + + def sub_handler(msg): + ros_received.append(msg) + if len(ros_received) == NUM_MSGS: + self.node.get_logger().info("Received all messages on ROS subscriber") + sub_completed_future.set_result(None) + elif len(ros_received) > NUM_MSGS: + raise AssertionError( + f"Received {len(ros_received)} messages on ROS subscriber but expected {NUM_MSGS}" + ) + + sub_a = self.node.create_subscription(String, A_TOPIC, sub_handler, NUM_MSGS) pub_b = self.node.create_publisher(String, B_TOPIC, NUM_MSGS) async def run_test(): @@ -176,7 +196,9 @@ async def run_test(): for _ in range(NUM_MSGS): pub_b.publish(String(data=B_STRING)) - await self.sleep(TIME_LIMIT) + ws_client.completed_future.add_done_callback(lambda _: self.executor.wake()) + await sub_completed_future + await ws_client.completed_future reactor.callFromThread(reactor.stop) return ws_client.received