#!/usr/bin/env python3
from __future__ import annotations
import time
import math
from collections import deque
from dataclasses import dataclass
from typing import Any, Optional
import cv2
import numpy as np
from ultralytics import YOLO
# [ROS 2 추가]
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Point
try:
import pyrealsense2 as rs
except ImportError as e:
raise SystemExit(
"pyrealsense2 import failed.\n"
"현재 Python 환경에서 librealsense/pyrealsense2가 안 보입니다.\n"
f"detail: {e}"
)
# =========================
# User config
# =========================
MODEL_NAME = "yolov8n.onnx"
CONF_THRES = 0.35
COLOR_WIDTH = 640
COLOR_HEIGHT = 480
DEPTH_WIDTH = 640
DEPTH_HEIGHT = 480
FPS = 30
MIN_DEPTH_M = 0.3
MAX_DEPTH_M = 8.0
ROI_X_MARGIN_RATIO = 0.25 # 좌우 각각 25% 제외
ROI_Y_MARGIN_RATIO = 0.20 # 상하 각각 20% 제외
DEPTH_SMOOTH_WINDOW = 5
# [추가] 너무 갑작스러운 거리 변화 무시 (미터 단위)
MAX_DEPTH_JUMP_M = 1.0
PRINT_EVERY_N_FRAMES = 10
class Target3DPublisher(Node):
def __init__(self):
super().__init__('target_3d_publisher')
self.publisher_ = self.create_publisher(Point, '/target_position', 10)
self.pub_raw = self.create_publisher(Point, '/target_position_raw', 10)
def publish_target(self, norm_yaw, norm_v, z_m):
msg = Point()
msg.x = float(norm_yaw)
msg.y = float(norm_v)
msg.z = float(z_m)
self.publisher_.publish(msg)
@dataclass
class TargetState:
track_id: int
conf: float
cx: int
cy: int
bbox: tuple[int, int, int, int]
z_m: Optional[float]
x_m: Optional[float]
y_m: Optional[float]
def clamp(v: int, lo: int, hi: int) -> int:
return max(lo, min(v, hi))
def shrink_bbox(bbox, img_w, img_h, x_margin=ROI_X_MARGIN_RATIO, y_margin=ROI_Y_MARGIN_RATIO):
x1, y1, x2, y2 = bbox
w, h = max(1, x2 - x1), max(1, y2 - y1)
rx1 = clamp(int(x1 + x_margin * w), 0, img_w - 1)
rx2 = clamp(int(x2 - x_margin * w), 0, img_w - 1)
ry1 = clamp(int(y1 + y_margin * h), 0, img_h - 1)
ry2 = clamp(int(y2 - y_margin * h), 0, img_h - 1)
if rx2 <= rx1: rx1, rx2 = x1, x2
if ry2 <= ry1: ry1, ry2 = y1, y2
return rx1, ry1, rx2, ry2
def compute_person_z_median(depth_m, bbox, min_z=MIN_DEPTH_M, max_z=MAX_DEPTH_M):
img_h, img_w = depth_m.shape[:2]
rx1, ry1, rx2, ry2 = shrink_bbox(bbox, img_w, img_h)
roi = depth_m[ry1:ry2, rx1:rx2]
if roi.size == 0: return None, 0, (rx1, ry1, rx2, ry2)
valid = roi[(roi > min_z) & (roi < max_z) & np.isfinite(roi)]
if valid.size == 0: return None, 0, (rx1, ry1, rx2, ry2)
# [핵심 로직 1] 뒷배경 노이즈 커트 (Percentile 필터링)
# 상/하위 15%의 극단적인 거리값을 쳐내서 순수 몸통 거리만 추출합니다.
p15 = np.percentile(valid, 15)
p85 = np.percentile(valid, 85)
valid_filtered = valid[(valid >= p15) & (valid <= p85)]
if valid_filtered.size == 0:
return float(np.median(valid)), int(valid.size), (rx1, ry1, rx2, ry2)
return float(np.median(valid_filtered)), int(valid_filtered.size), (rx1, ry1, rx2, ry2)
def pixel_to_camera_xyz(cx, cy, z_m, intr):
x_m = (cx - intr.ppx) / intr.fx * z_m
y_m = (cy - intr.ppy) / intr.fy * z_m
return float(x_m), float(y_m), float(z_m)
def main(args=None):
rclpy.init(args=args)
target_node = Target3DPublisher()
print("[INFO] starting RealSense pipeline...")
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, DEPTH_WIDTH, DEPTH_HEIGHT, rs.format.z16, FPS)
config.enable_stream(rs.stream.color, COLOR_WIDTH, COLOR_HEIGHT, rs.format.bgr8, FPS)
profile = pipeline.start(config)
align = rs.align(rs.stream.color)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# ---------------------------------------------------------
# [핵심 로직 2] RealSense Post-Processing Filters 초기화
# ---------------------------------------------------------
spatial_filter = rs.spatial_filter()
temporal_filter = rs.temporal_filter()
hole_filling_filter = rs.hole_filling_filter()
# 필터 강도 약간 세팅 (원하시면 수정 가능)
spatial_filter.set_option(rs.option.filter_smooth_alpha, 0.5)
spatial_filter.set_option(rs.option.filter_smooth_delta, 20)
model = YOLO(MODEL_NAME)
for _ in range(10): pipeline.wait_for_frames() # warmup
prev_t = time.time()
frame_idx = 0
last_track_id: Optional[int] = None
z_history: deque[float] = deque(maxlen=DEPTH_SMOOTH_WINDOW)
try:
while True:
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
if not depth_frame or not color_frame: continue
# [하드웨어 필터 적용] 깊이 맵 노이즈 다림질하기!
depth_frame = spatial_filter.process(depth_frame)
depth_frame = temporal_filter.process(depth_frame)
depth_frame = hole_filling_filter.process(depth_frame)
color_image = np.asanyarray(color_frame.get_data())
depth_raw = np.asanyarray(depth_frame.get_data()).astype(np.float32)
depth_m = depth_raw * depth_scale
h, w = color_image.shape[:2]
img_cx, img_cy = w // 2, h // 2
color_intr = color_frame.profile.as_video_stream_profile().intrinsics
results = model.track(source=color_image, persist=True, classes=[0], conf=CONF_THRES, imgsz=320, tracker="bytetrack.yaml", verbose=False)
detections = []
if results and len(results) > 0 and results[0].boxes is not None:
boxes = results[0].boxes
xyxy = boxes.xyxy.cpu().numpy() if boxes.xyxy is not None else []
confs = boxes.conf.cpu().numpy() if boxes.conf is not None else []
ids = boxes.id.cpu().numpy().astype(int) if boxes.id is not None else None
clss = boxes.cls.cpu().numpy().astype(int) if boxes.cls is not None else []
for i in range(len(xyxy)):
if (int(clss[i]) if len(clss) > i else -1) != 0: continue
x1, y1, x2, y2 = xyxy[i]
detections.append({
"track_id": int(ids[i]) if ids is not None and len(ids) > i else -1,
"conf": float(confs[i]) if len(confs) > i else 0.0,
"cx": (clamp(int(x1), 0, w - 1) + clamp(int(x2), 0, w - 1)) // 2,
"cy": (clamp(int(y1), 0, h - 1) + clamp(int(y2), 0, h - 1)) // 2,
"bbox": (clamp(int(x1), 0, w - 1), clamp(int(y1), 0, h - 1), clamp(int(x2), 0, w - 1), clamp(int(y2), 0, h - 1))
})
# 사람 선택 로직
primary = None
if len(detections) > 0:
if last_track_id is not None:
for det in detections:
if det["track_id"] == last_track_id:
primary = det
break
if primary is None:
primary = min(detections, key=lambda d: (d["cx"] - img_cx) ** 2 + (d["cy"] - img_cy) ** 2)
annotated = color_image.copy()
target_state = None
valid_depth_count = 0
for det in detections:
x1, y1, x2, y2 = det["bbox"]
is_p = primary and det["track_id"] == primary["track_id"]
color, thick = ((0, 0, 255), 3) if is_p else ((0, 255, 0), 2)
cv2.rectangle(annotated, (x1, y1), (x2, y2), color, thick)
if primary is not None:
track_id, conf, cx, cy, bbox = primary["track_id"], primary["conf"], primary["cx"], primary["cy"], primary["bbox"]
if track_id != last_track_id:
z_history.clear()
z_median, valid_depth_count, roi_bbox = compute_person_z_median(depth_m, bbox)
z_smooth, x_m, y_m = None, None, None
if z_median is not None:
# [핵심 로직 3] 순간 이동(Jump) 방어 로직
if len(z_history) > 0:
prev_z = np.median(np.array(z_history, dtype=np.float32))
if abs(z_median - prev_z) > MAX_DEPTH_JUMP_M:
z_median = prev_z # 값이 너무 튀면 버리고 이전 값 유지!
z_history.append(z_median)
z_smooth = float(np.median(np.array(z_history, dtype=np.float32)))
x_m, y_m, _ = pixel_to_camera_xyz(cx, cy, z_smooth, color_intr)
target_state = TargetState(track_id, conf, cx, cy, bbox, z_smooth, x_m, y_m)
last_track_id = track_id
cv2.line(annotated, (img_cx, img_cy), (cx, cy), (255, 0, 0), 2)
if z_smooth is not None:
# 제어용 정규화 계산
norm_yaw = (cx - img_cx) / color_intr.fx
norm_v = (cy - img_cy) / color_intr.fy
target_node.publish_target(norm_yaw, norm_v, z_smooth)
cv2.putText(annotated, f"Dist: {z_smooth:.2f} m", (bbox[0], max(10, bbox[1] - 30)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
if roi_bbox is not None:
rx1, ry1, rx2, ry2 = roi_bbox
cv2.rectangle(annotated, (rx1, ry1), (rx2, ry2), (255, 255, 0), 2)
now = time.time()
frame_idx += 1
cv2.putText(annotated, f"FPS: {1.0 / max(now - prev_t, 1e-6):.1f}", (20, 35), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2)
prev_t = now
rclpy.spin_once(target_node, timeout_sec=0.001)
cv2.imshow("depth_fusion", annotated)
if cv2.waitKey(1) & 0xFF in (27, ord("q")): break
finally:
pipeline.stop()
cv2.destroyAllWindows()
target_node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
mai
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Point
# 1. TwistStamped로 변경!
from geometry_msgs.msg import TwistStamped
class MavrosVerifyController(Node):
def __init__(self):
super().__init__('mavros_verify_controller')
self.target_sub = self.create_subscription(
Point,
'/target_position',
self.target_callback,
10
)
# 2. 퍼블리셔를 TwistStamped와 cmd_vel 토픽으로 변경
self.cmd_vel_pub = self.create_publisher(
TwistStamped,
'/mavros/setpoint_velocity/cmd_vel',
10
)
self.timer = self.create_timer(0.05, self.timer_callback)
self.is_hovering = True
self.current_dist = 0.0
self.forward_speed = 1.5
self.distance_threshold = 1.2
self.get_logger().info("MAVROS Verify Controller (TwistStamped) Started!")
def target_callback(self, msg):
self.current_dist = msg.z
if self.current_dist < self.distance_threshold:
if not self.is_hovering:
self.get_logger().info(f"🛑 [STOP] Depth({self.current_dist:.2f}m) < {self.distance_threshold}m -> HOVERING!")
self.is_hovering = True
else:
if self.is_hovering:
self.get_logger().info(f"🚀 [MOVE] Depth({self.current_dist:.2f}m) >= {self.distance_threshold}m -> MOVING FORWARD!")
self.is_hovering = False
def timer_callback(self):
# 3. 메시지에 시간(stamp)과 기준 좌표계(frame_id)를 박아줍니다.
twist_msg = TwistStamped()
twist_msg.header.stamp = self.get_clock().now().to_msg()
twist_msg.header.frame_id = "map" # 야매임
if self.is_hovering:
twist_msg.twist.linear.x = 0.0
twist_msg.twist.linear.y = 0.0
twist_msg.twist.linear.z = 0.0
else:
twist_msg.twist.linear.x = self.forward_speed
twist_msg.twist.linear.y = 0.0
twist_msg.twist.linear.z = 0.0
twist_msg.twist.angular.x = 0.0
twist_msg.twist.angular.y = 0.0
twist_msg.twist.angular.z = 0.0
self.cmd_vel_pub.publish(twist_msg)
def main(args=None):
rclpy.init(args=args)
verify_node = MavrosVerifyController()
try:
rclpy.spin(verify_node)
except KeyboardInterrupt:
verify_node.get_logger().info("Shutting down...")
finally:
verify_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Point
# 1. TwistStamped로 변경!
from geometry_msgs.msg import TwistStamped
class MavrosVerifyController(Node):
def __init__(self):
super().__init__('mavros_verify_controller')
self.target_sub = self.create_subscription(
Point,
'/target_position',
self.target_callback,
10
)
# 2. 퍼블리셔를 TwistStamped와 cmd_vel 토픽으로 변경
self.cmd_vel_pub = self.create_publisher(
TwistStamped,
'/mavros/setpoint_velocity/cmd_vel',
10
)
self.timer = self.create_timer(0.05, self.timer_callback)
self.is_hovering = True
self.current_dist = 0.0
self.forward_speed = 1.5
self.distance_threshold = 1.2
self.get_logger().info("MAVROS Verify Controller (TwistStamped) Started!")
def target_callback(self, msg):
self.current_dist = msg.z
if self.current_dist < self.distance_threshold:
if not self.is_hovering:
self.get_logger().info(f"🛑 [STOP] Depth({self.current_dist:.2f}m) < {self.distance_threshold}m -> HOVERING!")
self.is_hovering = True
else:
if self.is_hovering:
self.get_logger().info(f"🚀 [MOVE] Depth({self.current_dist:.2f}m) >= {self.distance_threshold}m -> MOVING FORWARD!")
self.is_hovering = False
def timer_callback(self):
# 3. 메시지에 시간(stamp)과 기준 좌표계(frame_id)를 박아줍니다.
twist_msg = TwistStamped()
twist_msg.header.stamp = self.get_clock().now().to_msg()
twist_msg.header.frame_id = "map" # 야매임
if self.is_hovering:
twist_msg.twist.linear.x = 0.0
twist_msg.twist.linear.y = 0.0
twist_msg.twist.linear.z = 0.0
else:
twist_msg.twist.linear.x = self.forward_speed
twist_msg.twist.linear.y = 0.0
twist_msg.twist.linear.z = 0.0
twist_msg.twist.angular.x = 0.0
twist_msg.twist.angular.y = 0.0
twist_msg.twist.angular.z = 0.0
self.cmd_vel_pub.publish(twist_msg)
def main(args=None):
rclpy.init(args=args)
verify_node = MavrosVerifyController()
try:
rclpy.spin(verify_node)
except KeyboardInterrupt:
verify_node.get_logger().info("Shutting down...")
finally:
verify_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
(다만 현재 코드의 53번째 줄을 보면 map 기준으로 드론이 움직이도록 설정했다. 현재는 ros2가 topic을 잘 받고 잘 전달하는지에만 초점을 맞췄기 때문에 야매성으로 코드를 짰다. 따라서 적어도 사람추적 제어를 활용할 때에는 해당 코드 부분을 무조건 수정해야 한다.)
실행 방법:
ros2 run mavros mavros_node --ros-args -p fcu_url:=udp://:14540@