[Drone] realsense camera를 사용하여 서버로 실시간 스트리밍

0. ssh -i ~/.ssh/id_ed25519 -Y quad@192.167.1.3

 

1.

ros2 launch mavros px4.launch   fcu_url:=serial:///dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0:921600   fcu_protocol:=v2.0

 

 

2.

ros2 launch realsense2_camera rs_launch.py   align_depth:=true   enable_gyro:=true enable_accel:=true   unite_imu_method:=2   enable_sync:=true   publish_tf:=true

 

3.

mkdir video_stream_ws
cd video_stream_ws

mkdir src
cd src


ros2 pkg create video_streamer --build-type ament_python --dependencies rclpy sensor_msgs

 

 

4. vim rtsp_upload.py 후에 아래 내용 작성

• chmod +x ~/video_stream_ws/src/video_streamer/video_streamer/image_upload.py

#!/usr/bin/env python3
import base64 # 바이너리(JPEG bytes)를 텍스트(base64)로 인코딩 하기 위한것
import threading # 콜백(ros수신)과 전송(http업로드)을 분리... 스레드/락을 사용하려고
import requests # http post를 보내기 위한 라이브러리
import cv2

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data

from sensor_msgs.msg import Image # ros2 카메라 이미지의 message typoe
from cv_bridge import CvBridge # ros <--> opencv bridge


# 이 노드는 image_topic을 구독해서, 별도의 sender 스레드가 이미지 프레임을 JPEG+base64로 만들어 HTTP POST로 서버에 업로드
class ImageUploadNode(Node):
    def __init__(self):
        super().__init__('image_uploader') # 노드 초기화

        # 1) 파라미터 설정
        # ---- params ----
        self.declare_parameter('image_topic', '/camera/camera/color/image_raw')
        self.declare_parameter('server_url', 'http://40.82.157.101:8088/upload_frame')
        self.declare_parameter('cam_id', 0)

        self.declare_parameter('jpeg_quality', 80) # open cv의 jpeg인코딩 품질 설정 파라미터. 높을수록 좋아요
        self.declare_parameter('post_timeout', 1.0) # http post 요청

        # 실제 파라미터 값 읽기
        self.image_topic = self.get_parameter('image_topic').value
        self.server_url = self.get_parameter('server_url').value
        self.cam_id = int(self.get_parameter('cam_id').value)

        self.jpeg_quality = int(self.get_parameter('jpeg_quality').value)
        self.post_timeout = float(self.get_parameter('post_timeout').value)

        # 2)ros image --> open cv 브릿지
        # ---- ROS image ----
        self.bridge = CvBridge()

        # 3) 최신 프레임만 저장 + sender 스레드에게 알림
        self.latest_frame = None # 콜백에서 들어온 가장 최신 프레임 1장만 저장. 이렇게 하여 큐를 쌓지 않고 최신만 유지해서 지연을 최소화
        self.lock = threading.Lock() # 콜백 스레드(image_cb)와 sender 스레드가 동시에 latest_frame에 접근하는 것을 막기 위한 mutex
        self.new_frame_evt = threading.Event() # sender 스레드에게 "새 프레임이 들어왔다"는 신호를 주는 이벤트
        self.stop_evt = threading.Event() # 노드 종룟시 sender loop종료를 유도하는 플래그 이벤드

        # 4) HTTP 세션 준비
        self.sess = requests.Session()

        # 5) sender thread 시작
        self.sender_th = threading.Thread(target=self.sender_loop, daemon=True) # 메인 스레드 종료시, 데몬 스레드는 자동 종료될 수 있음
        self.sender_th.start()

        # 6) subscriber (센서 QoS 추천)
        self.sub = self.create_subscription(
            Image,# 메시지 타입
            self.image_topic, # 구독할 토픽 이름
            self.image_cb, # 콜백 함수 
            qos_profile_sensor_data # qos 
        )

        self.get_logger().info(f"Subscribing: {self.image_topic}")
        self.get_logger().info(f"Uploading to: {self.server_url} cam_id={self.cam_id}")

    # ros subscriber 콜백
    # 이미지 변환을 수행 
    def image_cb(self, msg: Image):
        # ROS Image -> cv2 frame (bgr8)
        try:
            frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        except Exception as e:
            self.get_logger().warn(f"cv_bridge convert failed: {e}")
            return

        # 최신 프레임만 업데이트 (복사는 sender에서 함)
        with self.lock:
            self.latest_frame = frame
        self.new_frame_evt.set()

    # 이는 별도의 thread에서 실행되는 upload loop
    # 1. 이벤트 대기 
    # 2. 최신 프레임 가져오기
    # 3. JPEG 인코딩 --> base64 --> HTTP POST
    # ( 콜백을 막지 않기 위해서, 무거운 작업을 여기에서 동작하게 )
    def sender_loop(self):
        # rclpy.ok는 ros2 python라이브러리가 정상 동작하는지 알려주는 함수. rclpy.shutdown이나 crtl+c로 종료 흐름 들어감
        # stop_evt는 threading.Event()로 만든 스레드 간 종료 신호이다. 그래서 is_set()이 True인지 False인지 확인함.
        while rclpy.ok() and not self.stop_evt.is_set():
            self.new_frame_evt.wait() # new_frame_evt가 set이 될때까지 블로킹.
            self.new_frame_evt.clear()  # new_frame_evt.wait()에서 set이 true가 된다면, 바로 다음꺼를 받을수있게 clear를 해주는거

            # 최신 프레임을 가져옴. lock을 걸어줌 
            with self.lock:
                frame = self.latest_frame
                self.latest_frame = None  # 중복 전송 줄이기. 즉, 같은 프레임이 여러번 보내지는거 방지 or None이면 보낼게 없다는걸 의미 

            if frame is None:
                continue

            # 인코딩+전송은 여기서(콜백 막지 않게)
            ok, buf = cv2.imencode(".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), self.jpeg_quality])
            if not ok:
                continue

            payload = {
                "cam_id": int(self.cam_id),
                "image": base64.b64encode(buf).decode("utf-8"),
            }

            try:
                r = self.sess.post(self.server_url, json=payload, timeout=self.post_timeout)
                if r.status_code != 200:
                    self.get_logger().warn(f"server status={r.status_code} body={r.text[:200]}")
            except Exception as e:
                self.get_logger().warn(f"post error: {e}")

    def destroy_node(self):
        self.stop_evt.set()
        try:
            self.new_frame_evt.set()
        except Exception:
            pass
        super().destroy_node()


def main():
    rclpy.init()
    node = ImageUploadNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

 

 

5. vim setup.py

 'image_upload = video_streamer.image_upload:main',

 

 

6.

cd ~/video_stream_ws
colcon build --symlink-install
source install/setup.bash

 

 

7.

ros2 run video_streamer image_upload --ros-args \
  -p image_topic:=/camera/camera/color/image_raw \
  -p server_url:=http://ipAddr:port/upload_frame \
  -p cam_id:=0

 

video_stream_ws.zip

0.12MB