class RealsenseRead(Thread):
""" Realsense D415 Reader Class
Realsense D415 reader functions for EMAR Mini Emergency Assistance Robot.
"""
def __init__(self):
""" Initializes the class. """
super(RealsenseRead, self).__init__()
self.Helpers = Helpers("Realsense D415 Reader")
self.colorizer = rs.colorizer()
# OpenCV fonts
self.font = cv2.FONT_HERSHEY_SIMPLEX
self.black = (0, 0, 0)
self.green = (0, 255, 0)
self.white = (255, 255, 255)
# Starts the Realsense module
self.Realsense = Realsense()
# Connects to the Realsense camera
self.profile = self.Realsense.connect()
# Starts the socket module
self.Socket = Socket("Realsense D415 Reader")
# Sets up the object detection model
self.Model = Model()
self.Helpers.logger.info(
"Realsense D415 Reader Class initialization complete.")
def run(self):
""" Runs the module. """
time1 = 0
time2 = 0
fc = 0
dc = 0
fps = ""
dfps = ""
# Starts the socket server
soc = self.Socket.connect(
self.Helpers.confs["EMAR"]["ip"],
self.Helpers.confs["Realsense"]["socket"]["port"])
try:
while True:
t1 = time.perf_counter()
# Wait for depth and color frames
frames = self.Realsense.pipeline.wait_for_frames()
# Get color frames
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
# Get infrared frames
#ir1_frame = frames.get_infrared_frame(1)
#ir2_frame = frames.get_infrared_frame(2)
if not depth_frame or not color_frame:
#if not not ir1_frame or not ir2_frame:
self.Helpers.logger.info(
"Realsense D415 streams not ready, continuing.")
continue
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
#ir1_image = np.asanyarray(self.colorizer.colorize(ir1_frame).get_data())
#ir2_image = np.asanyarray(self.colorizer.colorize(ir2_frame).get_data())
colorized_depth = np.asanyarray(
self.colorizer.colorize(depth_frame).get_data())
# Sets the blob and gets the detections from forward pass
self.Model.setBlob(color_image)
detections = self.Model.forwardPass()
# Gets width, height and crop value for frame
width, height = self.Model.getDims(color_image)
# Writes header to frame
cv2.putText(color_image, "EMAR Mini Color Stream", (30, 50),
self.font, 0.7, self.black, 2, cv2.LINE_AA)
# Writes date to frame
cv2.putText(color_image, str(datetime.now()), (30, 80),
self.font, 0.5, self.black, 2, cv2.LINE_AA)
# Writes header to frame
cv2.putText(colorized_depth, "EMAR Mini Depth Stream",
(30, 50), self.font, 0.7, self.white, 2,
cv2.LINE_AA)
# Writes date to frame
cv2.putText(colorized_depth, str(datetime.now()), (30, 80),
self.font, 0.5, self.white, 2, cv2.LINE_AA)
for i in range(detections.shape[2]):
# Detection FPS
if i == 0:
dc += 1
# Gets and checks confidence of detection
confidence = detections[0, 0, i, 2]
if (confidence <=
self.Helpers.confs["MobileNetSSD"]["threshold"]):
continue
# Gets class of detection
class_id = int(detections[0, 0, i, 1])
# Gets bounding box of detection
box_blx = int(detections[0, 0, i, 3] * width)
box_bly = int(detections[0, 0, i, 4] * height)
box_trx = int(detections[0, 0, i, 5] * width)
box_try = int(detections[0, 0, i, 6] * height)
# Gets the meters from the depth frame
meters = depth_frame.as_depth_frame().get_distance(
box_blx + int((box_trx - box_blx) / 2), box_bly + int(
(box_try - box_bly) / 2))
# Prepares label text
if meters != 0.00:
label_text = self.Helpers.confs["MobileNetSSD"][
"classes"][int(class_id)] + " (" + str(
confidence) + "%)" + " {:.2f}".format(
meters) + "m"
else:
label_text = self.Helpers.confs["MobileNetSSD"][
"classes"][int(class_id)] + " (" + str(
confidence) + "%)"
cv2.rectangle(color_image, (box_blx, box_bly),
(box_trx, box_try), self.green, 1)
# Positions and writes the label
label_size = cv2.getTextSize(label_text, self.font, 0.5,
1)[0]
label_left = box_blx
label_top = box_bly - label_size[1]
if (label_top < 1):
label_top = 1
label_bottom = label_top + label_size[1]
cv2.putText(color_image, label_text,
(label_left, label_bottom), self.font, 0.5,
self.green, 1)
# Writes FPS and Detection FPS
cv2.putText(color_image, fps, (width - 170, 50), self.font,
0.5, self.black, 1, cv2.LINE_AA)
cv2.putText(color_image, dfps, (width - 170, 65), self.font,
0.5, self.black, 1, cv2.LINE_AA)
# Combine the color_image and colorized_depth frames together:
frame = np.hstack((color_image, colorized_depth))
# Combine the ir1_image and ir2_image frames together:
#frame = np.hstack((ir1_image, ir2_image))
# Streams the modified frame to the socket server
encoded, buffer = cv2.imencode('.jpg', frame)
soc.send(base64.b64encode(buffer))
# FPS calculation
fc += 1
if fc >= 15:
fps = "Stream: {:.1f} FPS".format(time1 / 15)
dfps = "Detection: {:.1f} FPS".format(dc / time2)
fc = 0
dc = 0
time1 = 0
time2 = 0
t2 = time.perf_counter()
elapsedTime = t2 - t1
time1 += 1 / elapsedTime
time2 += elapsedTime
finally:
# Stop streaming
self.Realsense.pipeline.stop()
class RealsenseRead(Thread):
""" Realsense D415 Reader Class
Realsense D415 reader functions for EMAR Mini Emergency Assistance Robot.
"""
def __init__(self):
""" Initializes the class. """
super(RealsenseRead, self).__init__()
self.Helpers = Helpers("Realsense D415 Reader")
self.colorizer = rs.colorizer()
# OpenCV fonts
self.font = cv2.FONT_HERSHEY_SIMPLEX
self.black = (0, 0, 0)
self.green = (0, 255, 0)
# Starts the Realsense module
self.Realsense = Realsense()
# Connects to the Realsense camera
self.profile = self.Realsense.connect()
# Starts the socket module
self.Socket = Socket("Realsense D415 Reader")
# Sets up the object detection model
self.Model = Model()
self.Helpers.logger.info("Realsense D415 Reader Class initialization complete.")
def run(self):
""" Runs the module. """
t1 = 0
t2 = 0
fc = 0
dc = 0
fps = ""
dfps = ""
# Starts the socket server
soc = self.Socket.connect(self.Helpers.confs["EMAR"]["ip"],
self.Helpers.confs["Realsense"]["socket"]["port"])
try:
while True:
t1 = time.perf_counter()
# Wait for a coherent pair of frames: depth and color
frames = self.Realsense.pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
#ir1_frame = frames.get_infrared_frame(1)
#ir2_frame = frames.get_infrared_frame(2)
if not depth_frame or not color_frame:
#if not not ir1_frame or not ir2_frame:
self.Helpers.logger.info("Realsense D415 streams not ready, continuing.")
continue
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
#ir1_image = np.asanyarray(self.colorizer.colorize(ir1_frame).get_data())
#ir2_image = np.asanyarray(self.colorizer.colorize(ir2_frame).get_data())
colorized_depth = np.asanyarray(self.colorizer.colorize(depth_frame).get_data())
width, height = self.Model.getDims(color_image)
self.Model.setBlob(color_image)
detections = self.Model.forwardPass()
# Writes header to frame
cv2.putText(color_image, "EMAR Mini Color Stream", (30,50), self.font,
0.7, self.black, 2, cv2.LINE_AA)
# Writes date to frame
cv2.putText(color_image, str(datetime.now()), (30,80), self.font,
0.5, self.black, 2, cv2.LINE_AA)
# Writes header to frame
cv2.putText(colorized_depth, "EMAR Mini Depth Stream", (30,50), self.font,
0.7, self.black, 2, cv2.LINE_AA)
# Writes date to frame
cv2.putText(colorized_depth, str(datetime.now()), (30,80), self.font,
0.5, self.black, 2, cv2.LINE_AA)
for i in range(100):
bi = i * 7
if i == 0:
dc += 1
# Object location
x1 = max(0, int(detections[bi + 3] * height))
y1 = max(0, int(detections[bi + 4] * width))
x2 = min(height, int(detections[bi + 5] * height))
y2 = min(width, int(detections[bi + 6] * width))
overlay = detections[bi:bi + 7]
bi = 0
class_id = overlay[bi + 1]
percentage = int(overlay[bi + 2] * 100)
if (percentage <= self.Helpers.confs["MobileNetSSD"]["threshold"]):
continue
# Calculates box coordinates
box_left = int(overlay[bi + 3] * width)
box_top = int(overlay[bi + 4] * height)
box_right = int(overlay[bi + 5] * width)
box_bottom = int(overlay[bi + 6] * height)
# Gets the meters from the depth frame
meters = depth_frame.as_depth_frame().get_distance(box_left+int((box_right-box_left)/2),
box_top+int((box_bottom-box_top)/2))
if meters != 0.00:
label_text = self.Helpers.confs["MobileNetSSD"]["classes"][int(class_id)] + " (" + str(percentage) + "%)"+ " {:.2f}".format(meters) + "m"
else:
label_text = self.Helpers.confs["MobileNetSSD"]["classes"][int(class_id)] + " (" + str(percentage) + "%)"
cv2.rectangle(color_image, (box_left, box_top), (box_right, box_bottom),
self.green, 1)
# Positions and writes the label
label_size = cv2.getTextSize(label_text, self.font, 0.5, 1)[0]
label_left = box_left
label_top = box_top - label_size[1]
if (label_top < 1):
label_top = 1
label_right = label_left + label_size[0]
label_bottom = label_top + label_size[1]
cv2.putText(color_image, label_text, (label_left, label_bottom), self.font, 0.5,
self.green, 1)
# Writes FPS and Detection Frames Per Second
cv2.putText(color_image, fps, (width-170,20), self.font, 0.5,
self.black, 1, cv2.LINE_AA)
cv2.putText(color_image, dfps, (width-170,35), self.font, 0.5,
self.black, 1, cv2.LINE_AA)
# Combine the color_image and colorized_depth frames together:
frame = np.hstack((color_image, colorized_depth))
# Combine the ir1_image and ir2_image frames together:
#frame = np.hstack((ir1_image, ir2_image))
# Streams the modified frame to the socket server
encoded, buffer = cv2.imencode('.jpg', frame)
soc.send(base64.b64encode(buffer))
# FPS calculation
fc += 1
if fc >= 15:
fps = "Stream: {:.1f} FPS".format(t1/15)
dfps = "Detection: {:.1f} FPS".format(dc/t2)
fc = 0
dc = 0
t1 = 0
t2 = 0
t2 = time.perf_counter()
elapsedTime = t2-t1
t1 += 1/elapsedTime
t2 += elapsedTime
finally:
# Stop streaming
self.Realsense.pipeline.stop()