diff --git a/Dockerfile b/Dockerfile index f23a09d..f34f661 100644 --- a/Dockerfile +++ b/Dockerfile @@ -47,12 +47,14 @@ ENV DT_LAUNCH_PATH "${LAUNCH_PATH}" ENV DT_LAUNCHER "${LAUNCHER}" # install apt dependencies +RUN apt-key adv --keyserver keyserver.ubuntu.com --recv-keys F42ED6FBAB17C654 COPY ./dependencies-apt.txt "${REPO_PATH}/" RUN dt-apt-install ${REPO_PATH}/dependencies-apt.txt # install python3 dependencies #COPY ./dependencies-py3.txt "${REPO_PATH}/" #RUN dt-pip3-install ${REPO_PATH}/dependencies-py3.txt +RUN pip install dt-apriltags # copy the source code COPY ./packages "${REPO_PATH}/packages" diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py index 01347d8..00c8e0a 100755 --- a/packages/bot_camera/src/bot_camera.py +++ b/packages/bot_camera/src/bot_camera.py @@ -13,6 +13,7 @@ from dt_class_utils import DTReminder from turbojpeg import TurboJPEG from cv_bridge import CvBridge +from dt_apriltags import Detector class BotCamera(DTROS): @@ -48,13 +49,57 @@ def cb_image(self, msg): img = self.bridge.compressed_imgmsg_to_cv2(msg) img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) - + + # track the contour of detected markers + img = self.marker_detecting(img) + # turn 'raw image' into 'raw image message' out_msg = self.bridge.cv2_to_imgmsg(img, "rgb8") # maintain original header out_msg.header = msg.header # publish image self.pub_img.publish(out_msg) + + def marker_detecting(self, in_image): + # more info about the dt-apriltag package you can see here: + # https://github.com/duckietown/lib-dt-apriltags + + # init AprilTag detector + tag_detector = Detector(families = "tag36h11", + nthreads = 1, + quad_decimate = 2.0, + quad_sigma = 0.0, + refine_edges = 1, + decode_sharpening = 0.25, + debug = 0) + + gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY) + tags = tag_detector.detect(gray_img) + + for tag in tags: + (topLeft, topRight, bottomRight, bottomLeft) = tag.corners + + topRight = (int(topRight[0]), int(topRight[1])) + bottomRight = (int(bottomRight[0]), int(bottomRight[1])) + bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1])) + topLeft = (int(topLeft[0]), int(topLeft[1])) + + #draw the bounding box + line_width = 2 + line_color = (0, 255, 0) + cv2.line(in_image, topLeft, topRight, line_color, line_width) + cv2.line(in_image, topRight, bottomRight, line_color, line_width) + cv2.line(in_image, bottomRight, bottomLeft, line_color, line_width) + cv2.line(in_image, bottomLeft, topLeft, line_color, line_width) + + #draw the center of marker + cv2.circle(in_image, tuple(map(int, tag.center)), 2, (0, 0, 255), -1) + + #draw the marker ID on the image + cv2.putText(in_image, str(tag.tag_id), org=(topLeft[0], topLeft[1] - 15), + fontFace = cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.5, color=(255,0,0)) + + return in_image if __name__ == "__main__":