repair communication and add .gitignore file

.gitignore

@@ -0,0 +1,21 @@
+.venv/
+.vscode/
+**/__pycache__/
+poetry.lock
+poetry.toml
+**/*.log
+**/*.txt
+**/build/
+**/install/
+**/log/
+*.spec
+dist/
+*.zip
+*.csv
+*.npz
+*.xml
+*.json
+*.yaml
+*.iml
+*.pyc
+.TXT

communication/server.py

@@ -50,10 +50,13 @@ class Server:
         """
         Send all committed messages
         """
-        if len(select([self.__socket], [], [], 0.0)[0]) == 0:
+        if not self.__send_buff:
+            return
+
+        if len(select([self.__socket], [], [], 0.0)[0]) != 0:
+            logger.debug("Socket is readable while sending; keeping full-duplex command send.")
+
         self.send_immediate(("".join(self.__send_buff)))
-        else:
-            logger.info("Server_Comm.py: Received a new packet while thinking!")
         self.__send_buff = []
 
     def commit(self, msg: str) -> None:
@@ -69,37 +72,37 @@ class Server:
         self.commit(msg)
         self.send()
 
-    def receive(self) -> None:
-        """
-        Receive the next message from the TCP/IP socket and updates world
-        """
+    def receive(self):
+
+        while True:
 
-        # Receive message length information
             if (
                     self.__socket.recv_into(
                         self.__rcv_buffer, nbytes=4, flags=socket.MSG_WAITALL
-            )
-            != 4
+                    ) != 4
             ):
                 raise ConnectionResetError
 
             msg_size = int.from_bytes(self.__rcv_buffer[:4], byteorder="big", signed=False)
 
-        # Ensure receive buffer is large enough to hold the message
             if msg_size > self.__rcv_buffer_size:
                 self.__rcv_buffer_size = msg_size
                 self.__rcv_buffer = bytearray(self.__rcv_buffer_size)
 
-        # Receive message with the specified length
             if (
                     self.__socket.recv_into(
                         self.__rcv_buffer, nbytes=msg_size, flags=socket.MSG_WAITALL
-            )
-            != msg_size
+                    ) != msg_size
             ):
                 raise ConnectionResetError
 
-        self.world_parser.parse(message=self.__rcv_buffer[:msg_size].decode())
+            self.world_parser.parse(
+                message=self.__rcv_buffer[:msg_size].decode()
+            )
+
+            # 如果socket没有更多数据就退出
+            if len(select([self.__socket], [], [], 0.0)[0]) == 0:
+                break
 
     def commit_beam(self, pos2d: list, rotation: float) -> None:
         assert len(pos2d) == 2

communication/world_parser.py

@@ -1,4 +1,5 @@
 import logging
+import os
 import re
 import numpy as np
 from scipy.spatial.transform import Rotation as R
@@ -7,6 +8,16 @@ from utils.math_ops import MathOps
 from world.commons.play_mode import PlayModeEnum
 
 logger = logging.getLogger()
+DEBUG_LOG_FILE = os.path.join(os.path.dirname(os.path.dirname(__file__)), "comm_debug.log")
+
+
+def _debug_log(message: str) -> None:
+    print(message)
+    try:
+        with open(DEBUG_LOG_FILE, "a", encoding="utf-8") as f:
+            f.write(message + "\n")
+    except OSError:
+        pass
 
 
 class WorldParser:
@@ -14,6 +25,36 @@ class WorldParser:
         from agent.base_agent import Base_Agent  # type hinting
 
         self.agent: Base_Agent = agent
+        self._hj_debug_prints = 0
+
+    def _normalize_motor_name(self, motor_name: str) -> str:
+        alias_map = {
+            "q_hj1": "he1",
+            "q_hj2": "he2",
+            "q_laj1": "lae1",
+            "q_laj2": "lae2",
+            "q_laj3": "lae3",
+            "q_laj4": "lae4",
+            "q_raj1": "rae1",
+            "q_raj2": "rae2",
+            "q_raj3": "rae3",
+            "q_raj4": "rae4",
+            "q_wj1": "te1",
+            "q_tj1": "te1",
+            "q_llj1": "lle1",
+            "q_llj2": "lle2",
+            "q_llj3": "lle3",
+            "q_llj4": "lle4",
+            "q_llj5": "lle5",
+            "q_llj6": "lle6",
+            "q_rlj1": "rle1",
+            "q_rlj2": "rle2",
+            "q_rlj3": "rle3",
+            "q_rlj4": "rle4",
+            "q_rlj5": "rle5",
+            "q_rlj6": "rle6",
+        }
+        return alias_map.get(motor_name, motor_name)
 
     def parse(self, message: str) -> None:
         perception_dict: dict = self.__sexpression_to_dict(message)
@@ -51,9 +92,29 @@ class WorldParser:
 
         robot = self.agent.robot
 
-        robot.motor_positions = {h["n"]: h["ax"] for h in perception_dict["HJ"]}
+        hj_states = perception_dict["HJ"] if isinstance(perception_dict["HJ"], list) else [perception_dict["HJ"]]
 
-        robot.motor_speeds = {h["n"]: h["vx"] for h in perception_dict["HJ"]}
+        if self._hj_debug_prints < 5:
+            names = [joint_state.get("n", "<missing>") for joint_state in hj_states]
+            normalized_names = [self._normalize_motor_name(name) for name in names]
+            matched_names = [name for name in names if name in robot.motor_positions]
+            matched_normalized_names = [name for name in normalized_names if name in robot.motor_positions]
+            # _debug_log(
+            #     "[ParserDebug] "
+            #     f"hj_count={len(hj_states)} "
+            #     f"sample_names={names[:8]} "
+            #     f"normalized_sample={normalized_names[:8]} "
+            #     f"matched={len(matched_names)}/{len(names)} "
+            #     f"matched_normalized={len(matched_normalized_names)}/{len(normalized_names)}"
+            # )
+            self._hj_debug_prints += 1
+
+        for joint_state in hj_states:
+            motor_name = self._normalize_motor_name(joint_state["n"])
+            if motor_name in robot.motor_positions:
+                robot.motor_positions[motor_name] = joint_state["ax"]
+            if motor_name in robot.motor_speeds:
+                robot.motor_speeds[motor_name] = joint_state["vx"]
 
         world._global_cheat_position = np.array(perception_dict["pos"]["p"])