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)