修复 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:
@@ -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