def main():
dev = usb.core.find(idVendor=0x2886, idProduct=0x0018)
# ser = serial.Serial("/dev/tty.usbmodem14201", 9600, timeout=1)
# ser = serial.Serial("/dev/ttyACM0", 9600, timeout=1) # Ubuntu VM
ser = serial.Serial("/dev/tty.usbmodem14301", 9600, timeout=1) # OSX
time.sleep(3)
prevAngle = 0
if dev:
mic = Tuning(dev)
mic.write("NONSTATNOISEONOFF", 1)
mic.write("STATNOISEONOFF", 1)
while True:
try:
# prev to ref
# 0 to 65:
# 65 to 0
# 350 to 0
# 0 to 270
rotAngle = 0
rotDir = CCW
cwDiff = 0
ccwDiff = 0
refAngle = mic.direction
print "refAngle:", refAngle, "prevAngle:", prevAngle
diff = refAngle - prevAngle
if abs(diff) < THRESHOLD:
time.sleep(2)
continue
if refAngle < prevAngle:
ccwDiff = (360 - prevAngle) + refAngle
cwDiff = prevAngle - refAngle
else:
ccwDiff = refAngle - prevAngle
cwDiff = prevAngle + (360 - refAngle)
if cwDiff < ccwDiff:
rotAngle = cwDiff
rotDir = CW
print "CW rotAngle:", rotAngle
else:
rotAngle = ccwDiff
rotDir = CCW
print "CCW rotAngle:", rotAngle
if rotAngle > 180:
print "~~~~~~~~~~~~ANGLE IS OVER 180 BAD MATH~~~~~~~~~~~~"
cmd = buildMotorCommand(motor1Id, rotDir, rotAngle / stepSize)
print cmd
prevAngle = refAngle
ser.flushInput()
ser.flushOutput()
ser.write(cmd)
time.sleep(2) # minimum time for a 360 from a send command
except KeyboardInterrupt:
print "terminating"
break
from tuning import Tuning
import usb.core
import usb.util
import time
dev = usb.core.find(idVendor=0x2886, idProduct=0x0018)
if dev:
mic = Tuning(dev)
mic.write("NONSTATNOISEONOFF", 1)
mic.write("NONSTATNOISEONOFF", 1)
while True:
try:
print(mic.direction)
time.sleep(1)
except KeyboardInterrupt:
break
class Moment(object):
def __init__(self):
self.stop = False
logging.basicConfig()
self.logger = logging.getLogger("MMNT")
if DEBUG:
self.logger.setLevel(logging.DEBUG)
else:
self.logger.setLevel(logging.INFO)
self.logger.info("Initializing")
self.masterSampleTime = time.time()
self.slaveSampleTime = time.time()
self.humanSampleTime = time.time()
self.micSampleTime = time.time()
self.logger.debug("Initializing motor control")
self.mc = MotorControl()
self.mc.resetMotors()
self.logger.debug("Initializing microphone")
dev = usb.core.find(idVendor=0x2886, idProduct=0x0018)
if not dev:
sys.exit("Could not find ReSpeaker Mic Array through USB")
self.mic = Tuning(dev)
self.mic.write("NONSTATNOISEONOFF", 1)
self.mic.write("STATNOISEONOFF", 1)
self.mic.write("ECHOONOFF", 1)
self.logger.debug("Initializing video streams")
self.topCamStream = VideoStream(1)
self.botCamStream = VideoStream(2)
self.logger.debug("Initializing models")
self.ht_model = ht.get_model()
self.tfPose = TfPoseEstimator(get_graph_path(TF_MODEL),
target_size=(VideoStream.DEFAULT_WIDTH,
VideoStream.DEFAULT_HEIGHT))
self.logger.info("Initialization complete")
self.topCamState = State.IDLE
self.botCamState = State.IDLE
self.topCamAngle = 0
self.topAngleUpdated = False
self.botCamAngle = 180
self.botAngleUpdated = False
self.master = Cams.TOP
self.lastMaster = Cams.TOP
self.botCamProc = None
self.topCamProc = None
self.audioMap = Map(15)
self.checkMic()
def stop(self):
self.stop = True
def updateTopAngle(self, angle):
if abs(angle - self.topCamAngle) > ANGLE_THRESHOLD and abs(
angle - self.botCamAngle) > OVERLAP_THRESHOLD:
self.topCamAngle = angle
self.topAngleUpdated = True
def updateBotAngle(self, angle):
if abs(angle - self.botCamAngle) > ANGLE_THRESHOLD and abs(
angle - self.topCamAngle) > OVERLAP_THRESHOLD:
self.botCamAngle = angle
self.botAngleUpdated = True
def updatePositions(self):
# Send Serial Commands
if self.topAngleUpdated and self.botAngleUpdated:
self.logger.debug("Top Angle: {}".format(self.topCamAngle))
self.logger.debug("Bot Angle: {}".format(self.botCamAngle))
self.topAngleUpdated = False
self.botAngleUpdated = False
self.mc.runMotors(self.topCamAngle, self.botCamAngle)
elif self.topAngleUpdated:
self.logger.debug("Top Angle: {}".format(self.topCamAngle))
self.topAngleUpdated = False
self.mc.runTopMotor(self.topCamAngle)
elif self.botAngleUpdated:
self.logger.debug("Bot Angle: {}".format(self.botCamAngle))
self.botAngleUpdated = False
self.mc.runBotMotor(self.botCamAngle)
def isWithinNoiseFov(self, angle):
topDiff = abs(angle - self.topCamAngle)
botDiff = abs(angle - self.botCamAngle)
if topDiff < NOISE_ANGLE_THRESHOLD:
self.topCamState |= State.NOISE
if self.topCamState == State.BOTH:
self.master = Cams.TOP
return True
else:
self.topCamState &= ~State.NOISE
if botDiff < NOISE_ANGLE_THRESHOLD:
self.botCamState |= State.NOISE
if self.botCamState == State.BOTH:
self.master = Cams.BOT
return True
else:
self.botCamState &= ~State.NOISE
return False
def checkMic(self):
speechDetected, micDOA = self.mic.speech_detected(), self.mic.direction
if not speechDetected:
# self.audioMap.update_map_with_no_noise()
self.topCamState &= ~State.NOISE
self.botCamState &= ~State.NOISE
return
self.logger.debug("speech detected from {}".format(micDOA))
self.audioMap.update_map_with_noise(micDOA)
primaryMicDOA, secondaryMicDOA = self.audioMap.get_POI_location()
if DEBUG:
self.audioMap.print_map()
if primaryMicDOA == -1:
self.logger.debug("no good audio source")
return
self.logger.debug("mapped audio from {}".format(primaryMicDOA))
# Check if camera is already looking at the primary noise source
if self.isWithinNoiseFov(primaryMicDOA):
# If camera is already looking, check the secondary noise source
if secondaryMicDOA == -1:
self.logger.debug("no good secondary audio source")
return
elif self.isWithinNoiseFov(secondaryMicDOA):
return
else:
micDOA = secondaryMicDOA
else:
micDOA = primaryMicDOA
topDiff = abs(micDOA - self.topCamAngle)
botDiff = abs(micDOA - self.botCamAngle)
# Camera is NOT looking at the noise source at this point
# If both Cameras are not tracking a human,
# move the closest camera
if self.topCamState < State.HUMAN and self.botCamState < State.HUMAN:
if botDiff < topDiff:
self.botCamState |= State.NOISE
self.updateBotAngle(micDOA)
if self.botCamState == State.IDLE:
self.master = Cams.TOP
else:
self.topCamState |= State.NOISE
self.updateTopAngle(micDOA)
if self.topCamState == State.IDLE:
self.master = Cams.BOT
# One of the cameras are on a human, if the other camera is not on a human, move it
elif self.topCamState < State.HUMAN:
self.topCamState |= State.NOISE
self.updateTopAngle(micDOA)
self.master = Cams.BOT
elif self.botCamState < State.HUMAN:
self.botCamState |= State.NOISE
self.updateBotAngle(micDOA)
self.master = Cams.TOP
# The cameras are on a human
else:
# If both are on a human, move the one that's not master
if self.topCamState == State.HUMAN and self.botCamState == State.HUMAN:
if self.master != Cams.BOT:
self.botCamState |= State.NOISE
self.updateBotAngle(micDOA)
else:
self.topCamState |= State.NOISE
self.updateTopAngle(micDOA)
# One of the cameras are on a HUMAN+NOISE, move the one that's not only on a HUMAN
elif self.topCamState == State.HUMAN:
self.topCamState |= State.NOISE
self.updateTopAngle(micDOA)
self.master = Cams.BOT
elif self.botCamState == State.HUMAN:
self.botCamState |= State.NOISE
self.updateBotAngle(micDOA)
self.master = Cams.TOP
def getBestFace(self, humans):
midX = -1
bestHuman = humans[0]
maxScore = 0
for human in humans:
gotMidX = False
score = 0
currMidX = -1
for part in headParts:
if part in human.body_parts:
score += human.body_parts[part].score
if not gotMidX:
currMidX = human.body_parts[
part].x * VideoStream.DEFAULT_WIDTH
gotMidX = True
if score > maxScore:
maxScore = score
midX = currMidX
bestHuman = human
return bestHuman, midX
def checkHumans(self, frame, camera):
humans = self.tfPose.inference(frame,
resize_to_default=True,
upsample_size=RESIZE_RATIO)
if len(humans):
if camera == Cams.TOP:
self.topCamState |= State.HUMAN
if self.topCamState == State.BOTH:
self.master = Cams.TOP
else:
self.botCamState |= State.HUMAN
if self.botCamState == State.BOTH:
self.master = Cams.BOT
if DISPLAY_VIDEO and DRAW_ON_FRAME:
TfPoseEstimator.draw_humans(frame, humans, imgcopy=False)
human, midX = self.getBestFace(humans)
if (ht.is_hands_above_head(human)):
self.logger.debug("HANDS ABOVE HEAD!!!")
if midX != -1:
centerDiff = abs(midX - VideoStream.DEFAULT_WIDTH / 2)
if centerDiff > FACE_THRESHOLD:
if midX < VideoStream.DEFAULT_WIDTH / 2:
# rotate CCW
if camera == Cams.TOP:
self.updateTopAngle(
(self.topCamAngle +
centerDiff * degreePerPixel) % 360)
else:
self.updateBotAngle(
(self.botCamAngle +
centerDiff * degreePerPixel) % 360)
elif midX > VideoStream.DEFAULT_WIDTH / 2:
# rotate CW
if camera == Cams.TOP:
self.updateTopAngle(
(self.topCamAngle -
centerDiff * degreePerPixel) % 360)
else:
self.updateBotAngle(
(self.botCamAngle -
centerDiff * degreePerPixel) % 360)
else:
if camera == Cams.TOP:
self.topCamState &= ~State.HUMAN
else:
self.botCamState &= ~State.HUMAN
return frame
def playVideo(self, cam):
if cam == Cams.TOP:
if self.botCamProc is not None and self.botCamProc.poll(
) is not None:
self.botCamProc.kill()
self.topCamProc = subprocess.Popen(
"ffmpeg -f v4l2 -i /dev/video3 -f v4l2 /dev/video5",
shell=True,
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL)
elif cam == Cams.BOT:
if self.topCamProc is not None and self.topCamProc.poll(
) is not None:
self.topCamProc.kill()
self.botCamProc = subprocess.Popen(
"ffmpeg -f v4l2 -i /dev/video4 -f v4l2 /dev/video5",
shell=True,
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL)
def start(self):
Thread(target=self.run, args=()).start()
def run(self):
self.stop = False
while not self.stop:
try:
topFrame = self.topCamStream.read()
botFrame = self.botCamStream.read()
if time.time() - self.humanSampleTime > HUMAN_SAMPLE_FREQ:
if topFrame is not None:
topFrame = self.checkHumans(topFrame, Cams.TOP)
if botFrame is not None:
botFrame = self.checkHumans(botFrame, Cams.BOT)
self.humanSampleTime = time.time()
if time.time() - self.micSampleTime > MIC_SAMPLE_FREQ:
self.checkMic()
self.micSampleTime = time.time()
self.updatePositions()
# if DISPLAY_VIDEO and topFrame is not None and botFrame is not None:
# if self.master == Cams.TOP:
# if topFrame is not None:
# cv.imshow('Master', topFrame)
# if botFrame is not None:
# cv.imshow('Slave', botFrame)
# else:
# if botFrame is not None:
# cv.imshow('Master', botFrame)
# if topFrame is not None:
# cv.imshow('Slave', topFrame)
# if cv.waitKey(1) == 27:
# pass
if DISPLAY_VIDEO and topFrame is not None and botFrame is not None:
if self.master == Cams.TOP:
top_master = np.concatenate((topFrame, botFrame),
axis=1)
cv.imshow('Master + Slave', top_master)
else:
bot_master = np.concatenate((botFrame, topFrame),
axis=1)
cv.imshow('Master + Slave', bot_master)
if cv.waitKey(1) == 27:
pass
except KeyboardInterrupt:
self.logger.debug("Keyboard interrupt! Terminating.")
break
self.mc.resetMotors()
def main():
logging.basicConfig()
logger = logging.getLogger("MMNT")
logger.setLevel(logging.INFO)
logger.info("Initializing")
masterSampleTime = time.time()
slaveSampleTime = time.time()
logger.debug("Initializing motor control")
mc = MotorControl()
mc.resetMotors()
logger.debug("Initializing microphone")
dev = usb.core.find(idVendor=0x2886, idProduct=0x0018)
if not dev:
sys.exit("Could not find ReSpeaker Mic Array through USB")
mic = Tuning(dev)
mic.write("NONSTATNOISEONOFF", 1)
mic.write("STATNOISEONOFF", 1)
logger.debug("Initializing models")
ht_model = ht.get_model()
tfPose = TfPoseEstimator(get_graph_path(TF_MODEL), target_size=(VideoStream.DEFAULT_WIDTH, VideoStream.DEFAULT_HEIGHT))
logger.debug("Initializing video streams")
topCamStream = VideoStream(1)
botCamStream = VideoStream(2)
topCamStream.start()
botCamStream.start()
masterCamID = TOP_CAM_ID
masterStream = topCamStream
slaveCamID = BOT_CAM_ID
slaveStream = botCamStream
masterTargetAngle = 0
slaveTargetAngle = 180
updateMasterAngle = False
updateSlaveAngle = False
masterTracking = False
logger.info("Initialization complete")
while True:
try:
# MASTER
masterFrame = masterStream.read()
if time.time() - masterSampleTime > MASTER_SAMPLE_FREQ:
humans = tfPose.inference(masterFrame, resize_to_default=True, upsample_size=RESIZE_RATIO)
if len(humans):
logger.debug("Master tracking")
masterTracking = True
if DISPLAY_VIDEO:
TfPoseEstimator.draw_humans(masterFrame, humans, imgcopy=False)
human = humans[0]
if (ht.is_hands_above_head(human)):
logger.debug("HANDS ABOVE HEAD!!!")
midX = -1
for part in headParts:
if part in human.body_parts:
midX = human.body_parts[part].x * VideoStream.DEFAULT_WIDTH
break
if midX != -1:
centerDiff = abs(midX - VideoStream.DEFAULT_WIDTH/2)
if centerDiff > FACE_THRESHOLD:
if midX < VideoStream.DEFAULT_WIDTH/2:
# rotate CCW
masterTargetAngle += centerDiff * degreePerPixel
elif midX > VideoStream.DEFAULT_WIDTH/2:
# rotate CW
masterTargetAngle -= centerDiff * degreePerPixel
masterTargetAngle = masterTargetAngle % 360
updateMasterAngle = True
masterSampleTime = time.time()
else:
logger.debug("Master stopped tracking")
masterTracking = False
# If master is not tracking a human, move towards speech
if not masterTracking:
speechDetected, micDOA = mic.speech_detected(), mic.direction
logger.debug("master speech detected:", speechDetected, "diff:", abs(micDOA - masterTargetAngle))
if speechDetected and abs(micDOA - masterTargetAngle) > ANGLE_THRESHOLD:
masterTargetAngle = micDOA
logger.debug("Update master angle:", masterTargetAngle)
masterSampleTime = time.time()
updateMasterAngle = True
# SLAVE
slaveFrame = slaveStream.read()
if time.time() - slaveSampleTime > SLAVE_SAMPLE_FREQ:
# If master is not tracking a human and a slave sees a human, move master to the visible human and move slave away
if not masterTracking and time.time() - masterSampleTime > MASTER_SAMPLE_FREQ:
humans = tfPose.inference(slaveFrame, resize_to_default=True, upsample_size=RESIZE_RATIO)
if len(humans):
logger.debug("slave found mans")
if DISPLAY_VIDEO:
TfPoseEstimator.draw_humans(slaveFrame, humans, imgcopy=False)
human = humans[0]
if (ht.is_hands_above_head(human)):
logger.debug("HANDS ABOVE HEAD!!!")
midX = -1
for part in headParts:
if part in human.body_parts:
midX = human.body_parts[part].x * VideoStream.DEFAULT_WIDTH
break
if midX != -1:
centerDiff = abs(midX - VideoStream.DEFAULT_WIDTH/2)
# if centerDiff > FACE_THRESHOLD:
if midX < VideoStream.DEFAULT_WIDTH/2:
# rotate CCW
masterTargetAngle = slaveTargetAngle + centerDiff * degreePerPixel
elif midX > VideoStream.DEFAULT_WIDTH/2:
# rotate CW
masterTargetAngle = slaveTargetAngle - centerDiff * degreePerPixel
masterTargetAngle = masterTargetAngle % 360
updateMasterAngle = True
masterSampleTime = time.time()
slaveTargetAngle = (masterTargetAngle + 180) % 360
updateSlaveAngle = True
logger.debug("Moving master to slave:", masterTargetAngle)
speechDetected, micDOA = mic.speech_detected(), mic.direction
speechMasterDiff = abs(micDOA - masterTargetAngle)
if speechDetected and speechMasterDiff > SLAVE_MASTER_THRESHOLD and abs(micDOA - slaveTargetAngle) > ANGLE_THRESHOLD:
slaveTargetAngle = micDOA
logger.debug("Update slave angle:", slaveTargetAngle)
slaveSampleTime = time.time()
updateSlaveAngle = True
# Send Serial Commands
if updateSlaveAngle and updateMasterAngle:
logger.debug("Slave Angle:", slaveTargetAngle)
logger.debug("Master Angle:", masterTargetAngle)
updateSlaveAngle = False
updateMasterAngle = False
if slaveCamID == BOT_CAM_ID:
mc.runMotors(masterTargetAngle, slaveTargetAngle)
else:
mc.runMotors(slaveTargetAngle, masterTargetAngle)
elif updateSlaveAngle:
mc.runMotor(slaveCamID, slaveTargetAngle)
logger.debug("Slave Angle:", slaveTargetAngle)
updateSlaveAngle = False
elif updateMasterAngle:
mc.runMotor(masterCamID, masterTargetAngle)
logger.debug("Master Angle:", masterTargetAngle)
updateMasterAngle = False
if DISPLAY_VIDEO:
cv.imshow('Master Camera', masterFrame)
cv.imshow('Slave Camera', slaveFrame)
if cv.waitKey(1) == 27:
pass
except KeyboardInterrupt:
logger.debug("Keyboard interrupt! Terminating.")
mc.stopMotors()
slaveStream.stop()
masterStream.stop()
mic.close()
time.sleep(2)
break
cv.destroyAllWindows()
