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 | 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.planner.shared_properties.avoidance_strategy = new_strategy.val
async def init_poses(self):
self.avoidance_backup = AvoidanceStrategy(self.planner.shared_properties.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.planner.shared_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.planner.shared_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.planner.shared_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:
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
|