Init

communication/server.py

@@ -0,0 +1,106 @@
+import logging
+import socket
+from select import select
+from communication.world_parser import WorldParser
+
+logger = logging.getLogger(__file__)
+
+
+class Server:
+    def __init__(self, host: str, port: int, world_parser: WorldParser):
+        self.world_parser: WorldParser = world_parser
+        self.__host: str = host
+        self.__port: str = port
+        self.__socket: socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.__socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
+        self.__send_buff = []
+        self.__rcv_buffer_size = 1024
+        self.__rcv_buffer = bytearray(self.__rcv_buffer_size)
+
+    def connect(self) -> None:
+        logger.info("Connecting to server at %s:%d...", self.__host, self.__port)
+        while True:
+            try:
+                self.__socket.connect((self.__host, self.__port))
+                break
+            except ConnectionRefusedError:
+                logger.error(
+                    "Connection refused. Make sure the server is running and listening on {self.__host}:{self.__port}."
+                )
+
+        logger.info(f"Server connection established to {self.__host}:{self.__port}.")
+
+    def shutdown(self) -> None:
+        self.__socket.close()
+        self.__socket.shutdown(socket.SHUT_RDWR)
+
+    def send_immediate(self, msg: str) -> None:
+        """
+        Sends only the desired message, without the buffer
+        """
+        try:
+            self.__socket.send(
+                (len(msg)).to_bytes(4, byteorder="big") + msg.encode()
+            )  # Add message length in the first 4 bytes
+        except BrokenPipeError:
+            print("\nError: socket was closed by rcssserver3d!")
+            exit()
+
+    def send(self) -> None:
+        """
+        Send all committed messages
+        """
+        if len(select([self.__socket], [], [], 0.0)[0]) == 0:
+            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:
+        """
+        Appends message to buffer
+        """
+        self.__send_buff.append(msg)
+
+    def commit_and_send(self, msg: str = "") -> None:
+        """
+        Appends a message to buffer and then sends the buffer
+        """
+        self.commit(msg)
+        self.send()
+
+    def receive(self) -> None:
+        """
+        Receive the next message from the TCP/IP socket and updates world
+        """
+
+        # Receive message length information
+        if (
+            self.__socket.recv_into(
+                self.__rcv_buffer, nbytes=4, flags=socket.MSG_WAITALL
+            )
+            != 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
+        ):
+            raise ConnectionResetError
+
+        self.world_parser.parse(message=self.__rcv_buffer[:msg_size].decode())
+
+    def commit_beam(self, pos2d: list, rotation: float) -> None:
+        assert len(pos2d) == 2
+        self.commit(f"(beam {pos2d[0]} {pos2d[1]} {rotation})")

communication/world_parser.py

@@ -0,0 +1,255 @@
+import logging
+import re
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+from utils.math_ops import MathOps
+from world.commons.play_mode import PlayModeEnum
+
+logger = logging.getLogger()
+
+
+class WorldParser:
+    def __init__(self, agent):
+        from agent.base_agent import Base_Agent  # type hinting
+
+        self.agent: Base_Agent = agent
+
+    def parse(self, message: str) -> None:
+        perception_dict: dict = self.__sexpression_to_dict(message)
+
+        world = self.agent.world
+
+        # Game parse
+
+        if world.is_left_team is None:
+            world.is_left_team = (
+                True
+                if perception_dict["GS"]["tl"] == world.team_name
+                else False if perception_dict["GS"]["tr"] == world.team_name else None
+            )
+
+        world.playmode = PlayModeEnum.get_playmode_from_string(
+            playmode=perception_dict["GS"]["pm"], is_left_team=world.is_left_team
+        )
+
+        world.game_time = perception_dict["GS"]["t"]
+        world.score_left = perception_dict["GS"]["sl"]
+        world.score_right = perception_dict["GS"]["sr"]
+
+        left_team_name: str = perception_dict["GS"].get("tl", None)
+        right_team_name: str = perception_dict["GS"].get("tr", None)
+        if left_team_name and right_team_name:
+            world.their_team_name = (
+                right_team_name if world.is_left_team else left_team_name
+            )
+
+        world.last_server_time = world.server_time
+        world.server_time = perception_dict["time"]["now"]
+
+        # Robot parse
+
+        robot = self.agent.robot
+
+        robot.motor_positions = {h["n"]: h["ax"] for h in perception_dict["HJ"]}
+
+        robot.motor_speeds = {h["n"]: h["vx"] for h in perception_dict["HJ"]}
+
+        world._global_cheat_position = np.array(perception_dict["pos"]["p"])
+
+        # changes quaternion from (w, x, y, z) to (x, y, z, w)
+        robot._global_cheat_orientation = np.array(perception_dict["quat"]["q"])
+        robot._global_cheat_orientation = robot._global_cheat_orientation[[1, 2, 3, 0]]
+
+        # flips 180 deg considering team side
+        try:
+            if not world.is_left_team:
+                world._global_cheat_position[:2] = -world._global_cheat_position[:2]
+
+                global_rotation = R.from_quat(robot.global_orientation_quat)
+                yaw180 = R.from_euler('z', 180, degrees=True)
+                fixed_rotation = yaw180 * global_rotation
+                robot._global_cheat_orientation = fixed_rotation.as_quat()
+
+            # updates global orientation
+            euler_angles_deg = R.from_quat(robot._global_cheat_orientation).as_euler('xyz', degrees=True)
+            robot.global_orientation_euler = np.array(
+                [MathOps.normalize_deg(axis_angle) for axis_angle in euler_angles_deg])
+            robot.global_orientation_quat = robot._global_cheat_orientation
+            world.global_position = world._global_cheat_position
+        except:
+            logger.exception(f'Failed to rotate orientation and position considering team side')
+
+        robot.gyroscope = np.array(perception_dict["GYR"]["rt"])
+
+        robot.accelerometer = np.array(perception_dict["ACC"]["a"])
+
+        world.is_ball_pos_updated = False
+
+        # Vision parse
+        if 'See' in perception_dict:
+
+            for seen_object in perception_dict['See']:
+                obj_type = seen_object['type']
+
+                if obj_type == 'B':  # Ball
+
+                    polar_coords = np.array(seen_object['pol'])
+                    local_cartesian_3d = MathOps.deg_sph2cart(polar_coords)
+
+                    world.ball_pos = MathOps.rel_to_global_3d(
+                        local_pos_3d=local_cartesian_3d,
+                        global_pos_3d=world.global_position,
+                        global_orientation_quat=robot.global_orientation_quat
+                    )
+                    world.is_ball_pos_updated = True
+
+                elif obj_type == "P":
+
+                    team = seen_object.get('team')
+                    player_id = seen_object.get('id')
+
+                    if team and player_id is not None:
+                        if (team == world.team_name):
+                            player = world.our_team_players[player_id - 1]
+                        else:
+                            player = world.their_team_players[player_id - 1]
+
+                        objects = [seen_object.get('head'), seen_object.get('l_foot'), seen_object.get('r_foot')]
+
+                        seen_objects = [object for object in objects if object]
+
+                        if seen_objects:
+                            local_cartesian_seen_objects = [MathOps.deg_sph2cart(object) for object in seen_objects]
+
+                            approximated_centroid = np.mean(local_cartesian_seen_objects, axis=0)
+
+                            player.position = MathOps.rel_to_global_3d(
+                                local_pos_3d=approximated_centroid,
+                                global_pos_3d=world.global_position,
+                                global_orientation_quat=robot._global_cheat_orientation
+                            )
+                            player.last_seen_time = world.server_time
+
+                elif obj_type:
+
+                    polar_coords = np.array(seen_object['pol'])
+                    world.field.field_landmarks.update_from_perception(
+                        landmark_id=obj_type,
+                        landmark_pos=polar_coords
+                    )
+
+    def __sexpression_to_dict(self, sexpression: str) -> dict:
+        """
+        Parses a sensor data string of nested parenthesis groups into a structured dictionary.
+        Repeated top-level tags are aggregated into lists.
+        """
+
+        def split_top_level(s: str):
+            """Return a list of substrings that are top-level parenthesized groups."""
+            groups = []
+            depth = 0
+            start = None
+            for i, ch in enumerate(s):
+                if ch == '(':
+                    if depth == 0:
+                        start = i
+                    depth += 1
+                elif ch == ')':
+                    depth -= 1
+                    if depth == 0 and start is not None:
+                        groups.append(s[start:i + 1])
+                        start = None
+            return groups
+
+        result = {}
+
+        top_groups = split_top_level(sexpression)
+
+        for grp in top_groups:
+            m = re.match(r'^\((\w+)\s*(.*)\)$', grp, re.DOTALL)
+            if not m:
+                continue
+            tag = m.group(1)
+            inner = m.group(2).strip()
+
+            if tag == "See":
+                see_items = []
+                subs = split_top_level(inner)
+
+                for sub in subs:
+                    sm = re.match(r'^\((\w+)\s*(.*)\)$', sub, re.DOTALL)
+                    if not sm:
+                        continue
+                    obj_type = sm.group(1)
+                    inner2 = sm.group(2)
+
+                    if obj_type == "P":  # Player
+                        player_data = {"type": "P"}
+                        team_m = re.search(r'\(team\s+([^)]+)\)', inner2)
+                        if team_m:
+                            player_data["team"] = team_m.group(1)
+                        id_m = re.search(r'\(id\s+([^)]+)\)', inner2)
+                        if id_m:
+                            try:
+                                player_data["id"] = int(id_m.group(1))
+                            except ValueError:
+                                player_data["id"] = id_m.group(1)
+
+                        parts = re.findall(r'\((\w+)\s*\(pol\s+([-0-9.\s]+)\)\)', inner2)
+                        for part_name, pol_str in parts:
+                            pol_vals = [float(x) for x in pol_str.strip().split()]
+                            player_data[part_name] = pol_vals
+
+                        see_items.append(player_data)
+                        continue
+
+                    # Generic
+                    pol_m = re.search(r'\(pol\s+([-0-9.\s]+)\)', inner2)
+                    vals = [float(x) for x in pol_m.group(1).strip().split()] if pol_m else []
+                    see_items.append({"type": obj_type, "pol": vals})
+
+                result.setdefault("See", []).extend(see_items)
+                continue
+
+            # Generic parse for other tags (time, GS, quat, pos, HJ, ...)
+            group = {}
+            children = split_top_level(inner)
+            if children:  # (key val1 val2)
+                for child in children:
+                    im = re.match(r'^\(\s*(\w+)\s+([^)]+)\)$', child.strip(), re.DOTALL)
+                    if not im:
+                        continue
+                    key = im.group(1)
+                    vals = im.group(2).strip().split()
+                    parsed = []
+                    for t in vals:
+                        try:
+                            parsed.append(float(t))
+                        except ValueError:
+                            parsed.append(t)
+                    group[key] = parsed[0] if len(parsed) == 1 else parsed
+            else:
+                # search pairs (key vals...)
+                items = re.findall(r"\(\s*(\w+)((?:\s+[^()]+)+)\)", inner)
+                for key, vals in items:
+                    tokens = vals.strip().split()
+                    parsed_vals = []
+                    for t in tokens:
+                        try:
+                            parsed_vals.append(float(t))
+                        except ValueError:
+                            parsed_vals.append(t)
+                    # Single value vs. list
+                    group[key] = parsed_vals[0] if len(parsed_vals) == 1 else parsed_vals
+
+            # Merge into result, handling repeated tags as lists
+            if tag in result:
+                if isinstance(result[tag], list):
+                    result[tag].append(group)
+                else:
+                    result[tag] = [result[tag], group]
+            else:
+                result[tag] = group
+
+        return result