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)