返回教程正文

配套源码

loopback_client.py

app/tests/loopback_client.py
ros_gateway Brick：在 App Lab 中建立可靠的 WebSocket 通道app/tests/loopback_client.py
Python206 行
  1. # SPDX-License-Identifier: MIT
  3. import argparse
  4. import json
  5. import time
  7. from websockets.sync.client import connect
  10. def now_ms():
  11. """
  12. @description : 获取当前 Unix 毫秒时间戳
  13. @param : 无参数
  14. @return : 当前 Unix 毫秒时间戳
  15. """
  16. return time.time_ns() // 1_000_000
  19. def send_message(websocket, message):
  20. """
  21. @description : 发送紧凑 JSON 文本消息
  22. @param websocket : 已建立的 WebSocket 客户端连接
  23. @param message : 待发送消息字典
  24. @return : 无返回值
  25. """
  26. websocket.send(json.dumps(message, separators=(",", ":")))
  29. def receive_json(websocket, timeout=3.0):
  30. """
  31. @description : 接收并解析一条 JSON 文本消息
  32. @param websocket : 已建立的 WebSocket 客户端连接
  33. @param timeout : 接收超时秒数
  34. @return : 解析后的消息字典
  35. """
  36. return json.loads(websocket.recv(timeout=timeout))
  39. def receive_until(websocket, predicate, timeout=4.0):
  40. """
  41. @description : 在限定时间内接收消息直到满足断言函数
  42. @param websocket : 已建立的 WebSocket 客户端连接
  43. @param predicate : 判断目标消息的函数
  44. @param timeout : 总超时秒数
  45. @return : 首条满足条件的消息
  46. """
  47. deadline = time.monotonic() + timeout
  48. while time.monotonic() < deadline:
  49. remaining = max(0.01, deadline - time.monotonic())
  50. message = receive_json(websocket, timeout=remaining)
  51. if predicate(message):
  52. return message
  53. raise TimeoutError("expected WebSocket message was not received")
  56. def run_test(url):
  57. """
  58. @description : 执行端口、握手、心跳、状态和错误路径的回环测试
  59. @param url : WebSocket 服务地址
  60. @return : 测试结果字典
  61. """
  62. results = {
  63. "url": url,
  64. "hello": False,
  65. "mode_change": False,
  66. "base_state": False,
  67. "heartbeat": False,
  68. "overspeed_rejected": False,
  69. "stale_rejected": False,
  70. "invalid_field_rejected": False,
  71. }
  73. with connect(url, open_timeout=3.0, close_timeout=2.0) as websocket:
  74. send_message(
  75. websocket,
  76. {
  77. "version": 1,
  78. "type": "hello",
  79. "role": "ros2",
  80. "node": "loopback_client",
  81. },
  82. )
  83. hello = receive_until(websocket, lambda item: item.get("type") == "hello")
  84. results["hello"] = hello.get("role") == "app"
  86. send_message(
  87. websocket,
  88. {
  89. "version": 1,
  90. "type": "mode_change",
  91. "seq": 1,
  92. "timestamp_ms": now_ms(),
  93. "mode": "ROS_TELEOP",
  94. },
  95. )
  96. acknowledgement = receive_until(
  97. websocket,
  98. lambda item: item.get("type") == "ack" and item.get("seq") == 1,
  99. )
  100. results["mode_change"] = acknowledgement.get("accepted") is True
  102. send_message(
  103. websocket,
  104. {
  105. "version": 1,
  106. "type": "heartbeat",
  107. "seq": 2,
  108. "timestamp_ms": now_ms(),
  109. },
  110. )
  111. send_message(
  112. websocket,
  113. {
  114. "version": 1,
  115. "type": "cmd_vel",
  116. "seq": 3,
  117. "timestamp_ms": now_ms(),
  118. "vx": 0.2,
  119. "vy": -0.1,
  120. "wz": 0.3,
  121. },
  122. )
  124. deadline = time.monotonic() + 2.2
  125. while time.monotonic() < deadline:
  126. message = receive_json(websocket, timeout=max(0.01, deadline - time.monotonic()))
  127. results["base_state"] |= message.get("type") == "base_state"
  128. results["heartbeat"] |= message.get("type") == "heartbeat"
  129. if results["base_state"] and results["heartbeat"]:
  130. break
  132. send_message(
  133. websocket,
  134. {
  135. "version": 1,
  136. "type": "cmd_vel",
  137. "seq": 4,
  138. "timestamp_ms": now_ms(),
  139. "vx": 99.0,
  140. "vy": 0.0,
  141. "wz": 0.0,
  142. },
  143. )
  144. overspeed = receive_until(
  145. websocket,
  146. lambda item: item.get("type") == "error" and item.get("seq") == 4,
  147. )
  148. results["overspeed_rejected"] = overspeed.get("code") == "out_of_range"
  150. send_message(
  151. websocket,
  152. {
  153. "version": 1,
  154. "type": "cmd_vel",
  155. "seq": 5,
  156. "timestamp_ms": now_ms() - 1000,
  157. "vx": 0.1,
  158. "vy": 0.0,
  159. "wz": 0.0,
  160. },
  161. )
  162. stale = receive_until(
  163. websocket,
  164. lambda item: item.get("type") == "error" and item.get("seq") == 5,
  165. )
  166. results["stale_rejected"] = stale.get("code") == "stale_command"
  168. send_message(
  169. websocket,
  170. {
  171. "version": 1,
  172. "type": "cmd_vel",
  173. "seq": 6,
  174. "timestamp_ms": now_ms(),
  175. "vx": "fast",
  176. "vy": 0.0,
  177. "wz": 0.0,
  178. },
  179. )
  180. invalid_field = receive_until(
  181. websocket,
  182. lambda item: item.get("type") == "error" and item.get("seq") == 6,
  183. )
  184. results["invalid_field_rejected"] = invalid_field.get("code") == "invalid_field"
  186. return results
  189. def main():
  190. """
  191. @description : 解析命令行参数、执行测试并设置进程退出码
  192. @param : 无参数
  193. @return : 无返回值
  194. """
  195. parser = argparse.ArgumentParser(description="Test the ROS Gateway WebSocket endpoint")
  196. parser.add_argument("--url", default="ws://127.0.0.1:8765/ros")
  197. arguments = parser.parse_args()
  198. results = run_test(arguments.url)
  199. print(json.dumps(results, ensure_ascii=False, indent=2))
  200. if not all(value for key, value in results.items() if key != "url"):
  201. raise SystemExit(1)
  204. if __name__ == "__main__":
  205. main()