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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283 | class CaptureCratesAction(Action):
"""
Action used to capture crates.
"""
def __init__(
self,
planner: "Planner",
strategy: Strategy,
collection_area_id: CollectionAreaID,
weight: float = 2000000.0,
):
self.custom_weight = weight
super().__init__(f"CaptureCollectionArea {collection_area_id.name}", planner, strategy)
self.before_action_func = self.before_action
self.collection_area_id = collection_area_id
self.shift_align = 160
self.shift_capture_front = self.shift_align + 15
self.shift_capture_back = self.shift_align + 15
self.shift_approach = self.shift_align + 160
if Camp().color == Camp.Colors.blue:
self.good_crate_id = 36
self.bad_crate_id = 47
else:
self.good_crate_id = 47
self.bad_crate_id = 36
@property
def collection_area(self) -> CollectionArea:
return self.planner.game_context.collection_areas[self.collection_area_id]
async def recycle(self):
self.collection_area.enabled = True
self.recycled = True
async def init_start_pose(self):
pass
async def before_action(self):
self.logger.info(f"{self.name}: before_action")
self.poses.clear()
# # TODO: force back, to remove
# self.planner.game_context.front_free = False
if self.planner.game_context.front_free:
self.logger.info(f"{self.name}: before_action: front selected")
self.side = "front"
self.shift_capture = self.shift_capture_front
self.crates_ids = self.planner.game_context.front_crates
self.arms_open = functools.partial(actuators.front_arms_open, self.planner)
self.arms_close = functools.partial(actuators.front_arms_close, self.planner)
self.lift_down = functools.partial(actuators.front_lift_down, self.planner)
self.lift_up = functools.partial(actuators.front_lift_up, self.planner)
else:
self.logger.info(f"{self.name}: before_action: back selected")
self.side = "back"
self.shift_capture = self.shift_capture_back
self.crates_ids = self.planner.game_context.back_crates
self.arms_open = functools.partial(actuators.back_arms_open, self.planner)
self.arms_close = functools.partial(actuators.back_arms_close, self.planner)
self.lift_down = functools.partial(actuators.back_lift_down, self.planner)
self.lift_up = functools.partial(actuators.back_lift_up, self.planner)
collection_area_copy = self.collection_area.model_copy()
if "O" not in self.collection_area.model_fields_set:
# Optimize collection area orientation based on robot position
pose_current = self.pose_current
if pose_current.x > self.collection_area.x:
collection_area_copy.O = 180.0
else:
collection_area_copy.O = 0.0
await self.init_start_pose()
# Approach
approach_pose = Pose(
**get_relative_pose(
collection_area_copy,
front_offset=-self.shift_approach,
angular_offset=0,
).model_dump(),
max_speed_linear=100,
max_speed_angular=100,
motion_direction=MotionDirection.BIDIRECTIONAL,
before_pose_func=self.before_approach,
after_pose_func=self.after_approach,
)
self.poses.append(approach_pose)
self.logger.info(
f"{self.name}: approach: x={approach_pose.x: 5.2f} y={approach_pose.y: 5.2f} O={approach_pose.O: 3.2f}°"
)
if self.side == "back":
rotation_pose = Pose(
x=approach_pose.x,
y=approach_pose.y,
O=(approach_pose.O + 180) % 360,
max_speed_linear=30,
max_speed_angular=30,
motion_direction=MotionDirection.BIDIRECTIONAL,
bypass_final_orientation=False,
before_pose_func=self.before_rotation,
after_pose_func=self.after_rotation,
)
self.poses.append(rotation_pose)
self.logger.info(
f"{self.name}: rotation: x={rotation_pose.x: 5.2f} y={rotation_pose.y: 5.2f} O={rotation_pose.O: 3.2f}°"
)
# Align
align_pose = Pose(
**get_relative_pose(
collection_area_copy,
front_offset=-self.shift_align,
angular_offset=0 if self.side == "front" else 180,
).model_dump(),
max_speed_linear=10,
max_speed_angular=15,
motion_direction=(MotionDirection.FORWARD_ONLY if self.side == "front" else MotionDirection.BACKWARD_ONLY),
bypass_final_orientation=True,
before_pose_func=self.before_align,
after_pose_func=self.after_align,
)
self.poses.append(align_pose)
self.logger.info(f"{self.name}: align: x={align_pose.x: 5.2f} y={align_pose.y: 5.2f} O={align_pose.O: 3.2f}°")
# Capture
capture_pose = Pose(
**get_relative_pose(
collection_area_copy,
front_offset=-self.shift_capture,
angular_offset=0 if self.side == "front" else 180,
).model_dump(),
max_speed_linear=10,
max_speed_angular=10,
motion_direction=(MotionDirection.BACKWARD_ONLY if self.side == "front" else MotionDirection.FORWARD_ONLY),
bypass_final_orientation=True,
before_pose_func=self.before_capture,
after_pose_func=self.after_capture,
)
self.poses.append(capture_pose)
self.logger.info(
f"{self.name}: capture: x={capture_pose.x: 5.2f} y={capture_pose.y: 5.2f} O={capture_pose.O: 3.2f}°"
)
async def before_approach(self):
self.logger.info(f"{self.name}: before_approach")
if self.planner.game_context.front_free:
await actuators.front_arms_close(self.planner)
await actuators.front_lift_up(self.planner)
if self.planner.game_context.back_free:
await actuators.back_arms_close(self.planner)
await actuators.back_lift_up(self.planner)
async def after_approach(self):
self.logger.info(f"{self.name}: after_approach")
await asyncio.sleep(0.2)
crates_found: list[tuple[int, models.Pose]] = await get_crates_position(self.planner)
self.logger.info(f"{self.name}: crates found:")
for crate_id, pose in crates_found:
self.logger.info(
f"{self.name}: - {crate_id}: x={pose.x: 5.2f} y={pose.y: 5.2f} O={pose.O: 3.2f}°"
f" dist={math.dist((0,0),(pose.x,pose.y)):5.2f}mm"
)
# Analyze crates
analyzer = CrateAnalyzer(self.good_crate_id, self.bad_crate_id)
valid_groups = analyzer.find_groups(crates_found)
if not valid_groups:
self.logger.info(f"{self.name}: Rejected: no valid crate group found")
self.poses.clear()
self.collection_area.invalid = True
if len(crates_found) > 0:
# Keep obstacle
self.collection_area.enabled = True
return
# Keep closest group
valid_groups.sort(key=lambda g: math.hypot(g.pose.x, g.pose.y))
group = valid_groups[0]
# Check alignment with robot (angle must be close to 0 or 180)
# Normalize angle to [-180, 180]
max_angle_distance = 10.0 # degrees
angle = group.pose.O
while angle > 180:
angle -= 360
while angle <= -180:
angle += 360
# CrateAnalyzer returns crates sorted by Y coordinate in group frame
# We need them sorted from Left (+Y robot) to Right (-Y robot) to map to actuators
if abs(angle) < max_angle_distance:
# Group aligned with robot (0°)
# Group Y+ is aligned with Robot Y+.
# Analyzer sorts by Group Y ascending -> Robot Y ascending (Right to Left).
# We need Left to Right -> Reverse.
self.crates_ids[:] = list(reversed(group.crate_ids))[:]
elif abs(angle) > 180 - max_angle_distance:
# Group flipped (180°)
# Group Y+ is aligned with Robot Y-.
# Analyzer sorts by Group Y ascending -> Robot Y descending (Left to Right).
# Already in correct order.
self.crates_ids[:] = group.crate_ids[:]
else:
self.logger.warning(f"{self.name}: Rejected group: angle {group.pose.O: .2f}° not close to 0° or 180°")
self.poses.clear()
self.collection_area.invalid = True
self.collection_area.enabled = True
return
if not self.planner.game_context.front_free:
# If front is not free, we take the back crates, which are in the opposite order, so we reverse them.
self.crates_ids.reverse()
self.logger.info(f"{self.name}: Accepted group (angle={angle:.1f}°): {self.crates_ids}")
async def before_rotation(self):
self.logger.info(f"{self.name}: before_rotation")
async def after_rotation(self):
self.logger.info(f"{self.name}: after_rotation")
async def before_align(self):
self.logger.info(f"{self.name}: before_align")
self.collection_area.enabled = False
await self.lift_down()
await self.arms_open()
self.logger.info(f"{self.name}: before_align end")
async def after_align(self):
self.logger.info(f"{self.name}: after_align")
async def before_capture(self):
self.logger.info(f"{self.name}: before_capture")
async def after_capture(self):
self.logger.info(f"{self.name}: after_capture")
await crates_utils.take_crates(self.planner, self.side)
# # TODO: force back, to remove
# self.planner.game_context.front_free = True
def weight(self) -> float:
if not self.collection_area.enabled:
self.logger.info(f"{self.name}: Rejected: collection area disabled")
return 0
if self.collection_area.invalid:
self.logger.info(f"{self.name}: Rejected: collection area marked as invalid")
return 0
if not self.planner.game_context.front_free and not self.planner.game_context.back_free:
self.logger.info(f"{self.name}: Rejected: both front and back are full")
return 0
return self.custom_weight
|