#if all(images):
while 1:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
if len(socks) == 0: #flush msg buffer
break
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret
data = pickle.loads(ret[1])
message_dict[topic] = data
if topic == zmq_topics.topic_button:
print('jm ', data)
jm.update_buttons(data)
if jm.record_event():
#togel recording
if not record_state:
record_state = datetime.now().strftime(
'%y%m%d-%H%M%S')
else:
record_state = False
pub_record_state.send_multipart([
zmq_topics.topic_record_state,
pickle.dumps(record_state)
])
message_dict[zmq_topics.topic_record_state] = record_state
if args.pub_data:
socket_pub.send_multipart([ret[0], ret[1]])
async def recv_and_process():
keep_running = True
thruster_cmd = np.zeros(8)
timer10hz = time.time() + 1 / 10.0
timer20hz = time.time() + 1 / 20.0
thrustersRate = 50.0
thrustersTimerHz = time.time() + 1 / thrustersRate
timer0_1hz = time.time() + 10
initPwmFocus = 1400
preFocusFileName = '../hw/focusState.bin'
if os.path.exists(preFocusFileName):
with open(preFocusFileName, 'r') as fid:
initPwmFocus = int(fid.read(4))
print('reset focus to:', initPwmFocus)
gainCtl = -1
expCtl = -1
camStateFile = '../hw/camSate.pkl'
if os.path.exists(camStateFile):
try:
with open(camStateFile, 'rb') as fid:
camState = pickle.load(fid)
if 'aGain' in camState.keys():
gainCtl = camState['aGain']
if 'aExp' in camState.keys():
expCtl = camState['aExp']
except:
print('no cam state...')
diskUsage = int(
os.popen('df -h / | tail -n 1').readline().strip().split()[-2][:-1])
system_state = {
'ts': time.time(),
'arm': False,
'mode': [],
'lights': 0,
'focus': initPwmFocus,
'record': False,
'diskUsage': diskUsage,
'autoGain': gainCtl,
'autoExp': expCtl
} #lights 0-5
thrusters_dict = {}
jm = Joy_map()
def togle_mode(m):
s = system_state
if m in s['mode']:
s['mode'].remove(m)
else:
s['mode'].append(m)
def test_togle(b):
return new_joy_buttons[b] == 1 and joy_buttons[b] == 0
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.002)[0]
for sock in socks:
if sock == thruster_sink:
source, _, thruster_src_cmd = sock.recv_pyobj()
thrusters_dict[source] = thruster_src_cmd
else:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_button or topic == zmq_topics.topic_gui_diveModes:
jm.update_buttons(data)
if jm.depth_hold_event():
print('Toggle depth hold...')
if ('IM_TRACKER_MODE' in system_state['mode']) and (
'DEPTH_HOLD' in system_state['mode']):
print(
'failed to toggle to DEPTH_HOLD (needs to stay on DEPTH_HOLD) while IM_TRACKER_MODE'
)
else:
togle_mode('DEPTH_HOLD')
if jm.att_hold_event():
print('Toggle attitude hold...')
#if ('IM_TRACKER_MODE' in system_state['mode']) and ('ATT_HOLD' in system_state['mode']):
# print('failed to toggle to ATT_HOLD (needs to stay on ATT_HOLD) while IM_TRACKER_MODE')
#else:
# togle_mode('ATT_HOLD')
togle_mode('ATT_HOLD')
if jm.Rx_hold_event():
togle_mode('RX_HOLD')
if jm.Ry_hold_event():
togle_mode('RY_HOLD')
if jm.Rz_hold_event():
togle_mode('RZ_HOLD')
if jm.record_event():
system_state['record'] = not system_state['record']
print('record state', system_state['record'])
if jm.arm_event():
system_state['arm'] = not system_state['arm']
print('state arm:', system_state['arm'])
if not system_state['arm']:
system_state['mode'] = []
elif topic == zmq_topics.topic_axes or topic == zmq_topics.topic_gui_controller:
jm.update_axis(data)
if jm.inc_lights_event():
system_state['lights'] = min(
5, system_state['lights'] + 1)
pub_sock.send_multipart([
zmq_topics.topic_lights,
pickle.dumps(system_state['lights'])
])
print('lights set to ', system_state['lights'])
if jm.dec_lights_event():
system_state['lights'] = max(
0, system_state['lights'] - 1)
pub_sock.send_multipart([
zmq_topics.topic_lights,
pickle.dumps(system_state['lights'])
])
print('lights set to ', system_state['lights'])
if jm.inc_focus_event():
system_state['focus'] = min(
2250, system_state['focus'] + focusResolution)
pub_sock.send_multipart([
zmq_topics.topic_focus,
pickle.dumps(system_state['focus'])
])
print('focus set to ', system_state['focus'])
if jm.dec_focus_event():
system_state['focus'] = max(
850, system_state['focus'] - focusResolution)
pub_sock.send_multipart([
zmq_topics.topic_focus,
pickle.dumps(system_state['focus'])
])
print('focus set to ', system_state['focus'])
elif topic == zmq_topics.topic_gui_focus_controller or topic == zmq_topics.topic_autoFocus:
pwm = data
system_state['focus'] = pwm
pub_sock.send_multipart([
zmq_topics.topic_focus,
pickle.dumps(system_state['focus'])
])
print('focus set to value ', system_state['focus'])
elif topic == zmq_topics.topic_gui_autoFocus:
os.system('../scripts/runAutofocus.sh')
elif topic == zmq_topics.topic_gui_start_stop_track:
print('start/stop tracker... ', data)
'''
if 'ATT_HOLD' not in system_state['mode']:
togle_mode('ATT_HOLD')
'''
if 'DEPTH_HOLD' not in system_state['mode']:
togle_mode('DEPTH_HOLD')
togle_mode('IM_TRACKER_MODE')
if 'IM_TRACKER_MODE' in system_state['mode']:
pub_sock.send_multipart(
[zmq_topics.topic_tracker_cmd,
pickle.dumps(data)])
else:
pub_sock.send_multipart([
zmq_topics.topic_tracker_cmd,
pickle.dumps({
'frameId': -1,
'trackPnt': (-1, -1)
})
])
elif topic == zmq_topics.topic_tracker_result:
#print('--->', data)
if data[1][0] < 0:
if 'IM_TRACKER_MODE' in system_state['mode']:
print('Tracker ended...')
togle_mode('IM_TRACKER_MODE')
elif topic == zmq_topics.topic_gui_toggle_auto_exp:
print('got auto exposure command')
if expCtl == 0:
expCtl = 1
else:
expCtl = 0
system_state['autoExp'] = expCtl
camPubSock.send_multipart([
zmq_topics.topic_cam_toggle_auto_exp,
pickle.dumps(expCtl)
])
elif topic == zmq_topics.topic_gui_toggle_auto_gain:
print('got auto gain command')
if gainCtl == 0:
gainCtl = 1
else:
gainCtl = 0
system_state['autoGain'] = gainCtl
camPubSock.send_multipart([
zmq_topics.topic_cam_toggle_auto_gain,
pickle.dumps(gainCtl)
])
elif topic == zmq_topics.topic_gui_inc_exp:
print('got camera inc exp.')
camPubSock.send_multipart(
[zmq_topics.topic_cam_inc_exp,
pickle.dumps([0])])
elif topic == zmq_topics.topic_gui_dec_exp:
print('got camera dec exp.')
camPubSock.send_multipart(
[zmq_topics.topic_cam_dec_exp,
pickle.dumps([0])])
elif topic == zmq_topics.topic_gui_exposureVal:
print('got camera exp. value:', data)
camPubSock.send_multipart(
[zmq_topics.topic_cam_exp_val,
pickle.dumps(data)])
tic = time.time()
if tic - timer10hz > 0:
timer10hz = tic + 1 / 10.0
system_state['ts'] = tic
pub_sock.send_multipart(
[zmq_topics.topic_system_state,
pickle.dumps(system_state)])
if tic - thrustersTimerHz > 0:
thrustersTimerHz = tic + 1 / thrustersRate
if not system_state['arm']:
thruster_cmd = np.zeros(8)
else:
for k in thrusters_dict:
thruster_cmd += thrusters_dict[k]
pub_sock.send_multipart([
zmq_topics.topic_thrusters_comand,
pickle.dumps((tic, list(thruster_cmd)))
])
thruster_cmd = np.zeros(8)
if tic - timer0_1hz > 0:
timer0_1hz = tic + 10
system_state['diskUsage'] = int(
os.popen('df -h / | tail -n 1').readline().strip().split()[-2]
[:-1])
#print('botton',ret)
await asyncio.sleep(0.001)