5. aruco 마커를 탐지
import cv2
from djitellopy import Tello
# 아루코 마커 검출기 준비
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) # 이는 4x4 비트 패턴을 쓰는 maker 50종이다.
params = cv2.aruco.DetectorParameters() # 아루코 검출 파라미터 설정 객체를 설정. 기본값으로도 동작한다. 환경에 따라서 튜닝가능
detector = cv2.aruco.ArucoDetector(aruco_dict, params) # 실제 aruco를 detect하는 객체를 생성
# tello드론 연결 및 스트리밍 시작
tello = Tello() # 텔로 객체 생성
tello.connect() # 드론과 wifi로 연결된 상태에서 connect를 호출하면 sdk 통신 시작
print("Battery:", tello.get_battery(), "%") # 정상 연결이 되었는지 확인
tello.streamon() # 드론 카메라 streaming 활성화
frame_read = tello.get_frame_read() # frame을 비동기로 읽어오는 helper객체를 얻는다.
# ----메인 루프----
while True:
frame = frame_read.frame # 현재의 최신 frame을 가져온다.
if frame is None: # 만약 프레임이 준비되지 않거나, 통신 문제가 존재할때 none이다.
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # aruco 검출은 일반적으로 그레이스케일에서 수행 ( 속도/안전성에서 유리 )
corners, ids, _ = detector.detectMarkers(gray) # 마커를 검출. corners는 마커별 4개의 코너 좌표.
if ids is not None:#ids가 none이라면, 마커가 하나도 검출되지 않았다는 뜻
cv2.aruco.drawDetectedMarkers(frame, corners, ids) # 검출된 마커의 테두리와 ID를 프레임 위에 그려줌.
cv2.putText(frame, f"IDs: {ids.flatten().tolist()}", # 좌상단에 IDs: 형태로 텍스트가 나올거임,
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0), 2) # 텍스트 시작 좌표 , 폰트 종류 , 폰트 크기 , 색상 , 두깨
cv2.imshow("ArUco test (Tello)", frame)# imshow하면 자동으로 창이 띄워지고 또다시 imshow가 불러지면, 창이 존재하는경우에 안의 이미지만 업데이트 하는거
if (cv2.waitKey(1) & 0xFF) == 27: # ESC
break
tello.streamoff()
cv2.destroyAllWindows()
6. 특정한 aruco마커만 탐지
import cv2
from djitellopy import Tello
import numpy as np
TARGET_IDS = {7, 8} # 목표로 하는 aruco마커 set
# -----aruco검출기 준비----------
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) # 이는 4x4 비트 패턴을 쓰는 maker 50종이다.
params = cv2.aruco.DetectorParameters() # 아루코 검출 파라미터 설정 객체를 설정. 기본값으로도 동작한다. 환경에 따라서 튜닝가능
detector = cv2.aruco.ArucoDetector(aruco_dict, params) # aruco maker 검출 객체 생성
# -----tello 연결 및 영상 스트리밍 시작-----
tello = Tello()# 텔로 객체 생성
tello.connect()# 드론과 wifi로 연결된 상태에서 connect를 호출하면 sdk 통신 시작
print("Battery:", tello.get_battery(), "%")# 정상 연결이 되었는지 확인
tello.streamon() # 드론 카메라 streaming 활성화
frame_read = tello.get_frame_read() # frame을 비동기로 읽어오는 helper객체를 얻는다.
# ----- 메인 루프 -----
while True:
frame = frame_read.frame # 현재의 최신 frame을 가져온다.
if frame is None: # 만약 프레임이 준비되지 않거나, 통신 문제가 존재할때 none이다.
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)# aruco 검출은 일반적으로 그레이스케일에서 수행 ( 속도/안전성에서 유리 )
corners, ids, _ = detector.detectMarkers(gray)# 마커를 검출. corners는 마커별 4개의 코너 좌표. ids는 검출된 마커의 id목록들
# ids: (N,1) 형태
if ids is not None: # ids가 none이라면 maker를 하나도 못찾은 상태이다.
ids_flat = ids.flatten() # ids는 ( N,1) 형태라서 다루기 불편. 그리서 ( N, )으로 만들어줌. 그러면 [ 7,8,9]와 같은 형태가 됨
# 7 또는 8만 남기기. enumerate를 하면 (인덱스,해당 인덱스의 값)이 나온다.
keep_idx = [i for i, mid in enumerate(ids_flat) if int(mid) in TARGET_IDS]
if keep_idx: # keep_idx가 비어있지 않은 경우. 즉, 목표하는 id를 하나라도 찾은 경우
filtered_corners = [corners[i] for i in keep_idx] # 목표하는 id의 인덱스만 남긴 corners를 새로 만든다
filtered_ids = ids[keep_idx] # 목표하는 id의 목표 인덱스만 골라낸다.
cv2.aruco.drawDetectedMarkers(frame, filtered_corners, filtered_ids)
cv2.putText(
frame,
f"Detected target IDs: {filtered_ids.flatten().tolist()}",
(10, 30),
cv2.FONT_HERSHEY_SIMPLEX,
1,
(0, 255, 0),
2
)
else:
cv2.putText(frame, "No target (7/8) detected", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
cv2.imshow("ArUco test (Tello) - only 7/8", frame)
if (cv2.waitKey(1) & 0xFF) == 27: # ESC
break
tello.streamoff()
cv2.destroyAllWindows()
7. aruco maker와의 거리 계산
# pip uninstall -y opencv-python opencv-contrib-python
# pip install opencv-contrib-python
import cv2
import numpy as np
from djitellopy import Tello
TARGET_IDS = {7, 8}
cameraMatrix = np.array([
[920.0, 0.0, 480.0],
[ 0.0, 920.0, 360.0],
[ 0.0, 0.0, 1.0]
], dtype=np.float32)
distCoeffs = np.array([-0.18, 0.06, 0.000, 0.000, -0.01], dtype=np.float32).reshape(-1, 1)
# 20cm 마커인 경우
MARKER_LENGTH_M = 0.20
# -----aruco검출기 준비----------
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) # 이는 4x4 비트 패턴을 쓰는 maker 50종이다.
params = cv2.aruco.DetectorParameters() # 아루코 검출 파라미터 설정 객체를 설정. 기본값으로도 동작한다. 환경에 따라서 튜닝가능
detector = cv2.aruco.ArucoDetector(aruco_dict, params) # aruco maker 검출 객체 생성
# -----tello 연결 및 영상 스트리밍 시작-----
tello = Tello() # 텔로 객체 생성
tello.connect() # 드론과 wifi로 연결된 상태에서 connect를 호출하면 sdk 통신 시작
print("Battery:", tello.get_battery(), "%") # 정상 연결이 되었는지 확인
tello.streamon() # 드론 카메라 streaming 활성화
frame_read = tello.get_frame_read() # frame을 비동기로 읽어오는 helper객체를 얻는다.
# ----- 메인 루프 -----
while True:
frame = frame_read.frame # 현재의 최신 frame을 가져온다.
if frame is None: # 만약 프레임이 준비되지 않거나, 통신 문제가 존재할때 none이다.
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # aruco 검출은 일반적으로 그레이스케일에서 수행 ( 속도/안전성에서 유리 )
corners, ids, _ = detector.detectMarkers(gray) # 마커를 검출. corners는 마커별 4개의 코너 좌표. ids는 검출된 마커의 id목록들
if ids is not None: # ids가 none이라면 maker를 하나도 못찾은 상태이다.
ids_flat = ids.flatten() # ids는 ( N,1) 형태라서 다루기 불편. 그리서 ( N, )으로 만들어줌. 그러면 [ 7,8,9]와 같은 형태가 됨
keep_idx = [i for i, mid in enumerate(ids_flat) if int(mid) in TARGET_IDS] # 7 또는 8만 남기기. enumerate를 하면 (인덱스,해당 인덱스의 값)이 나온다.
if keep_idx: # keep_idx가 비어있지 않은 경우. 즉, 목표하는 id를 하나라도 찾은 경우
filtered_corners = [corners[i] for i in keep_idx] # 목표하는 id의 인덱스만 남긴 corners를 새로 만든다
filtered_ids = ids[keep_idx] # 목표하는 id의 목표 인덱스만 골라낸다.
cv2.aruco.drawDetectedMarkers(frame, filtered_corners, filtered_ids)# 검출된 마커의 테두리와 ID를 프레임 위에 그려줌.
# *** 마커 들어올 때 멈추는 구간: 예외 잡아서 원인 출력
try:
# 아래는 marker의 2d corner의 위치를 가지고, 카메라가 marker를 어떤 방향(rvecs)으로 보고 있으며, 거리(tvecs)가 얼마나 떨어져있는지를 푼다.
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
filtered_corners, MARKER_LENGTH_M, cameraMatrix, distCoeffs)
y = 30 # 화면에 여러 줄의 텍스트를 찍기 위해서 y좌표를 누적 증가 시킨다.
for i, mid in enumerate(filtered_ids.flatten()):
# 여기부터 아래 3줄이 실질적 거리 구하는 부분
tvec = tvecs[i][0] # meters
dist = float(np.linalg.norm(tvec))
z = float(tvec[2])
cv2.putText(frame,
f"ID {int(mid)} dist={dist:.2f}m z={z:.2f}m",
(10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0,255,0), 2)
y += 30
# 축 그리기
cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], MARKER_LENGTH_M * 0.5)
except Exception as e:
print("Pose estimation error:", repr(e))
cv2.putText(frame, "Pose estimation ERROR (check console)",
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0,0,255), 2)
else:
cv2.putText(frame, "No target (7/8) detected", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
cv2.imshow("ArUco (Tello) - ID 7/8 + distance", frame)
if (cv2.waitKey(1) & 0xFF) == 27:
break
tello.streamoff()
cv2.destroyAllWindows()
8. aruco maker와의 거리와 각도를 계산
import cv2
import numpy as np
from djitellopy import Tello
# 목표 aruco 마커
TARGET_IDS = {7, 8}
# 카메라 값
cameraMatrix = np.array([
[920.0, 0.0, 480.0],
[ 0.0, 920.0, 360.0],
[ 0.0, 0.0, 1.0]
], dtype=np.float32)
distCoeffs = np.array([-0.18, 0.06, 0.000, 0.000, -0.01], dtype=np.float32).reshape(-1, 1)
MARKER_LENGTH_M = 0.20 # 20cm
# -----aruco검출기 준비----------
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(aruco_dict, params)
# -----tello 연결 및 영상 스트리밍 시작-----
tello = Tello() # 텔로 객체 생성
tello.connect() # 드론과 wifi로 연결된 상태에서 connect를 호출하면 sdk 통신 시작
print("Battery:", tello.get_battery(), "%") # 정상 연결이 되었는지 확인
tello.streamon() # 드론 카메라 streaming 활성화
frame_read = tello.get_frame_read() # frame을 비동기로 읽어오는 helper객체를 얻는다.
# ----- 메인 루프 -----
while True:
frame = frame_read.frame # 현재의 최신 frame을 가져온다.
if frame is None: # 만약 프레임이 준비되지 않거나, 통신 문제가 존재할때 none이다.
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # aruco 검출은 일반적으로 그레이스케일에서 수행 ( 속도/안전성에서 유리 )
corners, ids, _ = detector.detectMarkers(gray) # 마커를 검출. corners는 마커별 4개의 코너 좌표. ids는 검출된 마커의 id목록들
if ids is not None: # ids가 none이라면 marker를 하나도 못찾은 상태이다.
ids_flat = ids.flatten() # ids는 ( N,1) 형태라서 다루기 불편. 그리서 ( N, )으로 만들어줌. 그러면 [ 7,8,9]와 같은 형태가 됨
keep_idx = [i for i, mid in enumerate(ids_flat) if int(mid) in TARGET_IDS] # 7 또는 8만 남기기. enumerate를 하면 (인덱스,해당 인덱스의 값)이 나온다.
if keep_idx: # keep_idx가 비어있지 않은 경우. 즉, 목표하는 id를 하나라도 찾은 경우
filtered_corners = [corners[i] for i in keep_idx] # 목표하는 id의 인덱스만 남긴 corners를 새로 만든다
filtered_ids = ids[keep_idx] # 목표하는 id의 목표 인덱스만 골라낸다.
cv2.aruco.drawDetectedMarkers(frame, filtered_corners, filtered_ids)# 검출된 마커의 테두리와 ID를 프레임 위에 그려줌.
# 아래는 marker의 2d corner의 위치를 가지고, 카메라가 marker를 어떤 방향(rvecs)으로 보고 있으며, 거리(tvecs)가 얼마나 떨어져있는지를 푼다.
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
filtered_corners, MARKER_LENGTH_M, cameraMatrix, distCoeffs
)
y = 30 # 화면에 여러 줄의 텍스트를 찍기 위해서 y좌표를 누적 증가 시킨다.
for i, mid in enumerate(filtered_ids.flatten()):
# 아래 5줄이 주로 yaw와 거리를 계산하는 부분
tvec = tvecs[i][0] # [x, y, z] (meters)
x = float(tvec[0])
z = float(tvec[2])
# 마커를 정면으로 맞추기 위한 yaw 오차(도 단위)
yaw_error_deg = float(np.degrees(np.arctan2(x, z)))
dist = float(np.linalg.norm(tvec))
cv2.putText(frame,
f"ID {int(mid)} dist={dist:.2f}m yaw_err={yaw_error_deg:+.1f}deg",
(10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
y += 30
# marker위에 3d좌표축을 그려서 포즈 결과를 시각화.
cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], MARKER_LENGTH_M * 0.5)
else:
cv2.putText(frame, "No target (7/8) detected", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
cv2.imshow("ArUco (Tello) - yaw error", frame)
if (cv2.waitKey(1) & 0xFF) == 27: # ESC
break
tello.streamoff()
cv2.destroyAllWindows()
9. 비행하면서, aruco maker와의 거리와 각도를 계산
import cv2
import numpy as np
import time
from djitellopy import Tello
# 목표 aruco 마커
TARGET_IDS = {7, 8}
# aruco 마커 길이
MARKER_LENGTH_M = 0.20 # 20cm = 0.20m
AUTO_LAND_ON_EXIT = False # 원인분리용: 종료 시 자동착륙 안 함
# RC keep-alive 주기(초). 0.05s = 20Hz 정도
# tello는 rc제어 모드에서 일정 시간동안 rc명령이 오지 않는다면, 안전을 위해서 착륙을한다.
# 그래서 주기적으로 send_rc_control을 보내서, 나는 제어중이라는 것을 알리기 위한 것.
RC_KEEPALIVE_PERIOD = 0.05 # 0.05초. 초당 20번 보낸다.
# 카메라 값
cameraMatrix = np.array([
[920.0, 0.0, 480.0],
[ 0.0, 920.0, 360.0],
[ 0.0, 0.0, 1.0]
], dtype=np.float32)
# distCoeffs = 렌즈 왜곡 계수
distCoeffs = np.array([-0.18, 0.06, 0.000, 0.000, -0.01], dtype=np.float32).reshape(-1, 1)
# -----aruco검출기 준비----------
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(aruco_dict, params)
# ===== Tello 연결 =====
tello = Tello()
tello.connect()
print("Battery:", tello.get_battery(), "%")
tello.streamon()
frame_read = tello.get_frame_read()
# ------비행상태 관련-----
is_flying = False # 현재 드론이 이륙했는지를 기록
last_rc_sent = 0.0 # 마지막으로 send_rc_control을 보낸 기록을 저장히 위한 것. RC_KEEPALIVE_PERIOD만큼 시간이 지나면 다시 보내기 위한것.
print("\n[KEYS] t=takeoff, q=land, ESC=quit")
print("[INFO] AUTO_LAND_ON_EXIT =", AUTO_LAND_ON_EXIT)
print("[INFO] RC keep-alive period =", RC_KEEPALIVE_PERIOD, "sec\n")
# ----- 메인 루프 -----
try:
while True:
frame = frame_read.frame # 현재의 최신 frame을 가져온다.
if frame is None: # 만약 프레임이 준비되지 않거나, 통신 문제가 존재할때 none이다.
continue
# ----- 이륙 상태면 RC keep-alive를 주기적으로 보내서 자동착륙 방지-----
if is_flying:
now = time.time()
if (now - last_rc_sent) >= RC_KEEPALIVE_PERIOD:
# 아직 자동제어 안 하니까 0,0,0,0만 계속 보냄(호버 유지 신호)
tello.send_rc_control(0, 0, 0, 0)
last_rc_sent = now
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # aruco 검출은 일반적으로 그레이스케일에서 수행 ( 속도/안전성에서 유리 )
corners, ids, _ = detector.detectMarkers(gray) # 마커를 검출. corners는 마커별 4개의 코너 좌표. ids는 검출된 마커의 id목록들
if ids is not None: # ids가 none이라면 marker를 하나도 못찾은 상태이다.
ids_flat = ids.flatten() # ids는 ( N,1) 형태라서 다루기 불편. 그리서 ( N, )으로 만들어줌. 그러면 [ 7,8,9]와 같은 형태가 됨
keep_idx = [i for i, mid in enumerate(ids_flat) if int(mid) in TARGET_IDS] # 7 또는 8만 남기기. enumerate를 하면 (인덱스,해당 인덱스의 값)이 나온다.
if keep_idx: # keep_idx가 비어있지 않은 경우. 즉, 목표하는 id를 하나라도 찾은 경우
filtered_corners = [corners[i] for i in keep_idx] # 목표하는 id의 인덱스만 남긴 corners를 새로 만든다
filtered_ids = ids[keep_idx] # 목표하는 id의 목표 인덱스만 골라낸다.
cv2.aruco.drawDetectedMarkers(frame, filtered_corners, filtered_ids) # 검출된 마커의 테두리와 ID를 프레임 위에 그려줌.
# 아래는 marker의 2d corner의 위치를 가지고, 카메라가 marker를 어떤 방향(rvecs)으로 보고 있으며, 거리(tvecs)가 얼마나 떨어져있는지를 푼다.
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
filtered_corners, MARKER_LENGTH_M, cameraMatrix, distCoeffs
)
# 텍스트 표시 시작 y좌표 (여러 마커면 줄바꿈 위해 증가)
y = 30
# 각 마커에 대해 거리(dist)와 yaw 오차(yaw_error_deg)를 계산해서 표시
for i, mid in enumerate(filtered_ids.flatten()):
# 아래 5줄이 주로 yaw와 거리를 계산하는 부분
tvec = tvecs[i][0] # [x, y, z]
x = float(tvec[0])
z = float(tvec[2])
yaw_error_deg = float(np.degrees(np.arctan2(x, z)))
dist = float(np.linalg.norm(tvec))
cv2.putText(
frame,f"ID {int(mid)} dist={dist:.2f}m yaw_err={yaw_error_deg:+.1f}deg",
(10, y),cv2.FONT_HERSHEY_SIMPLEX,
0.8,(0, 255, 0),2)
y += 30
# marker위에 3d좌표축을 그려서 포즈 결과를 시각화.
cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs,rvecs[i], tvecs[i], MARKER_LENGTH_M * 0.5)
else:
cv2.putText(frame, "No target (7/8) detected",(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
# 상태 표시/키 안내
status = "FLYING" if is_flying else "LANDED"
# 상태를 화면에 텍스트로 표시
cv2.putText(frame,f"Status: {status} | Keys: t=takeoff q=land ESC=quit",
(10, frame.shape[0] - 20),cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
# OpenCV 창에 프레임 표시(창이 이미 있으면 프레임만 업데이트)
cv2.imshow("ArUco (Tello) - yaw debug", frame)
key = cv2.waitKey(1) & 0xFF
if key == 27: # ESC
print("[USER] ESC pressed -> quit")
break
if key == ord('t'):
if not is_flying:
print("[USER] Takeoff requested")
tello.takeoff() # 이륙명령
time.sleep(2) # 이륙후에 안정화를 대기
is_flying = True # 이륙중임을 알림.
# 이륙 직후 바로 keep-alive 시작
last_rc_sent = 0.0
else:
print("[INFO] Already flying")
if key == ord('q'):
if is_flying:
print("[USER] Land requested")
try:
tello.send_rc_control(0, 0, 0, 0) # 착륙전에 RC를 0으로 보내서 움직임을 멈추게 하라는 안전처리
except Exception:
pass
tello.land()
is_flying = False
else:
print("[INFO] Already landed")
finally:
print("\n🟡 FINALLY ENTERED | is_flying =", is_flying)
try:
cv2.destroyAllWindows()
except Exception:
pass
# 종료 시 RC 정지
try:
tello.send_rc_control(0, 0, 0, 0)
except Exception:
pass
# (옵션) 종료 시 자동 착륙
if AUTO_LAND_ON_EXIT and is_flying:
print("AUTO_LAND_ON_EXIT=True -> calling tello.land()")
try:
tello.land()
except Exception:
pass
try:
tello.streamoff()
except Exception:
pass
print("cleanup done")'코딩 및 기타' 카테고리의 다른 글
| [isaac sim] ROS2 tutorials 4 (0) | 2026.02.23 |
|---|---|
| [tello] test 10 11 12 (0) | 2026.01.30 |
| pycharm tello (0) | 2026.01.28 |
| [isaac sim] ROS2 tutorials 3 (0) | 2026.01.16 |
| [isaac sim] ROS2 tutorials 2 (0) | 2026.01.15 |