def cbGetApriltagDetections(self,params):
# print(params)
if self.image_msg == None:
return GetApriltagDetectionsResponse()
if hasattr(self.image_msg,'format'): # CompressedImage
try:
cv_image = bgr_from_jpg(self.image_msg.data)
except ValueError as e:
rospy.loginfo('Anti_instagram cannot decode image: %s' % e)
return
else: # Image
cv_image = self.bridge.imgmsg_to_cv2(self.image_msg, desired_encoding="bgr8")
self.pub_raw.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
# rect_image = cv_image
rect_image = self.rector.rect(cv_image)
self.pub_rect.publish(self.bridge.cv2_to_imgmsg(rect_image,"bgr8"))
tags = self.detector.detect(rect_image)
if self.visualization:
for tag in tags:
for idx in range(len(tag.corners)):
cv2.line(rect_image, tuple(tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0))
# cv2.putText(rect_image, "中文测试", # not work
cv2.putText(rect_image, str(tag.tag_id),
org=(tag.corners[0, 0].astype(int)+10,tag.corners[0, 1].astype(int)+10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8,
color=(0, 0, 255))
imgmsg = self.bridge.cv2_to_imgmsg(rect_image,"bgr8")
self.pub_detections.publish(imgmsg)
return self.toApriltagDetections(tags)
def cbGetApriltagDetections(self, params):
# print(params)
if self.cv_image != None:
rect_image = self.rector.rect(self.cv_image)
# self.pub_rect.publish(self.bridge.cv2_to_imgmsg(rect_image,"bgr8"))
tags = self.detector.detect(rect_image)
if self.visualization:
for tag in tags:
for idx in range(len(tag.corners)):
cv2.line(rect_image,
tuple(tag.corners[idx - 1, :].astype(int)),
tuple(tag.corners[idx, :].astype(int)),
(0, 255, 0))
# cv2.putText(rect_image, "中文测试", # not work
cv2.putText(rect_image,
str(tag.tag_id),
org=(tag.corners[0, 0].astype(int) + 10,
tag.corners[0, 1].astype(int) + 10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8,
color=(0, 0, 255))
imgmsg = self.bridge.cv2_to_imgmsg(rect_image, "bgr8")
self.pub_detections.publish(imgmsg)
return self.toApriltagDetections(tags)
return GetApriltagDetectionsResponse()
def toApriltagDetections(self,tags):
msg = GetApriltagDetectionsResponse()
for tag in tags:
r = R.from_dcm(np.array(tag.pose_R))
offset = np.array(tag.pose_t)*100
euler = r.as_euler('xyz', degrees=True)
detection = ApriltagPose(id=tag.tag_id,pose_r=euler,pose_t=offset)
msg.detections.append(detection)
return msg
def toApriltagDetections(self):
msg = GetApriltagDetectionsResponse()
items = self.detector.detections
for item in items:
detection = ApriltagPose(id=item[0],
pose_r=item[1],
pose_t=item[2])
msg.detections.append(detection)
return msg
def cbGetApriltagDetections(self, params):
# res = self.get_frame(GetFrameRequest())
# res = self.get_frame(GetFrameRequest([320,240]))
# self.image_msg = res.image
# print(params)
image_msg = self.image_msg
if image_msg == None:
return GetApriltagDetectionsResponse()
rect_image = toImage(image_msg)
resized_image = cv2.resize(rect_image, (640, 480))
# image_gray = cv2.cvtColor(resized_image, cv2.COLOR_BGR2GRAY)
self.detector.detect(resized_image, label_tags=self.visualization)
if self.visualization:
resized_image = cv2.resize(resized_image, (480, 360))
self.pubImage(resized_image)
return self.toApriltagDetections()