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 | class CaptureTribuneAction(Action):
"""
Action used to capture tribune using magnets.
"""
def __init__(
self,
planner: "Planner",
strategy: Strategy,
tribune_id: artifacts.TribuneID,
weight: float = 2000000.0,
):
self.custom_weight = weight
super().__init__(f"CaptureTribune {tribune_id.name}", planner, strategy)
self.before_action_func = self.before_action
self.tribune_id = tribune_id
self.shift_capture = 140
self.shift_approach = self.shift_capture + 150
self.shift_step_back = self.shift_capture + 80
@property
def tribune(self) -> artifacts.Tribune:
return self.planner.game_context.tribunes[self.tribune_id]
async def recycle(self):
self.tribune.enabled = True
self.recycled = True
async def before_action(self):
self.start_pose = self.pose_current
# Approach
approach_pose = Pose(
**get_relative_pose(
self.tribune,
front_offset=self.shift_approach,
angular_offset=180,
).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)
# Capture
capture_pose = Pose(
**get_relative_pose(
self.tribune,
front_offset=self.shift_capture,
angular_offset=180,
).model_dump(),
max_speed_linear=20,
max_speed_angular=20,
motion_direction=MotionDirection.FORWARD_ONLY,
bypass_final_orientation=False,
before_pose_func=self.before_capture,
after_pose_func=self.after_capture,
)
self.poses.append(capture_pose)
if (
(
self.tribune_id == artifacts.TribuneID.LocalCenter
and self.planner.shared_properties.table == TableEnum.Training
)
or self.tribune_id == artifacts.TribuneID.LocalTop
or self.tribune_id == artifacts.TribuneID.LocalTopSide
or self.tribune_id == artifacts.TribuneID.LocalBottomSide
or self.tribune_id == artifacts.TribuneID.OppositeTop
or self.tribune_id == artifacts.TribuneID.OppositeTopSide
or self.tribune_id == artifacts.TribuneID.OppositeBottomSide
):
# Step back
pose = Pose(
**get_relative_pose(
self.tribune,
front_offset=self.shift_step_back,
angular_offset=180,
).model_dump(),
max_speed_linear=50,
max_speed_angular=50,
motion_direction=MotionDirection.BIDIRECTIONAL,
bypass_final_orientation=False,
before_pose_func=self.before_step_back,
after_pose_func=self.after_step_back,
)
self.poses.append(pose)
async def before_approach(self):
self.logger.info(f"{self.name}: before_approach")
async def after_approach(self):
self.logger.info(f"{self.name}: after_approach")
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")
self.planner.game_context.tribunes_in_robot = 2
async def before_step_back(self):
self.logger.info(f"{self.name}: before_step_back")
async def after_step_back(self):
self.logger.info(f"{self.name}: after_step_back")
def weight(self) -> float:
if not self.tribune.enabled or self.planner.game_context.tribunes_in_robot != 0:
return 0
return self.custom_weight
|