新增延迟统计

unilabos/app/controler.py

@@ -31,6 +31,6 @@ def job_add(req: JobAddReq) -> JobData:
         action_kwargs = {"command": json.dumps(action_kwargs)}
     elif "command" in action_kwargs:
         action_kwargs = action_kwargs["command"]
-    print(f"job_add:{req.device_id} {action_name} {action_kwargs}")
-    HostNode.get_instance().send_goal(req.device_id, action_name=action_name, action_kwargs=action_kwargs, goal_uuid=req.job_id)
+    # print(f"job_add:{req.device_id} {action_name} {action_kwargs}")
+    HostNode.get_instance().send_goal(req.device_id, action_name=action_name, action_kwargs=action_kwargs, goal_uuid=req.job_id, server_info=req.server_info)
     return JobData(jobId=req.job_id)

unilabos/app/model.py

@@ -51,8 +51,9 @@ class Resp(BaseModel):
 class JobAddReq(BaseModel):
     device_id: str = Field(examples=["Gripper"], description="device id")
     data: dict = Field(examples=[{"position": 30, "torque": 5, "action": "push_to"}])
-    job_id: str = Field(examples=["sfsfsfeq"], description="goal uuid")
-    node_id: str = Field(examples=["sfsfsfeq"], description="node uuid")
+    job_id: str = Field(examples=["job_id"], description="goal uuid")
+    node_id: str = Field(examples=["node_id"], description="node uuid")
+    server_info: dict = Field(examples=[{"send_timestamp": 1717000000.0}], description="server info")
 
 
 class JobStepFinishReq(BaseModel):

unilabos/app/mq.py

@@ -12,7 +12,7 @@ import tempfile
 import os
 
 from unilabos.config.config import MQConfig
-from unilabos.app.controler import devices, job_add
+from unilabos.app.controler import job_add
 from unilabos.app.model import JobAddReq
 from unilabos.utils import logger
 from unilabos.utils.type_check import TypeEncoder
@@ -43,9 +43,10 @@ class MQTTClient:
     def _on_connect(self, client, userdata, flags, rc, properties=None):
         logger.info("[MQTT] Connected with result code " + str(rc))
         client.subscribe(f"labs/{MQConfig.lab_id}/job/start/", 0)
+        client.subscribe(f"labs/{MQConfig.lab_id}/pong/", 0)
 
     def _on_message(self, client, userdata, msg) -> None:
-        logger.info("[MQTT] on_message<<<< " + msg.topic + " " + str(msg.payload))
+        # logger.info("[MQTT] on_message<<<< " + msg.topic + " " + str(msg.payload))
         try:
             payload_str = msg.payload.decode("utf-8")
             payload_json = json.loads(payload_str)
@@ -59,6 +60,14 @@ class MQTTClient:
                 job_req = JobAddReq.model_validate(payload_json)
                 data = job_add(job_req)
                 return
+            elif msg.topic == f"labs/{MQConfig.lab_id}/pong/":
+                # 处理pong响应，通知HostNode
+                from unilabos.ros.nodes.presets.host_node import HostNode
+
+                host_instance = HostNode.get_instance(0)
+                if host_instance:
+                    host_instance.handle_pong_response(payload_json)
+                return
 
         except json.JSONDecodeError as e:
             logger.error(f"[MQTT] JSON 解析错误: {e}")
@@ -175,6 +184,28 @@ class MQTTClient:
         self.client.publish(address, json.dumps(action_info), qos=2)
         logger.debug(f"Action data published: address: {address}, {action_id}, {action_info}")
 
+    def send_ping(self, ping_id: str, timestamp: float):
+        """发送ping消息到服务端"""
+        if self.mqtt_disable:
+            return
+        address = f"labs/{MQConfig.lab_id}/ping/"
+        ping_data = {"ping_id": ping_id, "client_timestamp": timestamp, "type": "ping"}
+        self.client.publish(address, json.dumps(ping_data), qos=2)
+
+    def setup_pong_subscription(self):
+        """设置pong消息订阅"""
+        if self.mqtt_disable:
+            return
+        pong_topic = f"labs/{MQConfig.lab_id}/pong/"
+        self.client.subscribe(pong_topic, 0)
+        logger.debug(f"Subscribed to pong topic: {pong_topic}")
+
+    def handle_pong(self, pong_data: dict):
+        """处理pong响应（这个方法会在收到pong消息时被调用）"""
+        logger.debug(f"Pong received: {pong_data}")
+        # 这里会被HostNode的ping-pong处理逻辑调用
+        pass
+
 
 mqtt_client = MQTTClient()