返回教程正文

配套源码

main.py

app/python/main.py
ros_gateway Brick：在 App Lab 中建立可靠的 WebSocket 通道app/python/main.py
Python94 行
  1. # SPDX-License-Identifier: MIT
  3. import time
  5. from arduino.app_utils import App
  6. from ros_gateway import RosGateway
  9. gateway = RosGateway()
  10. last_state_publish = 0.0
  11. last_status_signature = None
  14. def handle_cmd_vel(command):
  15. """
  16. @description : 将经过协议校验的速度指令写入 App Lab 日志，本阶段不控制真实电机
  17. @param command : 包含 vx、vy、wz、seq 和 timestamp_ms 的速度指令
  18. @return : 无返回值
  19. """
  20. print(
  21. "[loopback] cmd_vel accepted: "
  22. f"seq={command['seq']} vx={command['vx']:.3f} "
  23. f"vy={command['vy']:.3f} wz={command['wz']:.3f}",
  24. flush=True,
  25. )
  28. def handle_mode_change(mode):
  29. """
  30. @description : 记录 ROS 2 请求的底盘模式切换
  31. @param mode : 新的底盘模式字符串
  32. @return : True 表示允许本次模式切换
  33. """
  34. print(f"[loopback] mode changed: {mode}", flush=True)
  35. return True
  38. def handle_safe_stop(reason):
  39. """
  40. @description : 处理通信超时或断线产生的安全停车事件，本阶段只记录日志
  41. @param reason : 触发安全停车的原因
  42. @return : 无返回值
  43. """
  44. print(f"[loopback] SAFE_STOP: {reason}; no CAN command sent", flush=True)
  47. def loop():
  48. """
  49. @description : 周期发布模拟底盘状态并保持 App 主循环非阻塞运行
  50. @param : 无参数
  51. @return : 无返回值
  52. """
  53. global last_state_publish, last_status_signature
  55. current_time = time.monotonic()
  56. if current_time - last_state_publish >= 1.0:
  57. status = gateway.get_status()
  58. gateway.publish_base_state(
  59. {
  60. "mode": status["mode"],
  61. "enabled": False,
  62. "wheel_position": [0.0, 0.0, 0.0, 0.0],
  63. "wheel_velocity": [0.0, 0.0, 0.0, 0.0],
  64. "battery_voltage": 0.0,
  65. "estop": False,
  66. "fault_code": 0,
  67. }
  68. )
  69. status_signature = (
  70. status["connected"],
  71. status["mode"],
  72. status["client_node"],
  73. status["server_error"],
  74. )
  75. if status_signature != last_status_signature:
  76. print(
  77. "[loopback] status changed: "
  78. f"connected={status['connected']} mode={status['mode']} "
  79. f"client={status['client_node'] or '-'} "
  80. f"server_error={status['server_error'] or '-'}",
  81. flush=True,
  82. )
  83. last_status_signature = status_signature
  84. last_state_publish = current_time
  86. time.sleep(0.05)
  89. gateway.on_cmd_vel(handle_cmd_vel)
  90. gateway.on_mode_change(handle_mode_change)
  91. gateway.on_stop(handle_safe_stop)
  93. App.run(user_loop=loop)