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_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 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 tracker = sTracker.tracker() tracker.init() imBuffer = {} maxImBuffer = 20 system_state = {'mode': []} trackerInitiated = False curYaw, curPitch, curRoll = 0.0, 0.0, 0.0 rates = [0] * 3 ## IDS fovX = 40.58 # deg. fovY = 30.23 # deg. pidY = None pidP = None pidR = None pidX = None tPitch = 0.0 tRoll = 0.0 tYaw = 0.0 tX = 0.0 maxAllowedPitch = 15 # deg 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: frameCnt, shape, ts, curExp, hasHighRes = pickle.loads(ret[1]) frame = np.frombuffer(ret[-2], 'uint8').reshape( (shape[0] // 2, shape[1] // 2, 3)).copy() if 'IM_TRACKER_MODE' in system_state[ 'mode'] and trackerInitiated: trackRes = tracker.track(frame) #print('--->', trackRes) if tracker.trackerInit: if trackRes is not None: msg = [ zmq_topics.topic_tracker_result, pickle.dumps([frameCnt, trackRes]) ] sock_pub.send_multipart(msg) centerX = frame.shape[1] // 2 centerY = frame.shape[0] // 2 iFovX = fovX / frame.shape[1] iFovY = fovY / frame.shape[0] #import ipdb; ipdb.set_trace() trckResCenteredX = (trackRes[0] - centerX) * iFovX trckResCenteredY = -(trackRes[1] - centerY) * iFovY #print('--> dx=%f, dy=%f'%(trckResCenteredX-curYaw, trckResCenteredY+curPitch) ) if pidX == None: tYaw = curYaw pidX = PID(**pos_pid_x) pidY = PID(**yaw_pid) pidP = PID(**pitch_im_pid) pidR = PID(**roll_pid) tPitch = curPitch + trckResCenteredY print('-1-> tP: %.2f' % tPitch) tPitch = min(max(tPitch, -maxAllowedPitch), maxAllowedPitch) print('-2-> tP: %.2f' % tPitch) tX = centerX - trackRes[0] #print('--> dx=%f, dp=%f'%(trackRes[0]-centerX, tPitch-curPitch) ) pitchCmd = pidP(curPitch, tPitch, 0, 0) rollCmd = 0 #pidR(curRoll, tRoll, 0, 0) yawCmd = pidY(curYaw, tYaw, 0, 0) ts = time.time() debug_pid = { 'P': pidR.p, 'I': pidR.i, 'D': pidR.d, 'C': rollCmd, 'T': tRoll, 'N': curRoll, 'R': rates[0], 'TS': ts } pub_sock.send_multipart([ zmq_topics.topic_att_hold_roll_pid, pickle.dumps(debug_pid, -1) ]) debug_pid = { 'P': pidP.p, 'I': pidP.i, 'D': pidP.d, 'C': pitchCmd, 'T': tPitch, 'N': curPitch, 'R': rates[1], 'TS': ts } pub_sock.send_multipart([ zmq_topics.topic_att_hold_pitch_pid, pickle.dumps(debug_pid, -1) ]) debug_pid = { 'P': pidY.p, 'I': pidY.i, 'D': pidY.d, 'C': yawCmd, 'T': tYaw, 'N': curYaw, 'R': rates[2], 'TS': ts } pub_sock.send_multipart([ zmq_topics.topic_att_hold_yaw_pid, pickle.dumps(debug_pid, -1) ]) xCmd = pidX(tX, 0) #print('xCmd: %f tx: %f'%(xCmd, tX), pidX.p, pidX.i, pidX.d) thrusterCmd = np.array( mixer.mix(0, xCmd, 0, rollCmd, pitchCmd, yawCmd, curPitch, curRoll)) #print( {'P':pidX.p,'I':pidX.i,'D':pidX.d,'C':xCmd,'T':tX,'N':0, 'TS':ts} ) thrusterCmd += mixer.mix(0, 0, 0, 0, -rates[1] * pidP.D, -rates[2] * pidY.D, 0, 0) thrusters_source.send_pyobj( ['trck', time.time(), thrusterCmd]) else: print('oitracker break...') trackerInitiated = False sock_pub.send_multipart([ zmq_topics.topic_tracker_result, pickle.dumps([frameCnt, (-1, -1)]) ]) if pidX is not None: print('kill controllers...') pidY.reset(), pidP.reset(), pidX.reset() pidY = None pidP = None pidX = None thrusters_source.send_pyobj( ['trck', time.time(), mixer.zero_cmd()]) if topic == zmq_topics.topic_imu: curYaw, curPitch, curRoll = data['yaw'], data['pitch'], data[ 'roll'] rates = [x / np.pi * 180 for x in data['rates']] elif topic == zmq_topics.topic_tracker_cmd: #print('-->', data) if data['frameId'] == -1: tracker.stopTracker() trackerInitiated = False sock_pub.send_multipart([ zmq_topics.topic_tracker_result, pickle.dumps([-1, [-1, -1]]) ]) if pidX is not None: pidY.reset(), pidP.reset(), pidX.reset() pidY = None pidP = None pidX = None thrusters_source.send_pyobj( ['trck', time.time(), mixer.zero_cmd()]) print('got kill tracker...') else: trackerInitiated = tracker.initTracker(data['trackPnt']) trackRes = tracker.track(frame) print('--->', trackerInitiated, data['trackPnt']) if topic == zmq_topics.topic_system_state: system_state = data await asyncio.sleep(0.001)