Apollo3D/Apollo3D_SE
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修复 5 个 bug + 3 项稳定性改进

Browse Source 
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) 降低视觉噪声
This commit is contained in:
jjh
2026-04-02 21:38:02 +08:00
parent 010567978d
commit a70557d2cc
5 changed files with 83 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
behaviors/custom/reinforcement/walk/walk.py
View File

@@ -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

10
communication/server.py
View File

@@ -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)

28
communication/world_parser.py
View File

@@ -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":

54
utils/math_ops.py
View File

@@ -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):

58
world/commons/field.py
View File

@@ -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),
}