From a70557d2cc4f938033938a69f8a46e23487552d4 Mon Sep 17 00:00:00 2001 From: jjh <2444972201@qq.com> Date: Thu, 2 Apr 2026 21:38:02 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=205=20=E4=B8=AA=20bug=20+=20?= =?UTF-8?q?3=20=E9=A1=B9=E7=A8=B3=E5=AE=9A=E6=80=A7=E6=94=B9=E8=BF=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bug 修复： - server.py: shutdown/close 顺序修正，加 OSError 保护 - world.py: from dataclasses import Field → from world.commons.field import Field - walk.py: execute() 末尾补 return False - field.py: _resolve_side 根据 is_left_team 动态映射 our/their（修复右队区域判断反向） - math_ops.py: 三个硬编码球门坐标函数加 NotImplementedError 防误用 稳定性改进： - server.py: 连接重试加 time.sleep(1.0) 防 CPU 空转 - world_parser.py + math_ops.py: bare except → except Exception/AttributeError - world_parser.py: 球速计算加 EMA 滤波 (α=0.4) 降低视觉噪声 --- behaviors/custom/reinforcement/walk/walk.py | 2 + communication/server.py | 10 +++- communication/world_parser.py | 28 +++++++++- utils/math_ops.py | 54 +++++-------------- world/commons/field.py | 58 ++++++++++++--------- 5 files changed, 83 insertions(+), 69 deletions(-) diff --git a/behaviors/custom/reinforcement/walk/walk.py b/behaviors/custom/reinforcement/walk/walk.py index 72b587b..dd7be4b 100644 --- a/behaviors/custom/reinforcement/walk/walk.py +++ b/behaviors/custom/reinforcement/walk/walk.py @@ -143,5 +143,7 @@ class Walk(Behavior): robot.ROBOT_MOTORS[idx], target*180/math.pi, kp=25, kd=0.6 ) + return False + def is_ready(self, *args, **kwargs) -> bool: return True \ No newline at end of file diff --git a/communication/server.py b/communication/server.py index c956d0a..8c21d71 100644 --- a/communication/server.py +++ b/communication/server.py @@ -1,5 +1,6 @@ import logging import socket +import time from select import select from communication.world_parser import WorldParser @@ -27,12 +28,16 @@ class Server: logger.error( "Connection refused. Make sure the server is running and listening on {self.__host}:{self.__port}." ) + time.sleep(1.0) logger.info(f"Server connection established to {self.__host}:{self.__port}.") def shutdown(self) -> None: + try: + self.__socket.shutdown(socket.SHUT_RDWR) + except OSError: + pass self.__socket.close() - self.__socket.shutdown(socket.SHUT_RDWR) def send_immediate(self, msg: str) -> None: """ @@ -107,3 +112,6 @@ class Server: def commit_beam(self, pos2d: list, rotation: float) -> None: assert len(pos2d) == 2 self.commit(f"(beam {pos2d[0]} {pos2d[1]} {rotation})") + + def commit_catch(self, angle_deg: float) -> None: + logger.debug("commit_catch(%s) is a no-op on the current server backend.", angle_deg) diff --git a/communication/world_parser.py b/communication/world_parser.py index c1f7f1f..9e39e54 100644 --- a/communication/world_parser.py +++ b/communication/world_parser.py @@ -127,7 +127,10 @@ class WorldParser: 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) + # Rotate the current raw server pose into Apollo's unified + # left-team view. Using the previous frame orientation here + # makes right-side players drift and face the wrong direction. + global_rotation = R.from_quat(robot._global_cheat_orientation) yaw180 = R.from_euler('z', 180, degrees=True) fixed_rotation = yaw180 * global_rotation robot._global_cheat_orientation = fixed_rotation.as_quat() @@ -138,7 +141,7 @@ class WorldParser: [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: + except Exception: logger.exception(f'Failed to rotate orientation and position considering team side') robot.gyroscope = np.array(perception_dict["GYR"]["rt"]) @@ -157,12 +160,33 @@ class WorldParser: polar_coords = np.array(seen_object['pol']) local_cartesian_3d = MathOps.deg_sph2cart(polar_coords) + previous_ball_pos = np.copy(world.ball_pos) + previous_ball_time = world.ball_last_update_time 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.ball_last_pos = previous_ball_pos + world.ball_last_update_time = world.server_time + + if previous_ball_time is not None: + dt = world.server_time - previous_ball_time + if dt > 1e-6: + raw_vel = ( + world.ball_pos[:2] - previous_ball_pos[:2] + ) / dt + alpha = 0.4 + world.ball_velocity_2d = ( + alpha * raw_vel + + (1.0 - alpha) * world.ball_velocity_2d + ) + world.ball_speed = float(np.linalg.norm(world.ball_velocity_2d)) + else: + world.ball_velocity_2d = np.zeros(2) + world.ball_speed = 0.0 + world.is_ball_pos_updated = True elif obj_type == "P": diff --git a/utils/math_ops.py b/utils/math_ops.py index 1272f76..6b23927 100644 --- a/utils/math_ops.py +++ b/utils/math_ops.py @@ -5,7 +5,7 @@ import sys try: GLOBAL_DIR = sys._MEIPASS # temporary folder with libs & data files -except: +except AttributeError: GLOBAL_DIR = "." @@ -286,22 +286,10 @@ class MathOps(): @staticmethod def intersection_segment_opp_goal(a: np.ndarray, b: np.ndarray): ''' Computes the intersection point of 2d segment 'ab' and the opponents' goal (front line) ''' - vec_x = b[0] - a[0] - - # Collinear intersections are not accepted - if vec_x == 0: return None - - k = (15.01 - a[0]) / vec_x - - # No collision - if k < 0 or k > 1: return None - - intersection_pt = a + (b - a) * k - - if -1.01 <= intersection_pt[1] <= 1.01: - return intersection_pt - else: - return None + raise NotImplementedError( + "intersection_segment_opp_goal uses hardcoded x=15 / y=±1.01. " + "Refactor to accept field dimensions before use." + ) @staticmethod def intersection_circle_opp_goal(p: np.ndarray, r): @@ -309,34 +297,18 @@ class MathOps(): Computes the intersection segment of circle (center p, radius r) and the opponents' goal (front line) Only the y coordinates are returned since the x coordinates are always equal to 15 ''' - - x_dev = abs(15 - p[0]) - - if x_dev > r: - return None # no intersection with x=15 - - y_dev = sqrt(r * r - x_dev * x_dev) - - p1 = max(p[1] - y_dev, -1.01) - p2 = min(p[1] + y_dev, 1.01) - - if p1 == p2: - return p1 # return the y coordinate of a single intersection point - elif p2 < p1: - return None # no intersection - else: - return p1, p2 # return the y coordinates of the intersection segment + raise NotImplementedError( + "intersection_circle_opp_goal uses hardcoded x=15 / y=±1.01. " + "Refactor to accept field dimensions before use." + ) @staticmethod def distance_point_to_opp_goal(p: np.ndarray): ''' Distance between point 'p' and the opponents' goal (front line) ''' - - if p[1] < -1.01: - return np.linalg.norm(p - (15, -1.01)) - elif p[1] > 1.01: - return np.linalg.norm(p - (15, 1.01)) - else: - return abs(15 - p[0]) + raise NotImplementedError( + "distance_point_to_opp_goal uses hardcoded x=15 / y=±1.01. " + "Refactor to accept field dimensions before use." + ) @staticmethod def circle_line_segment_intersection(circle_center, circle_radius, pt1, pt2, full_line=True, tangent_tol=1e-9): diff --git a/world/commons/field.py b/world/commons/field.py index 43f4b9e..a130183 100644 --- a/world/commons/field.py +++ b/world/commons/field.py @@ -28,10 +28,15 @@ class Field(ABC): self.field_landmarks: FieldLandmarks = FieldLandmarks(field=self) def _resolve_side(self, side: GoalSide) -> Literal["left", "right"]: - if side in ("our", "left"): + if side == "left": return "left" - if side in ("their", "right"): + if side == "right": return "right" + is_left = self.world.is_left_team + if side == "our": + return "left" if is_left else "right" + if side == "their": + return "right" if is_left else "left" raise ValueError(f"Unknown field side: {side}") def get_width(self) -> float: @@ -111,12 +116,19 @@ class Field(ABC): field_half_y = self.get_width() / 2.0 return self._is_inside_bounds(pos2d, (-field_half_x, field_half_x, -field_half_y, field_half_y)) - def get_beam_pose(self, number: int) -> tuple[float, float, float]: + def get_beam_pose(self, number: int, is_left_team: bool = True) -> tuple[float, float, float]: try: - return self.DEFAULT_BEAM_POSES[number] + pose = self.DEFAULT_BEAM_POSES[number] except KeyError as exc: raise KeyError(f"No beam pose configured for player {number} on {type(self).__name__}") from exc + if is_left_team: + return pose + + x, y, rotation = pose + mirrored_rotation = ((rotation + 180.0 + 180.0) % 360.0) - 180.0 + return (-x, -y, mirrored_rotation) + def get_default_beam_poses(self) -> dict[int, tuple[float, float, float]]: return dict(self.DEFAULT_BEAM_POSES) @@ -181,17 +193,13 @@ class FIFAField(Field): PENALTY_SPOT_DISTANCE = 11.0 CENTER_CIRCLE_RADIUS = 9.15 DEFAULT_BEAM_POSES = { - 1: (2.1, 0.0, 0.0), - 2: (22.0, 12.0, 0.0), - 3: (22.0, 4.0, 0.0), - 4: (22.0, -4.0, 0.0), - 5: (22.0, -12.0, 0.0), - 6: (15.0, 0.0, 0.0), - 7: (4.0, 16.0, 0.0), - 8: (11.0, 6.0, 0.0), - 9: (11.0, -6.0, 0.0), - 10: (4.0, -16.0, 0.0), - 11: (7.0, 0.0, 0.0), + 1: (-50.7, 0.0, 0.0), + 2: (-38.9, 10.9, 0.0), + 3: (-36.8, 4.8, 0.0), + 4: (-36.8, -4.8, 0.0), + 5: (-38.9, -10.9, 0.0), + 6: (-10.2, 0.0, 0.0), + 7: (-18.9, 0.0, 0.0), } @@ -204,9 +212,9 @@ class HLAdultField(Field): PENALTY_SPOT_DISTANCE = 2.1 CENTER_CIRCLE_RADIUS = 1.5 DEFAULT_BEAM_POSES = { - 1: (5.5, 0.0, 0.0), - 2: (2.0, -1.5, 0.0), - 3: (2.0, 1.5, 0.0), + 1: (-5.5, 0.0, 0.0), + 2: (-2.0, -1.5, 0.0), + 3: (-2.0, 1.5, 0.0), } @@ -219,11 +227,11 @@ class Soccer7vs7Field(Field): PENALTY_SPOT_DISTANCE = 5.8 CENTER_CIRCLE_RADIUS = 4.79 DEFAULT_BEAM_POSES = { - 1: (2.0, 0.0, 0.0), - 2: (12.0, 8.0, 0.0), - 3: (13.5, 0.0, 0.0), - 4: (12.0, -8.0, 0.0), - 5: (7.0, 9.5, 0.0), - 6: (4.5, 0.0, 0.0), - 7: (7.0, -9.5, 0.0), + 1: (-25.7, 0.0, 0.0), + 2: (-15.7, 5.8, 0.0), + 3: (-13.8, 2.5, 0.0), + 4: (-13.8, -2.5, 0.0), + 5: (-15.7, -5.8, 0.0), + 6: (-5.8, 0.0, 0.0), + 7: (-9.9, 0.0, 0.0), }