2 Commits

Author SHA1 Message Date
徐学颢
4decbe1938 Revert "add kick behavior with keyframe, but still need to be improved"
This reverts commit 047d3d60c4.
2026-04-18 18:42:53 +08:00
徐学颢
047d3d60c4 add kick behavior with keyframe, but still need to be improved 2026-04-18 18:32:32 +08:00
9 changed files with 115 additions and 361 deletions

View File

@@ -1,7 +1,10 @@
from dataclasses import Field
import logging import logging
from typing import Mapping
import numpy as np import numpy as np
from utils.math_ops import MathOps from utils.math_ops import MathOps
from world.commons.field import FIFAField, HLAdultField, Soccer7vs7Field
from world.commons.play_mode import PlayModeEnum, PlayModeGroupEnum from world.commons.play_mode import PlayModeEnum, PlayModeGroupEnum
@@ -16,6 +19,36 @@ class Agent:
based on the current state of the world and game conditions. based on the current state of the world and game conditions.
""" """
BEAM_POSES: Mapping[type[Field], Mapping[int, tuple[float, float, float]]] ={
FIFAField: {
1: (2.1, 0, 0),
2: (22.0, 12.0, 0),
3: (22.0, 4.0, 0),
4: (22.0, -4.0, 0),
5: (22.0, -12.0, 0),
6: (15.0, 0.0, 0),
7: (4.0, 16.0, 0),
8: (11.0, 6.0, 0),
9: (11.0, -6.0, 0),
10: (4.0, -16.0, 0),
11: (7.0, 0.0, 0),
},
HLAdultField: {
1: (7.0, 0.0, 0),
2: (2.0, -1.5, 0),
3: (2.0, 1.5, 0),
},
Soccer7vs7Field: {
1: (2.1, 0, 0),
2: (22.0, 12.0, 0),
3: (22.0, 4.0, 0),
4: (22.0, -4.0, 0),
5: (22.0, -12.0, 0),
6: (15.0, 0.0, 0),
7: (4.0, 16.0, 0)
}
}
def __init__(self, agent): def __init__(self, agent):
""" """
Creates a new DecisionMaker linked to the given agent. Creates a new DecisionMaker linked to the given agent.
@@ -43,10 +76,9 @@ class Agent:
PlayModeGroupEnum.ACTIVE_BEAM, PlayModeGroupEnum.ACTIVE_BEAM,
PlayModeGroupEnum.PASSIVE_BEAM, PlayModeGroupEnum.PASSIVE_BEAM,
): ):
beam_pose = self.agent.world.field.get_beam_pose(self.agent.world.number)
self.agent.server.commit_beam( self.agent.server.commit_beam(
pos2d=beam_pose[:2], pos2d=self.BEAM_POSES[type(self.agent.world.field)][self.agent.world.number][:2],
rotation=beam_pose[2], rotation=self.BEAM_POSES[type(self.agent.world.field)][self.agent.world.number][2],
) )
if self.is_getting_up or self.agent.skills_manager.is_ready(skill_name="GetUp"): if self.is_getting_up or self.agent.skills_manager.is_ready(skill_name="GetUp"):
@@ -63,12 +95,7 @@ class Agent:
def carry_ball(self): def carry_ball(self):
""" """
Minimal catch-ball behavior. Basic example of a behavior: moves the robot toward the goal while handling the ball.
All players share the same logic:
1. approach a point behind the ball
2. reposition with a lateral offset if they are close but not yet behind it
3. push the ball forward once they are aligned
""" """
their_goal_pos = self.agent.world.field.get_their_goal_position()[:2] their_goal_pos = self.agent.world.field.get_their_goal_position()[:2]
ball_pos = self.agent.world.ball_pos[:2] ball_pos = self.agent.world.ball_pos[:2]
@@ -76,23 +103,12 @@ class Agent:
ball_to_goal = their_goal_pos - ball_pos ball_to_goal = their_goal_pos - ball_pos
bg_norm = np.linalg.norm(ball_to_goal) bg_norm = np.linalg.norm(ball_to_goal)
if bg_norm <= 1e-6: if bg_norm == 0:
ball_to_goal_dir = np.array([1.0, 0.0]) return
else: ball_to_goal_dir = ball_to_goal / bg_norm
ball_to_goal_dir = ball_to_goal / bg_norm
lateral_dir = np.array([-ball_to_goal_dir[1], ball_to_goal_dir[0]]) dist_from_ball_to_start_carrying = 0.30
carry_ball_pos = ball_pos - ball_to_goal_dir * dist_from_ball_to_start_carrying
back_offset = 0.40
side_offset = 0.35
push_distance = 0.80
approach_distance = 0.90
push_start_distance = 0.55
behind_margin = 0.08
angle_tolerance = np.deg2rad(20.0)
behind_point = ball_pos - ball_to_goal_dir * back_offset
push_target = ball_pos + ball_to_goal_dir * push_distance
my_to_ball = ball_pos - my_pos my_to_ball = ball_pos - my_pos
my_to_ball_norm = np.linalg.norm(my_to_ball) my_to_ball_norm = np.linalg.norm(my_to_ball)
@@ -104,36 +120,25 @@ class Agent:
cosang = np.dot(my_to_ball_dir, ball_to_goal_dir) cosang = np.dot(my_to_ball_dir, ball_to_goal_dir)
cosang = np.clip(cosang, -1.0, 1.0) cosang = np.clip(cosang, -1.0, 1.0)
angle_diff = np.arccos(cosang) angle_diff = np.arccos(cosang)
aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= angle_tolerance)
behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < -behind_margin ANGLE_TOL = np.deg2rad(7.5)
aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= ANGLE_TOL)
behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < 0
desired_orientation = MathOps.vector_angle(ball_to_goal) desired_orientation = MathOps.vector_angle(ball_to_goal)
lateral_sign = np.sign(np.cross(ball_to_goal_dir, my_to_ball_dir)) if not aligned or not behind_ball:
if lateral_sign == 0: self.agent.skills_manager.execute(
lateral_sign = 1.0 if (my_pos[1] - ball_pos[1]) >= 0 else -1.0 "Walk",
target_2d=carry_ball_pos,
reposition_point = behind_point + lateral_dir * lateral_sign * side_offset is_target_absolute=True,
orientation=None if np.linalg.norm(my_pos - carry_ball_pos) > 2 else desired_orientation
if my_to_ball_norm > approach_distance: )
target_2d = behind_point
orientation = None
elif not behind_ball:
target_2d = reposition_point
orientation = None if np.linalg.norm(my_pos - reposition_point) > 0.8 else desired_orientation
elif not aligned and my_to_ball_norm > push_start_distance:
target_2d = behind_point
orientation = desired_orientation
else: else:
target_2d = push_target self.agent.skills_manager.execute(
orientation = desired_orientation "Walk",
target_2d=their_goal_pos,
is_target_absolute=True,
orientation=desired_orientation
)
if np.linalg.norm(target_2d - my_pos) <= 1e-4:
target_2d = my_pos + ball_to_goal_dir * 0.30
self.agent.skills_manager.execute(
"Walk",
target_2d=target_2d,
is_target_absolute=True,
orientation=orientation,
)

View File

@@ -5,7 +5,6 @@ from world.robot import T1, Robot
from behaviors.behavior_manager import BehaviorManager from behaviors.behavior_manager import BehaviorManager
from world.world import World from world.world import World
from communication.server import Server from communication.server import Server
from communication.monitor_client import MonitorClient
from communication.world_parser import WorldParser from communication.world_parser import WorldParser
logger = logging.getLogger(__file__) logger = logging.getLogger(__file__)
@@ -36,7 +35,6 @@ class Base_Agent:
self.server: Server = Server( self.server: Server = Server(
host=host, port=port, world_parser=self.world_parser host=host, port=port, world_parser=self.world_parser
) )
self.monitor: MonitorClient = MonitorClient(host=host, port=port + 1)
self.robot: Robot = T1(agent=self) self.robot: Robot = T1(agent=self)
self.skills_manager: BehaviorManager = BehaviorManager(agent=self) self.skills_manager: BehaviorManager = BehaviorManager(agent=self)
self.decision_maker: Agent = Agent(agent=self) self.decision_maker: Agent = Agent(agent=self)
@@ -55,7 +53,6 @@ class Base_Agent:
- Sends the next set of commands to the server. - Sends the next set of commands to the server.
""" """
self.server.connect() self.server.connect()
self.monitor.connect()
self.server.send_immediate( self.server.send_immediate(
f"(init {self.robot.name} {self.world.team_name} {self.world.number})" f"(init {self.robot.name} {self.world.team_name} {self.world.number})"
@@ -81,5 +78,4 @@ class Base_Agent:
Logs a shutdown message and closes the server connection. Logs a shutdown message and closes the server connection.
""" """
logger.info("Shutting down.") logger.info("Shutting down.")
self.monitor.close() self.server.shutdown()
self.server.shutdown()

View File

@@ -1,63 +0,0 @@
import logging
import socket
logger = logging.getLogger(__name__)
class MonitorClient:
"""
TCP client for the RCSSServerMJ monitor port.
Sends monitor commands via the length-prefixed S-expression protocol.
"""
def __init__(self, host: str = "localhost", port: int = 60001):
self._host = host
self._port = port
self._socket: socket.socket | None = None
def connect(self) -> None:
self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
self._socket.connect((self._host, self._port))
logger.info("Monitor connection established to %s:%d.", self._host, self._port)
def close(self) -> None:
if self._socket is not None:
self._socket.close()
self._socket = None
def send(self, msg: str) -> None:
data = msg.encode()
self._socket.send(len(data).to_bytes(4, byteorder="big") + data)
def place_ball(
self,
pos: tuple[float, float, float],
vel: tuple[float, float, float] | None = None,
) -> None:
msg = f"(ball (pos {pos[0]} {pos[1]} {pos[2]})"
if vel is not None:
msg += f" (vel {vel[0]} {vel[1]} {vel[2]})"
msg += ")"
self.send(msg)
def drop_ball(self) -> None:
self.send("(dropBall)")
def kick_off(self, side: str = "Left") -> None:
self.send(f"(kickOff {side})")
def set_play_mode(self, mode: str) -> None:
self.send(f"(playMode {mode})")
def place_player(
self,
unum: int,
team_name: str,
pos: tuple[float, float, float],
) -> None:
self.send(
f"(agent (unum {unum}) (team {team_name}) (pos {pos[0]} {pos[1]} {pos[2]}))"
)

View File

@@ -41,14 +41,7 @@ CLI parameter (a usage help is also available):
- `--port <port>` to specify the agent port (default: 60000) - `--port <port>` to specify the agent port (default: 60000)
- `-n <number>` Player number (111) (default: 1) - `-n <number>` Player number (111) (default: 1)
- `-t <team_name>` Team name (default: 'Default') - `-t <team_name>` Team name (default: 'Default')
- `-f <field>` Field profile (default: `fifa`)
### Field profiles
There are two supported ways to run Apollo3D:
- Official rules test: use the server with `--rules ssim`, and run agents with `-f fifa`. This matches the current `rcssservermj` default field for the SSIM rule book.
- Apollo custom 7v7: run agents with `-f sim3d_7vs7`. This profile is kept for Apollo's custom small-field setup and should not be treated as the official SSIM geometry baseline.
### Run a team ### Run a team
You can also use a shell script to start the entire team, optionally specifying host and port: You can also use a shell script to start the entire team, optionally specifying host and port:
@@ -62,8 +55,6 @@ Using **Poetry**:
poetry run ./start_7v7.sh [host] [port] poetry run ./start_7v7.sh [host] [port]
``` ```
`start_7v7.sh` now launches agents explicitly with `-f sim3d_7vs7`.
CLI parameter: CLI parameter:
- `[host]` Server IP address (default: 'localhost') - `[host]` Server IP address (default: 'localhost')
@@ -94,4 +85,4 @@ This project was developed and contributed by:
- **Pedro Rabelo** - **Pedro Rabelo**
- **Melissa Damasceno** - **Melissa Damasceno**
Contributions, bug reports, and feature requests are welcome via pull requests. Contributions, bug reports, and feature requests are welcome via pull requests.

View File

@@ -16,7 +16,7 @@ parser.add_argument("-t", "--team", type=str, default="Default", help="Team name
parser.add_argument("-n", "--number", type=int, default=1, help="Player number") parser.add_argument("-n", "--number", type=int, default=1, help="Player number")
parser.add_argument("--host", type=str, default="127.0.0.1", help="Server host") parser.add_argument("--host", type=str, default="127.0.0.1", help="Server host")
parser.add_argument("--port", type=int, default=60000, help="Server port") parser.add_argument("--port", type=int, default=60000, help="Server port")
parser.add_argument("-f", "--field", type=str, default='fifa', help="Field to be played") parser.add_argument("-f", "--field", type=str, default='sim3d_7vs7', help="Field to be played")
args = parser.parse_args() args = parser.parse_args()

View File

@@ -5,5 +5,5 @@ host=${1:-localhost}
port=${2:-60000} port=${2:-60000}
for i in {1..7}; do for i in {1..7}; do
python3 run_player.py --host $host --port $port -n $i -t SE -f sim3d_7vs7 & python3 run_player.py --host $host --port $port -n $i -t SE &
done done

View File

@@ -51,7 +51,6 @@ class MathOps():
if size == 0: return vec if size == 0: return vec
return vec / size return vec / size
@staticmethod
def rel_to_global_3d(local_pos_3d: np.ndarray, global_pos_3d: np.ndarray, def rel_to_global_3d(local_pos_3d: np.ndarray, global_pos_3d: np.ndarray,
global_orientation_quat: np.ndarray) -> np.ndarray: global_orientation_quat: np.ndarray) -> np.ndarray:
''' Converts a local 3d position to a global 3d position given the global position and orientation (quaternion) ''' ''' Converts a local 3d position to a global 3d position given the global position and orientation (quaternion) '''

View File

@@ -1,229 +1,62 @@
from __future__ import annotations from abc import ABC, abstractmethod
from typing_extensions import override
from abc import ABC
from typing import Literal
import numpy as np
from world.commons.field_landmarks import FieldLandmarks from world.commons.field_landmarks import FieldLandmarks
GoalSide = Literal["our", "their", "left", "right"]
Bounds2D = tuple[float, float, float, float]
class Field(ABC): class Field(ABC):
FIELD_DIM: tuple[float, float, float]
LINE_WIDTH: float
GOAL_DIM: tuple[float, float, float]
GOALIE_AREA_DIM: tuple[float, float]
PENALTY_AREA_DIM: tuple[float, float] | None
PENALTY_SPOT_DISTANCE: float
CENTER_CIRCLE_RADIUS: float
DEFAULT_BEAM_POSES: dict[int, tuple[float, float, float]]
def __init__(self, world): def __init__(self, world):
from world.world import World # type hinting from world.world import World # type hinting
self.world: World = world self.world: World = world
self.field_landmarks: FieldLandmarks = FieldLandmarks(field=self) self.field_landmarks: FieldLandmarks = FieldLandmarks(world=self.world)
def _resolve_side(self, side: GoalSide) -> Literal["left", "right"]: def get_our_goal_position(self):
if side in ("our", "left"): return (-self.get_length() / 2, 0)
return "left"
if side in ("their", "right"):
return "right"
raise ValueError(f"Unknown field side: {side}")
def get_width(self) -> float: def get_their_goal_position(self):
return self.FIELD_DIM[1] return (self.get_length() / 2, 0)
def get_length(self) -> float: @abstractmethod
return self.FIELD_DIM[0] def get_width(self):
raise NotImplementedError()
def get_goal_dim(self) -> tuple[float, float, float]: @abstractmethod
return self.GOAL_DIM def get_length(self):
raise NotImplementedError()
def get_goal_half_width(self) -> float:
return self.GOAL_DIM[1] / 2.0
def get_center_circle_radius(self) -> float:
return self.CENTER_CIRCLE_RADIUS
def get_goal_position(self, side: GoalSide = "our") -> tuple[float, float]:
resolved_side = self._resolve_side(side)
x = -self.get_length() / 2.0 if resolved_side == "left" else self.get_length() / 2.0
return (x, 0.0)
def get_our_goal_position(self) -> tuple[float, float]:
return self.get_goal_position("our")
def get_their_goal_position(self) -> tuple[float, float]:
return self.get_goal_position("their")
def _build_box_bounds(self, depth: float, width: float, side: GoalSide) -> Bounds2D:
resolved_side = self._resolve_side(side)
field_half_x = self.get_length() / 2.0
half_width = width / 2.0
if resolved_side == "left":
return (-field_half_x, -field_half_x + depth, -half_width, half_width)
return (field_half_x - depth, field_half_x, -half_width, half_width)
def get_goalie_area_bounds(self, side: GoalSide = "our") -> Bounds2D:
return self._build_box_bounds(
depth=self.GOALIE_AREA_DIM[0],
width=self.GOALIE_AREA_DIM[1],
side=side,
)
def get_penalty_area_bounds(self, side: GoalSide = "our") -> Bounds2D:
if self.PENALTY_AREA_DIM is None:
raise ValueError(f"{type(self).__name__} does not define a penalty area")
return self._build_box_bounds(
depth=self.PENALTY_AREA_DIM[0],
width=self.PENALTY_AREA_DIM[1],
side=side,
)
def get_penalty_spot(self, side: GoalSide = "our") -> tuple[float, float]:
resolved_side = self._resolve_side(side)
x = (self.get_length() / 2.0) - self.PENALTY_SPOT_DISTANCE
return (-x, 0.0) if resolved_side == "left" else (x, 0.0)
def _is_inside_bounds(self, pos2d: np.ndarray | tuple[float, float], bounds: Bounds2D) -> bool:
x, y = float(pos2d[0]), float(pos2d[1])
min_x, max_x, min_y, max_y = bounds
return min_x <= x <= max_x and min_y <= y <= max_y
def is_inside_goalie_area(
self, pos2d: np.ndarray | tuple[float, float], side: GoalSide = "our"
) -> bool:
return self._is_inside_bounds(pos2d, self.get_goalie_area_bounds(side))
def is_inside_penalty_area(
self, pos2d: np.ndarray | tuple[float, float], side: GoalSide = "our"
) -> bool:
return self._is_inside_bounds(pos2d, self.get_penalty_area_bounds(side))
def is_inside_field(self, pos2d: np.ndarray | tuple[float, float]) -> bool:
field_half_x = self.get_length() / 2.0
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]:
try:
return 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
def get_default_beam_poses(self) -> dict[int, tuple[float, float, float]]:
return dict(self.DEFAULT_BEAM_POSES)
def get_canonical_landmarks(self) -> dict[str, np.ndarray]:
field_half_x = self.get_length() / 2.0
field_half_y = self.get_width() / 2.0
goal_half_y = self.get_goal_half_width()
penalty_marker_x = field_half_x - self.PENALTY_SPOT_DISTANCE
goalie_area_x = field_half_x - self.GOALIE_AREA_DIM[0]
goalie_marker_y = self.GOALIE_AREA_DIM[1] / 2.0
landmarks = {
"l_luf": np.array([-field_half_x, field_half_y, 0.0]),
"l_llf": np.array([-field_half_x, -field_half_y, 0.0]),
"l_ruf": np.array([field_half_x, field_half_y, 0.0]),
"l_rlf": np.array([field_half_x, -field_half_y, 0.0]),
"t_cuf": np.array([0.0, field_half_y, 0.0]),
"t_clf": np.array([0.0, -field_half_y, 0.0]),
"x_cuc": np.array([0.0, self.CENTER_CIRCLE_RADIUS, 0.0]),
"x_clc": np.array([0.0, -self.CENTER_CIRCLE_RADIUS, 0.0]),
"p_lpm": np.array([-penalty_marker_x, 0.0, 0.0]),
"p_rpm": np.array([penalty_marker_x, 0.0, 0.0]),
"g_lup": np.array([-field_half_x, goal_half_y, self.GOAL_DIM[2]]),
"g_llp": np.array([-field_half_x, -goal_half_y, self.GOAL_DIM[2]]),
"g_rup": np.array([field_half_x, goal_half_y, self.GOAL_DIM[2]]),
"g_rlp": np.array([field_half_x, -goal_half_y, self.GOAL_DIM[2]]),
"l_luga": np.array([-goalie_area_x, goalie_marker_y, 0.0]),
"l_llga": np.array([-goalie_area_x, -goalie_marker_y, 0.0]),
"l_ruga": np.array([goalie_area_x, goalie_marker_y, 0.0]),
"l_rlga": np.array([goalie_area_x, -goalie_marker_y, 0.0]),
"t_luga": np.array([-field_half_x, goalie_marker_y, 0.0]),
"t_llga": np.array([-field_half_x, -goalie_marker_y, 0.0]),
"t_ruga": np.array([field_half_x, goalie_marker_y, 0.0]),
"t_rlga": np.array([field_half_x, -goalie_marker_y, 0.0]),
}
if self.PENALTY_AREA_DIM is not None:
penalty_area_x = field_half_x - self.PENALTY_AREA_DIM[0]
penalty_marker_y = self.PENALTY_AREA_DIM[1] / 2.0
landmarks.update(
{
"l_lupa": np.array([-penalty_area_x, penalty_marker_y, 0.0]),
"l_llpa": np.array([-penalty_area_x, -penalty_marker_y, 0.0]),
"l_rupa": np.array([penalty_area_x, penalty_marker_y, 0.0]),
"l_rlpa": np.array([penalty_area_x, -penalty_marker_y, 0.0]),
"t_lupa": np.array([-field_half_x, penalty_marker_y, 0.0]),
"t_llpa": np.array([-field_half_x, -penalty_marker_y, 0.0]),
"t_rupa": np.array([field_half_x, penalty_marker_y, 0.0]),
"t_rlpa": np.array([field_half_x, -penalty_marker_y, 0.0]),
}
)
return landmarks
class FIFAField(Field): class FIFAField(Field):
FIELD_DIM = (105.0, 68.0, 40.0) def __init__(self, world):
LINE_WIDTH = 0.1 super().__init__(world)
GOAL_DIM = (1.6, 7.32, 2.44)
GOALIE_AREA_DIM = (5.5, 18.32) @override
PENALTY_AREA_DIM = (16.5, 40.32) def get_width(self):
PENALTY_SPOT_DISTANCE = 11.0 return 68
CENTER_CIRCLE_RADIUS = 9.15
DEFAULT_BEAM_POSES = { @override
1: (2.1, 0.0, 0.0), def get_length(self):
2: (22.0, 12.0, 0.0), return 105
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),
}
class HLAdultField(Field): class HLAdultField(Field):
FIELD_DIM = (14.0, 9.0, 40.0) def __init__(self, world):
LINE_WIDTH = 0.05 super().__init__(world)
GOAL_DIM = (0.6, 2.6, 1.8)
GOALIE_AREA_DIM = (1.0, 4.0)
PENALTY_AREA_DIM = (3.0, 6.0)
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),
}
@override
def get_width(self):
return 9
@override
def get_length(self):
return 14
class Soccer7vs7Field(Field): class Soccer7vs7Field(Field):
FIELD_DIM = (55.0, 36.0, 40.0) def __init__(self, world):
LINE_WIDTH = 0.1 super().__init__(world)
GOAL_DIM = (0.84, 3.9, 2.44)
GOALIE_AREA_DIM = (2.9, 9.6) @override
PENALTY_AREA_DIM = (8.6, 21.3) def get_width(self):
PENALTY_SPOT_DISTANCE = 5.8 return 36
CENTER_CIRCLE_RADIUS = 4.79
DEFAULT_BEAM_POSES = { @override
1: (2.0, 0.0, 0.0), def get_length(self):
2: (12.0, 8.0, 0.0), return 55
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),
}

View File

@@ -1,16 +1,14 @@
from __future__ import annotations
import numpy as np import numpy as np
from utils.math_ops import MathOps from utils.math_ops import MathOps
class FieldLandmarks: class FieldLandmarks:
def __init__(self, field): def __init__(self, world):
self.field = field from world.world import World # type hinting
self.world = field.world
self.landmarks: dict[str, np.ndarray] = {} self.world: World = world
self.canonical_landmarks: dict[str, np.ndarray] = field.get_canonical_landmarks()
self.landmarks: dict = {}
def update_from_perception(self, landmark_id: str, landmark_pos: np.ndarray) -> None: def update_from_perception(self, landmark_id: str, landmark_pos: np.ndarray) -> None:
""" """
@@ -23,19 +21,14 @@ class FieldLandmarks:
global_pos_3d = MathOps.rel_to_global_3d( global_pos_3d = MathOps.rel_to_global_3d(
local_pos_3d=local_cart_3d, local_pos_3d=local_cart_3d,
global_pos_3d=world.global_position, global_pos_3d=world.global_position,
global_orientation_quat=world.agent.robot.global_orientation_quat, global_orientation_quat=world.agent.robot.global_orientation_quat
) )
self.landmarks[landmark_id] = global_pos_3d self.landmarks[landmark_id] = global_pos_3d
def get_landmark_position( def get_landmark_position(self, landmark_id: str) -> np.ndarray | None:
self, landmark_id: str, use_canonical: bool = False
) -> np.ndarray | None:
""" """
Returns the current perceived or canonical global position for a landmark. Returns the calculated 2d global position for a given landmark ID.
Returns None if the landmark is not currently visible or processed.
""" """
source = self.canonical_landmarks if use_canonical else self.landmarks return self.global_positions.get(landmark_id)
return source.get(landmark_id)
def get_canonical_landmark_position(self, landmark_id: str) -> np.ndarray | None:
return self.canonical_landmarks.get(landmark_id)