15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131 | class AlignBottomAction(Action):
"""
Action used to align the back of the robot on the border before game start.
Only on Bottom start position.
"""
def __init__(
self,
planner: "Planner",
actions: Actions,
*,
final_pose: models.Pose = Pose(x=-750, y=-250, O=0),
reset_countdown=False,
weight: float = 2000000.0,
):
self.final_pose = final_pose
self.reset_countdown = reset_countdown
self.custom_weight = weight
super().__init__("Align Bottom action", planner, actions)
self.before_action_func = self.init_poses
def set_avoidance(self, new_strategy: AvoidanceStrategy):
logger.info(f"{self.name}: set avoidance to {new_strategy.name}")
self.game_context.avoidance_strategy = new_strategy
self.planner.shared_properties["avoidance_strategy"] = new_strategy
async def init_poses(self):
self.avoidance_backup = self.game_context.avoidance_strategy
# On start, the robot is aligned on the right (blue camp) border of the Bottom start position
self.start_pose = AdaptedPose(
x=-750,
y=-500 + self.game_context.properties.robot_width / 2,
O=0,
)
await self.planner.sio_ns.emit("pose_start", self.start_pose.model_dump())
# Align back
pose = Pose(
x=-1200,
y=self.start_pose.y,
O=self.start_pose.O,
max_speed_linear=10,
max_speed_angular=10,
allow_reverse=True,
bypass_anti_blocking=True,
timeout_ms=0,
bypass_final_orientation=True,
before_pose_func=self.before_align_back,
after_pose_func=self.after_align_back,
)
self.poses.append(pose)
# Step forward
pose = Pose(
x=-950 + self.game_context.properties.robot_length / 2,
y=self.start_pose.y,
O=self.start_pose.O,
max_speed_linear=50,
max_speed_angular=50,
allow_reverse=False,
bypass_final_orientation=False,
before_pose_func=self.before_step_forward,
after_pose_func=self.after_step_forward,
)
self.poses.append(pose)
# Final pose
pose = AdaptedPose(
x=self.final_pose.x,
y=self.final_pose.y,
O=self.final_pose.O,
max_speed_linear=50,
max_speed_angular=50,
allow_reverse=True,
before_pose_func=self.before_final_pose,
after_pose_func=self.after_final_pose,
)
self.poses.append(pose)
async def before_align_back(self):
logger.info(f"{self.name}: before_align_back")
self.set_avoidance(AvoidanceStrategy.Disabled)
async def after_align_back(self):
logger.info(f"{self.name}: after_align_back")
current_pose = models.Pose(
x=-1000 + self.game_context.properties.robot_length / 2,
y=self.start_pose.y,
O=0,
)
await self.planner.sio_ns.emit("pose_start", current_pose.model_dump())
await asyncio.sleep(1)
async def before_step_forward(self):
logger.info(f"{self.name}: before_step_forward")
async def after_step_forward(self):
logger.info(f"{self.name}: after_step_forward")
async def before_final_pose(self):
logger.info(f"{self.name}: before_final_pose")
async def after_final_pose(self):
logger.info(f"{self.name}: after_final_pose")
self.set_avoidance(self.avoidance_backup)
if self.reset_countdown:
# self.game_context.countdown = self.game_context.game_duration
now = datetime.now(UTC)
self.planner.countdown_start_timestamp = now
await self.planner.sio_ns.emit(
"start_countdown",
(self.planner.robot_id, self.game_context.game_duration, now.isoformat(), "deepskyblue"),
)
def weight(self) -> float:
return self.custom_weight
|