sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add websocket to cameracontroller for video stream

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-02 11:11:38 +02:00
parent d1b51dd7f4
commit 5031dd9aa8
2 changed files with 82 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
src/api_communication/api_communication/api_listener.py
View File

@@ -22,6 +22,8 @@ import cv2
RES_4K_H = 3496
RES_4K_W = 4656
# TODO change video to be handled by camera controller through websocket with different port
class RequestCommand(Enum):
GET_COMMANDS_TYPES = -1 # to get the available commands and types
TAKEOFF = 0
@@ -84,9 +86,6 @@ class ApiListener(Node):
target=self.handle_responses, daemon=True)
self.response_thread.start()
self.video_trhead = threading.Thread(target=self.send_video)
self.video_trhead.start()
self.event_loop = None
def drone_status_callback(self, msg):
@@ -97,24 +96,24 @@ class ApiListener(Node):
self.status_data['control_mode'] = msg.control_mode
self.status_data['route_setpoint'] = msg.route_setpoint
def send_video(self):
self.get_logger().info('Starting video thread')
vid = cv2.VideoCapture(0)
# def send_video(self):
# self.get_logger().info('Starting video thread')
# vid = cv2.VideoCapture(0)
# vid.set(cv2.CAP_PROP_FRAME_WIDTH, RES_4K_W)
# vid.set(cv2.CAP_PROP_FRAME_HEIGHT, RES_4K_H)
while True:
try:
while vid.isOpened():
img,frame = vid.read()
frame = cv2.resize(frame,(640,480))
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 65]
man = cv2.imencode('.jpg', frame, encode_param)[1]
self.get_logger().info('Sending video')
self.message_queue.append(man.tobytes())
# # vid.set(cv2.CAP_PROP_FRAME_WIDTH, RES_4K_W)
# # vid.set(cv2.CAP_PROP_FRAME_HEIGHT, RES_4K_H)
# while True:
# try:
# while vid.isOpened():
# img,frame = vid.read()
# frame = cv2.resize(frame,(640,480))
# encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 65]
# man = cv2.imencode('.jpg', frame, encode_param)[1]
# self.get_logger().info('Sending video')
# self.message_queue.append(man.tobytes())
except Exception as e:
self.get_logger().error('Something went wrong while reading video: ' + str(e))
# except Exception as e:
# self.get_logger().error('Something went wrong while reading video: ' + str(e))
async def publish_message(self, message):
# self.get_logger().info(f'Publishing message: {message}')

64
src/camera/camera/camera_controller.py
View File

@@ -5,10 +5,17 @@ from sensor_msgs.msg import Image
import os
from datetime import datetime
import asyncio
import websockets.server
import threading
import cv2
#resolution of the camera
RES_4K_H = 3496
RES_4K_W = 4656
#TODO change to serve video stream through websockets connection
class CameraController(Node):
def __init__(self):
super().__init__('camera_controller')
@@ -17,6 +24,18 @@ class CameraController(Node):
self.srv = self.create_service(
TakePicture, 'drone/picture', self.take_picture_callback)
self.websocket = None
self.server = None
self.event_loop = None
self.video_thread = threading.Thread(target=self.handle_video_connection)
self.video_thread.start()
self.websocket_thread = threading.Thread(target=self.start_websocket_server)
self.websocket_thread.start()
def take_picture_callback(self, request, response):
if (request.input_name == "default"):
self.get_logger().info("Taking picture with default filename")
@@ -30,6 +49,51 @@ class CameraController(Node):
return response
async def handle_video_connection(self):
asyncio.ensure_future(self.send_video(),loop=self.event_loop)
async def send_video(self):
self.get_logger().info('Starting video thread')
vid = cv2.VideoCapture(0)
# vid.set(cv2.CAP_PROP_FRAME_WIDTH, RES_4K_W)
# vid.set(cv2.CAP_PROP_FRAME_HEIGHT, RES_4K_H)
while True:
try:
while vid.isOpened():
if self.websocket is not None:
img,frame = vid.read()
frame = cv2.resize(frame,(640,480))
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 65]
man = cv2.imencode('.jpg', frame, encode_param)[1]
self.get_logger().info('Sending video')
await self.websocket.send(man.tobytes())
except Exception as e:
self.get_logger().error('Something went wrong while reading and sending video: ' + str(e))
async def start_websocket_server(self):
self.get_logger().info('Starting websocket server for video')
self.event_loop = asyncio.get_event_loop()
self.server = await websockets.server.serve(self.websocket_handler, '0.0.0.0', 9002)
await self.server.wait_closed()
async def websocket_handler(self,websocket):
self.get_logger().info('New connection')
self.websocket = websocket
try:
async for message in websocket:
self.get_logger().info(f"Received message: {message}")
except websockets.exceptions.ConnectionClosed:
self.get_logger().info('Connection closed')
self.websocket = None
except Exception as e:
self.get_logger().error('Something went wrong!')
self.get_logger().error(str(e))
self.websocket = None
def main(args=None):
rclpy.init(args=args)