def piCameraSendFrameHelper(stream, frame):
''' do the actual frame processing '''
global piCameraLastFrameIndex
try:
# record index of new frame
piCameraLastFrameIndex = frame.index
# get the frame data and display it
stream.seek(frame.position)
image = stream.read(frame.frame_size)
timestamp = time.time()
message = rdf.newPublishMessage('picam', outTopic, vdefs.DATATYPE,
timestamp)
message[vdefs.WIDTH] = cameraWidth
message[vdefs.HEIGHT] = cameraHeight
message[vdefs.RATE] = cameraRate
message[vdefs.FORMAT] = 'mjpeg'
if ManifoldPython.isServiceActive(
sourcePort) and ManifoldPython.isClearToSend(sourcePort):
ManifoldPython.sendMulticastData(sourcePort, json.dumps(message),
image, timestamp)
except:
print("Send frame error ", sys.exc_info()[0], sys.exc_info()[1])
def sendMessage():
global waitForComplete
ret = False
if (ManifoldPython.isServiceActive(clientPort)):
ret = ManifoldPython.sendE2EData(clientPort, json.dumps(textMessage), '', time.time())
if ret:
print('Sent: ' + testText)
waitForComplete = True
def callback(samples, frame_count, time_info, status):
timestamp = time.time()
message = rdf.newPublishMessage('audio', audioTopic, adefs.DATATYPE,
timestamp)
message[adefs.CHANNELS] = audioChannels
message[adefs.RATE] = audioRate
message[adefs.SAMPTYPE] = 'int16'
message[adefs.FORMAT] = 'pcm'
if ManifoldPython.isServiceActive(
sourcePort) and ManifoldPython.isClearToSend(sourcePort):
ManifoldPython.sendMulticastData(sourcePort, json.dumps(message),
samples, timestamp)
return (None, pyaudio.paContinue)
def processVideoMessage():
global firstVideo, screen, frameCount
while True:
ret, data, image, timestamp = ManifoldPython.getMulticastData(
videoPort)
if not ret:
return
try:
jsonObj = json.loads(data)
if firstVideo:
firstVideo = False
size = (int(jsonObj[vdefs.WIDTH]), int(jsonObj[vdefs.HEIGHT]))
screen = pygame.display.set_mode(size)
imageFile = cStringIO.StringIO(image)
imageSurface = pygame.image.load(imageFile)
screen.blit(imageSurface, (0, 0))
pygame.draw.rect(screen, (0, 0, 0), (10, 5, 600, 30))
label = myfont.render(
'Frame rate: %d fps, %s:%03d' %
(frameRate, time.ctime(timestamp), (timestamp * 1000) % 1000),
1, (255, 255, 255))
screen.blit(label, (20, 10))
pygame.display.flip()
frameCount += 1
except:
print("video data error", sys.exc_info()[0], sys.exc_info()[1])
def processReceivedMessage():
# have to throw away frames to ensure queue empty - processing load is too high
# in some cases for the forwarding to work correctly
while True:
global processingFaceRec, faceRecMessage, faceRecImage,
processingForward, forwardMessage, forwardImage, forwardTS, inPort
ret, data, image, timestamp = ManifoldPython.getMulticastData(inPort)
if not ret:
return
try:
if processingForward:
continue
jsonObj = json.loads(data)
if not processingFaceRec:
faceRecMessage = copy.deepcopy(jsonObj)
faceRecImage = image
processingFaceRec=True
if not processingForward:
forwardMessage = copy.deepcopy(jsonObj)
forwardImage = image
forwardTS = timestamp
processingForward=True
except:
print ("Receive JSON error", sys.exc_info()[0],sys.exc_info()[1])
return
def receiveThread():
global processing, jsonToProcess, imageToProcess, imageTS, receiveThreadExit
while not receiveThreadExit:
ret, data, image, timestamp = ManifoldPython.getMulticastData(inPort)
if not ret:
time.sleep(0.001)
continue
if processing:
time.sleep(0.001)
continue
try:
jsonObj = json.loads(data)
if not processing:
jsonToProcess = jsonObj
imageToProcess = image
imageTS = timestamp
processing=True
except:
print ("JSON error", sys.exc_info()[0],sys.exc_info()[1])
def readSensors():
global lastSensorReadTime
if ((time.time() - lastSensorReadTime) < sampleInterval):
return
# call background loops
if accel.sensorValid:
accel.background()
if light.sensorValid:
light.background()
if temperature.sensorValid:
temperature.background()
if pressure.sensorValid:
pressure.background()
if humidity.sensorValid:
humidity.background()
# time send send the sensor readings
lastSensorReadTime = time.time()
message = rdf.newPublishMessage('sensors', outTopic, sdefs.DATATYPE,
lastSensorReadTime)
if accel.dataValid:
accelData = accel.readAccel()
message[sdefs.ACCEL_DATA] = accelData
if light.dataValid:
lightData = light.readLight()
message[sdefs.LIGHT_DATA] = lightData
if temperature.dataValid:
temperatureData = temperature.readTemperature()
message[sdefs.TEMPERATURE_DATA] = temperatureData
if pressure.dataValid:
pressureData = pressure.readPressure()
message[sdefs.PRESSURE_DATA] = pressureData
if humidity.dataValid:
humidityData = humidity.readHumidity()
message[sdefs.HUMIDITY_DATA] = humidityData
if ManifoldPython.isServiceActive(
sourcePort) and ManifoldPython.isClearToSend(sourcePort):
ManifoldPython.sendMulticastData(sourcePort, json.dumps(message), '',
lastSensorReadTime)
def processReceivedText():
ret, data, binaryData, timestamp = ManifoldPython.getE2EData(ttyOutPort)
if not ret:
return
try:
serialPort.write(binaryData)
except:
print("JSON error", sys.exc_info()[0], sys.exc_info()[1])
def processReceivedText():
ret, data, binaryData, timestamp = ManifoldPython.getMulticastData(
ttyInPort)
if not ret:
return
try:
print(binaryData)
except:
print("JSON error", sys.exc_info()[0], sys.exc_info()[1])
def processCompletion():
global completionToProcess, gotCompletion
ret, data, binaryData, timestamp = ManifoldPython.getMulticastData(sinkPort)
if not ret:
return
if gotCompletion:
return
completionToProcess = data
gotCompletion = True
def processReceivedMessage():
global topicMap
while True:
try:
ret, data, nothing, timestamp = ManifoldPython.getMulticastData(
inPort)
if not ret:
return
topicMap[inTopic].newJSONData(data)
except:
print("Sensor data error", sys.exc_info()[0], sys.exc_info()[1])
def processReceivedMessage():
global processing, jsonToProcess, imageToProcess, imageTS
ret, data, image, timestamp = ManifoldPython.getMulticastData(inPort)
if not ret:
return
try:
jsonObj = json.loads(data)
if not processing:
jsonToProcess = jsonObj
imageToProcess = image
imageTS = timestamp
processing = True
except:
print("JSON error", sys.exc_info()[0], sys.exc_info()[1])
def audioThread():
firstAudio = True
audioStream = None
audioDevice = pyaudio.PyAudio()
# Activate the audio stream
audioPort = ManifoldPython.addMulticastSink(audioTopic)
if (audioPort == -1):
print("Failed to activate audio stream")
ManifoldPython.stop()
sys.exit()
while not audioThreadExit:
ret, data, samples, timestamp = ManifoldPython.getMulticastData(
audioPort)
if not ret:
time.sleep(0.001)
continue
try:
jsonObj = json.loads(data)
if firstAudio:
firstAudio = False
audioStream = audioDevice.open(format=pyaudio.paInt16,
channels=int(
jsonObj[adefs.CHANNELS]),
rate=int(jsonObj[adefs.RATE]),
output=True)
audioStream.write(samples)
except:
print("audio data error", sys.exc_info()[0], sys.exc_info()[1])
try:
audioStream.stop_stream()
audioStream.close()
except:
pass
audioDevice.terminate()
ManifoldPython.removeService(audioPort)
except:
print('python tty [-i <ttyInTopic>] [-o <ttyOutTopic>]' + rdf.manifoldArgs)
print('\nDefaults:')
print(' -i %s\n -o %s\n' % (ttyInTopic, ttyOutTopic))
print(rdf.manifoldArgsDefaults)
sys.exit(2)
for opt, arg in opts:
if opt == '-i':
ttyInTopic = arg
if opt == '-o':
ttyOutTopic = arg
# start ManifoldPython running
ManifoldPython.start("ttytest", sys.argv, False)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# wake up the console
print("ttytest starting...")
sys.stdout.flush()
# Activate the client endpoint
ttyOutPort = ManifoldPython.addE2EClient(ttyOutTopic)
if (ttyOutPort == -1):
print("Failed to activate E2E client endpoint")
ManifoldPython.stop()
sys.exit()
print(rdf.manifoldArgsDefaults)
sys.exit(2)
for opt, arg in opts:
if opt == '-i':
ttyInTopic = arg
if opt == '-o':
ttyOutTopic = arg
if opt == 'p':
ttyPort = arg
if opt == 'r':
ttyRate = int(arg)
# start ManifoldPython running
ManifoldPython.start("tty", sys.argv, False)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# wake up the console
print("tty starting...")
sys.stdout.flush()
# Activate the service endpoint
ttyOutPort = ManifoldPython.addE2EService(ttyOutTopic)
if (ttyOutPort == -1):
print("Failed to activate E2E service endpoint")
ManifoldPython.stop()
sys.exit()
print('python sensorview [-i <inTopic>] [-p <plotInterval>]' +
rdf.manifoldArgs)
print('\nDefaults:')
print(' -i %s\n -p 1.0\n' % (inTopic))
print(rdf.manifoldArgsDefaults)
sys.exit(2)
for opt, arg in opts:
if opt == '-i':
inTopic = arg
if opt == '-p':
plotInterval = int(arg)
# start ManifoldPython running
ManifoldPython.start("avview", sys.argv, False)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# wake up the console
print("sensorview starting...")
sys.stdout.flush()
topicMap[inTopic] = srecs.SensorRecords(inTopic, plotInterval)
# start up the plotter
sensorPlot = splot.sensorplot()
# lastPlotTime is used to control plot updates
except:
print('python modet [-i <inTopic>] [-o <outTopic>]' + rdf.manifoldArgs)
print('\nDefaults:')
print(' -i %s\n -o modet' % inTopic)
print(rdf.manifoldArgsDefaults)
sys.exit(2)
for opt, arg in opts:
if opt == '-i':
inTopic = arg
if opt == '-o':
outTopic = arg
# start ManifoldPython running
ManifoldPython.start("modet", sys.argv, False)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# wake up the console
print("modet starting...")
sys.stdout.flush()
# Tuning factors. The default values seem to work well at 1280 x 720 frame size, 30fps.
# This is how much to scale down the image before background subtraction. Bigger images need bigger factors
scaleFactor = 6
# This controls the length of the history used by the background subtractor - adjust for frame rate
bgHistory = 200
for opt, arg in opts:
if opt == '-h':
cameraHeight = int(arg)
if opt == '-n':
cameraIndex = int(arg)
if opt == '-r':
cameraRate = int(arg)
if opt == '-v':
outTopic = arg
if opt == '-w':
cameraWidth = int(arg)
# start ManifoldPython running
ManifoldPython.start("uvccam", sys.argv, True)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# Activate the source stream
sourcePort = ManifoldPython.addMulticastSource(outTopic)
if (sourcePort == -1):
print("Failed to activate stream service")
ManifoldPython.stop()
sys.exit()
# Open the camera device
if (not ManifoldPython.vidCapOpen(cameraIndex, cameraWidth, cameraHeight,
cameraRate)):
print("Failed to open vidcap")
def forwardThread():
global processingForward, forwardMessage, forwardImage, forwardTS, forwardThreadExit, inPort
# start ManifoldPython running
ManifoldPython.start("facerec", sys.argv, False)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# Activate the input stream
inPort = ManifoldPython.addMulticastSink(inTopic)
if (inPort == -1):
print("Failed to activate input stream")
ManifoldPython.stop()
sys.exit()
# Activate the output stream
outPort = ManifoldPython.addMulticastSource(outTopic)
if (outPort == -1):
print("Failed to activate output stream")
ManifoldPython.stop()
sys.exit()
while not forwardThreadExit:
time.sleep(0.01)
processReceivedMessage()
if not processingForward:
continue
try:
image = forwardImage
inJpeg = np.frombuffer(image, dtype=np.uint8)
cv2Image = cv2.imdecode(inJpeg, cv2.IMREAD_COLOR)
except:
print ("Forward jpeg error", sys.exc_info()[0],sys.exc_info()[1])
try:
dataLock.acquire()
for detectedData in globalDetectedData:
name = detectedData['name']
namepos = detectedData['namepos']
cv2.putText(cv2Image, name, namepos,
cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.75,
color=(152, 255, 204), thickness=2)
landMarks = detectedData['landmarks']
for landMark in landMarks:
cv2.circle(cv2Image, center=landMark, radius=3,
color=(102, 204, 255), thickness=-1)
except:
pass
dataLock.release()
# generate the output jpeg
retVal, outJpeg = cv2.imencode('.jpeg', cv2Image)
forwardMessage[rtndefs.TOPIC] = outTopic
if ManifoldPython.isServiceActive(outPort) and ManifoldPython.isClearToSend(outPort):
ManifoldPython.sendMulticastData(outPort, json.dumps(forwardMessage), outJpeg, forwardTS)
processingForward = False
ManifoldPython.removeService(inPort)
ManifoldPython.removeService(outPort)
ManifoldPython.stop()
for opt, arg in opts:
if opt == '-h':
cameraHeight = int(arg)
if opt == '-n':
cameraIndex = int(arg)
if opt == '-r':
cameraRate = int(arg)
if opt == '-v':
outTopic = arg
if opt == '-w':
cameraWidth = int(arg)
# start ManifoldPython running
ManifoldPython.start("picam", sys.argv, False)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# Activate the source stream
sourcePort = ManifoldPython.addMulticastSource(outTopic)
if (sourcePort == -1):
print("Failed to activate stream service")
ManifoldPython.stop()
sys.exit()
# wake up the console
print("picam starting...")
sys.stdout.flush()
except:
print ('python imageproc [-i <iniFile>] [-o <outTopic>]' + rdf.manifoldArgs)
print ('\nDefaults:')
print (' -i RTIMULib\n -o imageproc')
print (rdf.manifoldArgsDefaults)
sys.exit(2)
for opt, arg in opts:
if opt == '-i':
iniFile = arg
if opt == '-o':
outTopic = arg
# start ManifoldPython running
ManifoldPython.start("imu", sys.argv, False)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# wake up the console
print("imu starting...")
sys.stdout.flush()
# Activate the output stream
outPort = ManifoldPython.addMulticastSource(outTopic)
if (outPort == -1):
print("Failed to activate output stream")
ManifoldPython.stop()
sys.exit()
print (' -c ' + completionTopic)
print (' -m ' + testText)
print (' -t ' + textTopic)
sys.exit(2)
for opt, arg in opts:
if opt == '-c':
completionTopic = arg
if opt == '-m':
testText = arg
elif opt == '-t':
textTopic = arg
# start ManifoldPython running
ManifoldPython.start("ttstest", sys.argv, False)
# this delay is necessary to allow Qt startup to complete
time.sleep(1.0)
# wake up the console
print("ttstest starting...")
sys.stdout.flush()
# Activate the client endpoint
clientPort = ManifoldPython.addE2EClient(textTopic)
if (clientPort == -1):
print("Failed to activate E2E client endpoint")
ManifoldPython.stop()
sys.exit()