def __init__(self):
# Initialize image converter
self.bridge = CvBridge()
self.counter = 0
self.old_x = 0
self.old_y = 0
# Initialize raw image subscriber
self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image,
self.imageCb)
self.image = []
# Initialize april tag publisher
self.marker_pub = rospy.Publisher("/marked_april_tag",
Image,
queue_size=1)
self.marked_image = []
self.pub_image = 0
self.pose_pub = rospy.Publisher("/pose", pose, queue_size=1)
# Initialize detection subscriber
self.detection_sub = rospy.Subscriber("/apriltags/detections",
AprilTagDetections,
self.detectionCb)
self.detection_result = AprilTagDetections()
rospy.loginfo("Yay I am started")
def processtag_callback(self, rgb_data, depth_data, tag_data):
try:
self.rgb_image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
self.depth_image = self.bridge.imgmsg_to_cv2(depth_data, "16UC1")
except CvBridgeError as e:
print(e)
all_detections = tag_data.detections
detections_transformed = []
marker_transformed = []
if (self.rgb_image != None and self.depth_image != None):
for current_detection in all_detections:
current_fused, current_marker = fuser.fuse_transform(
current_detection, self.rgb_image, self.depth_image,
self.camera_intrinsics, tag_data.header.frame_id)
current_marker.ns = 'tag' + str(current_detection.id)
current_marker.id = current_detection.id
detections_transformed.append(current_fused)
marker_transformed.append(current_marker)
detection_msg = AprilTagDetections()
detection_msg.detections = detections_transformed
marker_msg = MarkerArray()
marker_msg.markers = marker_transformed
self.detection_publish.publish(detection_msg)
self.marker_publish.publish(marker_msg)
def detection_callback(self, data):
all_detections = data.detections
detections_transformed = []
marker_transformed = []
if (self.rgb_image != None and self.depth_image != None):
for current_detection in all_detections:
current_fused, current_marker = fuser.fuse_transform(
current_detection, self.rgb_image, self.depth_image,
self.camera_intrinsics, data.header.frame_id)
detections_transformed.append(current_fused)
marker_transformed.append(current_marker)
detection_msg = AprilTagDetections()
detection_msg.detections = detections_transformed
marker_msg = MarkerArray()
marker_msg.markers = marker_transformed
self.detection_publish.publish(detection_msg)
self.marker_publish.publish(marker_msg)
def __init__(self):
# Initialize image converter
self.bridge = CvBridge()
# Initialize raw image subscriber
self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image,
self.imageCb)
self.image = []
# Initialize april tag publisher
self.marker_pub = rospy.Publisher("/marked_april_tag",
Image,
queue_size=1)
self.marked_image = []
self.pub_image = 0
# Initialize detection subscriber
self.detection_sub = rospy.Subscriber("/apriltags/detections",
AprilTagDetections,
self.detectionCb)
self.detection_result = AprilTagDetections()