|
|
@@ -0,0 +1,87 @@
|
|
|
+## License: Apache 2.0. See LICENSE file in root directory.
|
|
|
+## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
|
|
|
+
|
|
|
+###############################################
|
|
|
+## Open CV and Numpy integration ##
|
|
|
+###############################################
|
|
|
+
|
|
|
+import pyrealsense2 as rs
|
|
|
+import numpy as np
|
|
|
+import cv2
|
|
|
+import socket
|
|
|
+import math
|
|
|
+import pickle
|
|
|
+import sys
|
|
|
+import time
|
|
|
+import datetime
|
|
|
+
|
|
|
+max_length = 65000
|
|
|
+host = "192.168.50.158"
|
|
|
+port = 5000
|
|
|
+
|
|
|
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
|
+pipeline = rs.pipeline()
|
|
|
+config = rs.config()
|
|
|
+config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
|
|
|
+config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
|
|
|
+
|
|
|
+pipeline.start(config)
|
|
|
+
|
|
|
+try:
|
|
|
+ while True:
|
|
|
+ frames = pipeline.wait_for_frames()
|
|
|
+ depth_frame = frames.get_depth_frame()
|
|
|
+ color_frame = frames.get_color_frame()
|
|
|
+ if not depth_frame or not color_frame:
|
|
|
+ continue
|
|
|
+
|
|
|
+ depth_image = np.asanyarray(depth_frame.get_data())
|
|
|
+ color_image = np.asanyarray(color_frame.get_data())
|
|
|
+
|
|
|
+ depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
|
|
|
+
|
|
|
+ text_depth = "depth value of point is " + str(
|
|
|
+ np.round(depth_frame.get_distance(320, 240), 4)) + "meter(s)"
|
|
|
+ color_image = cv2.circle(color_image, (320, 240), 1, (0, 255, 255), 10)
|
|
|
+ color_image = cv2.putText(color_image, text_depth, (10, 20), cv2.FONT_HERSHEY_PLAIN, 2, (0, 0, 255), 1,
|
|
|
+ cv2.LINE_AA)
|
|
|
+
|
|
|
+ images = np.hstack((color_image, depth_colormap))
|
|
|
+ images = cv2.resize(images, (640,320),interpolation=cv2.INTER_AREA)
|
|
|
+ retval, buffer = cv2.imencode(".jpg", images)
|
|
|
+
|
|
|
+ if retval:
|
|
|
+ # convert to byte array
|
|
|
+ buffer = buffer.tobytes()
|
|
|
+ # get size of the frame
|
|
|
+ buffer_size = len(buffer)
|
|
|
+
|
|
|
+ num_of_packs = 1
|
|
|
+ if buffer_size > max_length:
|
|
|
+ num_of_packs = math.ceil(buffer_size / max_length)
|
|
|
+
|
|
|
+ frame_info = {"packs": num_of_packs}
|
|
|
+ # send the number of packs to be expected
|
|
|
+ print("Number of packs:", num_of_packs)
|
|
|
+ sock.sendto(pickle.dumps(frame_info), (host, port))
|
|
|
+
|
|
|
+ left = 0
|
|
|
+ right = max_length
|
|
|
+
|
|
|
+ for i in range(num_of_packs):
|
|
|
+ print("left:", left)
|
|
|
+ print("right:", right)
|
|
|
+
|
|
|
+ # truncate data to send
|
|
|
+ data = buffer[left:right]
|
|
|
+ left = right
|
|
|
+ right += max_length
|
|
|
+
|
|
|
+ # send the frames accordingly
|
|
|
+ sock.sendto(data, (host, port))
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+finally:
|
|
|
+ pipeline.stop()
|
