修复 5 个 bug + 3 项稳定性改进
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:
@@ -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)
|
||||
|
||||
@@ -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":
|
||||
|
||||
Reference in New Issue
Block a user