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
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 | @register_plugin(name="payload.PayloadScanForTagMode", base_cls=Mode)
class PayloadScanForTagMode(Mode):
"""
Spin in place looking for a specific AprilTag.
Returns:
"found" — target tag is visible; stop and hand off to approach mode
"not_found" — completed a full 360° without seeing the tag; caller
should reposition and try again
"""
required_vision_nodes = (PayloadAprilTagNode,)
transition_labels = ("found", "not_found")
requires_camera = True
def __init__(
self,
node: Node,
vehicle: Payload,
tag_id: int = 0,
tag_size_m: float = 0.0508,
tag_family: str = DEFAULT_TAG_FAMILY,
spin_angular_speed: float = 0.4,
loop: bool = False,
compressed_image: bool = False,
):
super().__init__(node, vehicle)
self.tag_id = int(tag_id)
self.tag_size_m = float(tag_size_m)
self.tag_family = str(tag_family) if tag_family else DEFAULT_TAG_FAMILY
self.spin_angular_speed = float(spin_angular_speed)
self.loop = bool(loop)
self._compressed_image = bool(compressed_image)
self._bridge = CvBridge()
def on_enter(self) -> None:
self._angle_swept = 0.0
self._status = "continue"
self._latest_image = None
# Match spin direction to however PayloadDLZNavigateMode was travelling.
# CCW travel → positive angular (spin left/CCW).
# CW travel → negative angular (spin right/CW).
nav_direction = getattr(self.node, "dlz_navigate_direction", None)
if nav_direction == "cw":
self._signed_spin_speed = -self.spin_angular_speed
else:
self._signed_spin_speed = self.spin_angular_speed
self._image_sub = None
self._annotated_pub = None
if getattr(self.node, "vision_debug", False):
cam_topic = self.vehicle.namespaced_path("camera")
if self._compressed_image:
self._image_sub = self.node.create_subscription(
CompressedImage, f"{cam_topic}/compressed", self._image_cb, 1
)
else:
self._image_sub = self.node.create_subscription(
Image, cam_topic, self._image_cb, 1
)
annotated_topic = self.vehicle.namespaced_path("annotated_image/compressed")
self._annotated_pub = self.node.create_publisher(
CompressedImage, annotated_topic, 1
)
self.log(
f"PayloadScanForTagMode: scanning for tag {self.tag_id} "
f"(spin={self._signed_spin_speed:.2f} rad/s, nav_direction={nav_direction or 'unset→ccw'})"
)
def _image_cb(self, msg) -> None:
self._latest_image = msg
def _get_bgr(self) -> Optional[np.ndarray]:
msg = self._latest_image
if msg is None:
return None
if self._compressed_image:
buf = np.frombuffer(msg.data, dtype=np.uint8)
return cv2.imdecode(buf, cv2.IMREAD_COLOR)
return self._bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
def _publish_annotated(self, bgr: np.ndarray, response) -> None:
if self._annotated_pub is None:
return
debug = bgr.copy()
h, w = debug.shape[:2]
if response is not None and response.has_image:
for i, tid in enumerate(response.all_tag_ids):
cx = int(response.all_center_x[i])
cy = int(response.all_center_y[i])
tz = response.all_tvec_z[i]
color = (0, 255, 0) if int(tid) == self.tag_id else (0, 255, 255)
r = 12
cv2.circle(debug, (cx, cy), r, color, 2)
cv2.drawMarker(debug, (cx, cy), color, cv2.MARKER_CROSS, r * 2, 1)
cv2.putText(
debug,
f"id{int(tid)} {tz:.2f}m",
(cx + r + 4, cy - 4),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
color,
2,
)
sweep_deg = math.degrees(self._angle_swept)
cv2.putText(
debug,
f"SCAN sweep={sweep_deg:.0f} deg target=id{self.tag_id}",
(5, 25),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
(255, 255, 255),
2,
)
try:
msg = self._bridge.cv2_to_compressed_imgmsg(debug, dst_format="jpeg")
msg.header.stamp = self.node.get_clock().now().to_msg()
self._annotated_pub.publish(msg)
except Exception:
pass
def _request_state(self) -> Optional[PayloadAprilTagState.Response]:
req = PayloadAprilTagState.Request()
req.tag_size_m = self.tag_size_m
req.tag_family = self.tag_family
return self.send_request(PayloadAprilTagNode, req)
def on_update(self, time_delta: float) -> None:
if self._status != "continue":
self.vehicle.stop()
return
response = self._request_state()
bgr = self._get_bgr()
if bgr is not None:
self._publish_annotated(bgr, response)
if response is not None and response.has_image:
tag_ids = list(response.all_tag_ids)
if self.tag_id in tag_ids:
self.vehicle.stop()
self.log(f"PayloadScanForTagMode: tag {self.tag_id} found → approach")
if self.loop:
self._angle_swept = 0.0
else:
self._status = "found"
return
# Check if full rotation complete before spinning more
if self._angle_swept >= _TWO_PI:
self.vehicle.stop()
self.log(
f"PayloadScanForTagMode: 360° complete, tag {self.tag_id} not found"
)
if self.loop:
self._angle_swept = 0.0
else:
self._status = "not_found"
return
self._angle_swept += abs(self._signed_spin_speed) * time_delta
self.vehicle.drive(0.0, self._signed_spin_speed)
def check_status(self) -> str:
return self._status
def on_exit(self) -> None:
self.vehicle.stop()
if self._image_sub is not None:
self.node.destroy_subscription(self._image_sub)
self._image_sub = None
if self._annotated_pub is not None:
self.node.destroy_publisher(self._annotated_pub)
self._annotated_pub = None
|