async def recv_and_process():
keep_running = True
jm = Joy_map()
yaw, pitch, roll = 0, 0, 0
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_axes:
#print('joy ',ret[jm.yaw])
jm.update_axis(data)
roll_copensate, pitch_copensate = 0, 0
joy = jm.joy_mix()
if not joy['inertial']: ## default is inertial
roll_copensate, pitch_copensate = roll, pitch
thruster_joy_cmd = mixer.mix(joy['ud'], joy['lr'], joy['fb'],
joy['roll'], joy['pitch'],
joy['yaw'], pitch_copensate,
roll_copensate)
thrusters_source.send_pyobj(
['joy', time.time(), thruster_joy_cmd])
if topic == zmq_topics.topic_button:
jm.update_buttons(data)
if topic == zmq_topics.topic_imu:
yaw, pitch, roll = data['yaw'], data['pitch'], data['roll']
await asyncio.sleep(0.001)
async def recv_and_process():
keep_running = True
st = tracker.StereoTrack()
jm = Joy_map()
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_stereo_camera:
frame_cnt, shape = data
imgl = np.frombuffer(ret[2], 'uint8').reshape(shape).copy()
imgr = np.frombuffer(ret[3], 'uint8').reshape(shape).copy()
try:
ret = st(imgl, imgr)
ret['ts'] = time.time()
ret['fnum'] = frame_cnt
sock_pub.send_multipart(
[zmq_topics.topic_tracker,
pickle.dumps(ret)])
except:
print("Exception in tracker reseting tracker:")
traceback.print_exc(file=sys.stdout)
st.reset()
if topic == zmq_topics.topic_button:
jm.update_buttons(data)
if jm.track_lock_event():
print('got lock event from joy')
st.reset()
if topic == zmq_topics.topic_axes:
jm.update_axis(data)
if config.tracker == 'rope':
if jm.inc_freqs_track_rope():
st.inc_clear_freqs()
print('inc clear freqs')
if jm.dec_freqs_track_rope():
st.dec_clear_freqs()
print('dec clear freqs')
await asyncio.sleep(0.001)
if __name__ == '__main__':
if udp:
imgSock = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
imgSock.bind(('', config.udpPort))
else:
init_gst_reader(1)
sx, sy = config.cam_res_rgbx, config.cam_res_rgby
data_file_fd = None
#main_camera_fd=None
message_dict = {}
rcv_cnt = 0
record_state = False
jm = Joy_map()
bmargx, bmargy = config.viewer_blacks
while 1:
#join=np.zeros((sy,sx*2,3),'uint8')
join = np.zeros((sy + bmargy, sx * 2 + bmargx, 3), 'uint8')
if udp:
data, addr = imgSock.recvfrom(1024 * 64)
img = cv2.imdecode(pickle.loads(data), 1)
#import ipdb; ipdb.set_trace()
images = [img]
else:
images = get_imgs()
rcv_cnt += 1
#if all(images):
while 1:
async def recv_and_process():
keep_running=True
target_att=np.zeros(3)
pid_y,pid_p,pid_r=[None]*3
system_state={'mode':[]}
jm=Joy_map()
joy=None
while keep_running:
socks=zmq.select(subs_socks,[],[],0.005)[0]
for sock in socks:
ret=sock.recv_multipart()
topic,data=ret[0],pickle.loads(ret[1])
if topic==zmq_topics.topic_imu:
yaw,pitch,roll=data['yaw'],data['pitch'],data['roll']
if 0 and 'yawr' in data:
ans = (data['yawr'],data['pitchr'],data['rollr'])
yawr,pitchr,rollr=ans
else:
#ans=#mixer.from_ang_rates_to_euler_rates(yaw,pitch,roll,data['rates'])
#if ans is not None:
# yawr,pitchr,rollr=mixer.from_ang_rates_to_euler_rates(yaw,pitch,roll,data['rates'])
rates = [x/np.pi*180 for x in data['rates']]
joy = jm.joy_mix()
if 'ATT_HOLD' in system_state['mode']:# and ans is not None:
if pid_y is None:
pid_y=PID(**yaw_pid)
pid_p=PID(**pitch_pid)
pid_r=PID(**roll_pid)
else:
#if joy and joy['inertial'] and abs(joy['yaw'])<0.05:
if joy and abs(joy['yaw'])<0.05:
yaw_cmd = pid_y(yaw,target_att[0],0,0)
else:
target_att[0]=yaw
yaw_cmd=0
#print('R{:06.3f} Y{:06.3f} YT{:06.3f} C{:06.3f}'.format(yawr,yaw,target_att[0],yaw_cmd))
if joy and abs(joy['pitch'])<0.1:
pitch_cmd = pid_p(pitch,target_att[1],0,0)
else:
target_att[1]=pitch
pitch_cmd=0
#print('R{:06.3f} P{:06.3f} PT{:06.3f} C{:06.3f}'.format(pitchr,pitch,target_att[1],pitch_cmd))
roll_cmd = pid_r(roll,0 if roll_target_0 else target_att[2],0,0)
#print('RR{:06.3f} R{:06.3f} RT{:06.3f} C{:06.3f}'.format(rollr,roll,target_att[2],roll_cmd))
ts=time.time()
debug_pid = {'P':pid_r.p,'I':pid_r.i,'D':pid_r.d,'C':roll_cmd,'T':0,'N':roll, 'R':rates[0], 'TS':ts}
pub_sock.send_multipart([zmq_topics.topic_att_hold_roll_pid, pickle.dumps(debug_pid,-1)])
debug_pid = {'P':pid_p.p,'I':pid_p.i,'D':pid_p.d,'C':pitch_cmd,'T':target_att[1],'N':pitch, 'R':rates[1],'TS':ts}
pub_sock.send_multipart([zmq_topics.topic_att_hold_pitch_pid, pickle.dumps(debug_pid,-1)])
debug_pid = {'P':pid_y.p,'I':pid_y.i,'D':pid_y.d,'C':yaw_cmd,'T':target_att[0],'N':yaw, 'R':rates[2], 'TS':ts}
pub_sock.send_multipart([zmq_topics.topic_att_hold_yaw_pid, pickle.dumps(debug_pid,-1)])
thruster_cmd = np.array(mixer.mix(0,0,0,roll_cmd,pitch_cmd,yaw_cmd,pitch,roll))
thruster_cmd += mixer.mix(0,0,0,-rates[0]*pid_r.D,-rates[1]*pid_p.D,-rates[2]*pid_y.D,0,0)
thrusters_source.send_pyobj(['att',time.time(),thruster_cmd])
else:
if pid_y is not None:
pid_y.reset(),pid_r.reset(),pid_y.reset()
target_att=[yaw,0,0]
thrusters_source.send_pyobj(['att',time.time(),mixer.zero_cmd()])
if topic==zmq_topics.topic_axes:
jm.update_axis(data)
if topic==zmq_topics.topic_button:
jm.update_buttons(data)
#target_depth+=data[jm.ud]
if topic==zmq_topics.topic_system_state:
_,system_state=data
await asyncio.sleep(0.001)
async def recv_and_process():
keep_running = True
pitch, roll = 0, 0
target_range = 0
pid = None
ab = None
rate = 0
system_state = {'mode': []}
jm = Joy_map()
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_sonar:
new_sonar_ts, range = data[
'ts'], data['sonar'][0] / 1000 # Convert to m
if ab is None:
ab = ab_filt([range, 0])
else:
depth, rate = ab(range, new_sonar_ts - sonar_ts)
sonar_ts = new_sonar_ts
if 'SONAR_HOLD' in system_state['mode']:
if pid is None:
pid = PID(**sonar_pid)
else:
ud_command = -pid(range, target_range, rate, 0)
debug_pid = {
'P': pid.p,
'I': pid.i,
'D': pid.d,
'C': ud_command,
'T': target_range,
'N': range,
'TS': new_sonar_ts
}
pub_sock.send_multipart([
zmq_topics.topic_sonar_hold_pid,
pickle.dumps(debug_pid, -1)
])
thruster_cmd = mixer.mix(ud_command, 0, 0, 0, 0, 0,
pitch, roll)
thrusters_source.send_pyobj(
['sonar', time.time(), thruster_cmd])
else:
if pid is not None:
pid.reset()
thrusters_source.send_pyobj(
['sonar', time.time(),
mixer.zero_cmd()])
target_range = range
if topic == zmq_topics.topic_axes:
jm.update_axis(data)
target_range += -jm.joy_mix()['ud'] / 25.0
if topic == zmq_topics.topic_imu:
pitch, roll = data['pitch'], data['roll']
if topic == zmq_topics.topic_system_state:
_, system_state = data
await asyncio.sleep(0.001)
async def recv_and_process():
keep_running = True
pitch, roll = 0, 0
target_depth = 0
pid = None
ab = None
rate = 0
system_state = {'mode': []}
jm = Joy_map()
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_depth:
new_depth_ts, depth = data['ts'], data['depth']
if ab is None:
ab = ab_filt([depth, 0])
else:
depth, rate = ab(depth, new_depth_ts - depth_ts)
depth_ts = new_depth_ts
if 'DEPTH_HOLD' in system_state['mode']:
if pid is None:
pid = PID(**depth_pid)
else:
ud_command = pid(depth, target_depth, rate, 0)
debug_pid = {
'P': pid.p,
'I': pid.i,
'D': pid.d,
'C': ud_command,
'T': target_depth,
'N': depth,
'TS': new_depth_ts
}
pub_sock.send_multipart([
zmq_topics.topic_depth_hold_pid,
pickle.dumps(debug_pid, -1)
])
thruster_cmd = mixer.mix(ud_command, 0, 0, 0, 0, 0,
pitch, roll)
thrusters_source.send_pyobj(
['depth', time.time(), thruster_cmd])
else:
if pid is not None:
pid.reset()
thrusters_source.send_pyobj(
['depth', time.time(),
mixer.zero_cmd()])
target_depth = depth
if topic == zmq_topics.topic_axes:
jm.update_axis(data)
target_depth += jm.joy_mix()['ud'] / 250.0
if topic == zmq_topics.topic_gui_depthAtt:
if data['dDepth'] is not None:
target_depth = data['dDepth']
print('set new depth: %.2f' % target_depth)
if topic == zmq_topics.topic_imu:
pitch, roll = data['pitch'], data['roll']
if topic == zmq_topics.topic_system_state:
system_state = data
await asyncio.sleep(0.001)
async def recv_and_process():
keep_running = True
target_pos = np.zeros(3)
pids = [None] * 3
system_state = {'mode': []}
jm = Joy_map()
joy = None
while keep_running:
socks = zmq.select(subs_socks, [], [], 0.005)[0]
for sock in socks:
ret = sock.recv_multipart()
topic, data = ret[0], pickle.loads(ret[1])
if topic == zmq_topics.topic_imu:
yaw, pitch, roll = data['yaw'], data['pitch'], data['roll']
if topic == zmq_topics.topic_tracker:
td = data
cmds = [0] * 3
for ind in range(len(pids)):
#mapping from track image coords to world coords
imap = ['dx_f', 'dy_f', 'dz_f'][ind]
pid_states = ['RX_HOLD', 'RY_HOLD', 'RZ_HOLD']
if pid_states[ind] not in system_state['mode'] \
or pids[ind] is None:
pids[ind] = PID(**pos_pids[ind])
if td['valid'] and imap in td:
x, _ = td[imap]
#we want it to return to 0 for the y lock
target_pos[ind] = 0 if ind == 1 else x
else:
if td['valid'] and imap in td:
x, v = td[imap]
cmds[ind] = -pids[ind](x, target_pos[ind], v)
#print('pid=',ind,x,v,str(pids[ind]))
ts = time.time()
debug_pid = {
'P': pids[ind].p,
'I': pids[ind].i,
'D': pids[ind].d,
'C': cmds[ind],
'T': target_pos[ind],
'N': x,
'R': v,
'TS': ts
}
pub_sock.send_multipart([
zmq_topics.topic_pos_hold_pid_fmt % ind,
pickle.dumps(debug_pid, -1)
])
thruster_cmd = mixer.mix(cmds[2], cmds[1], cmds[0], 0, 0, 0, 0,
0)
thrusters_source.send_pyobj(['pos', time.time(), thruster_cmd])
if topic == zmq_topics.topic_axes:
jm.update_axis(data)
if topic == zmq_topics.topic_button:
jm.update_buttons(data)
#target_depth+=data[jm.ud]
if topic == zmq_topics.topic_system_state:
_, system_state = data
await asyncio.sleep(0.001)
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)
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
system_state = {'arm': False, 'mode': [], 'lights': 0} #lights 0-5
thrusters_dict = {}
last_axes_joy_message_time = 0 #keep alive time
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:
jm.update_buttons(data)
if jm.depth_hold_event():
togle_mode('DEPTH_HOLD')
if 'SONAR_HOLD' in system_state['mode']:
system_state['mode'].remove('SONAR_HOLD')
if jm.sonar_hold_event():
togle_mode('SONAR_HOLD')
if 'DEPTH_HOLD' in system_state['mode']:
system_state['mode'].remove('DEPTH_HOLD')
if jm.att_hold_event():
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.cam_calib_event():
togle_mode('CAM_CALIB')
if jm.image_rect_event():
togle_mode('RECT')
if jm.arm_event():
system_state['arm'] = not system_state['arm']
if not system_state['arm']:
system_state['mode'] = []
if topic == zmq_topics.topic_axes:
last_axes_joy_message_time = time.time()
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'])
tic = time.time()
if tic - last_axes_joy_message_time > 2.0: #when lost joy signal disarm
system_state['arm'] = False
if tic - timer10hz > 0:
timer10hz = tic + 1 / 10.0
pub_sock.send_multipart([
zmq_topics.topic_system_state,
pickle.dumps((tic, system_state))
])
if tic - timer20hz > 0:
timer20hz = tic + 1 / 20.0
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)
#print('botton',ret)
await asyncio.sleep(0.001)