def __call__(self, imgl, imgr):
valid, pt_l_x, pt_l_y, pt_r_x, pt_r_y = self.__track_and_validate(
imgl, imgr)
res = {}
res['pt_l'] = (pt_l_x, pt_l_y)
res['pt_r'] = (pt_r_x, pt_r_y)
res['range'] = -1
res['range_f'] = -1
t_pt = triangulate(self.proj_cams[0], self.proj_cams[1], pt_l_x,
pt_l_y, pt_r_x, pt_r_y)
res['range'] = t_pt[0] # range in the x direction
#res['range_z']=t_pt[2] # range in the z direction
#if valid:#abs(range_f-res['range']) < 0.20: #range jumps less then 2m per sec (0.2/0.1)
#if self.new_ref:
#if self.new_ref or self.t_pt is None:
dx = t_pt[0]
dy = t_pt[1]
dz = t_pt[2]
valid = valid and dx > 0.1 and dx < 5.0
if self.dx_filt is not None:
valid = valid and abs(self.dx_filt.x -
dx) < config.diff_range_valid
if valid and self.ref_point is None:
self.ref_point = (dx, dy, dz)
if self.ref_point is not None:
dy -= self.ref_point[1]
dz -= self.ref_point[2]
res['valid'] = valid
if self.dx_filt is None or not valid:
self.dx_filt = ab_filt((dx, 0))
self.dy_filt = ab_filt((dy, 0))
self.dz_filt = ab_filt((dz, 0))
res['dx'] = dx
res['dy'] = dy
res['dz'] = dz
res['dx_f'] = self.dx_filt(dx)
res['dy_f'] = self.dy_filt(dy)
res['dz_f'] = self.dz_filt(dz)
res['range_f'] = res['dx_f'][0]
#else:
# print('new range filt')
# self.range_filt = ab_filt((res['range'],0))
res['new_ref'] = False
res['ref_cnt'] = 0
res['clr_frq'] = self.clear_freqs
if self.rope_debug is not None:
res['rope_debug'] = self.rope_debug
self.last_res = res
return res
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)
def __call__(self, imgl, imgr):
imgl1r = imgl[:, :, 0].copy()
imgr1r = imgr[:, :, 0].copy()
imgl1b = imgl[:, :, 2].copy()
imgr1b = imgr[:, :, 2].copy()
valid, pt_l_x, pt_l_y, pt_r_x, pt_r_y = self.__track_and_validate(
imgl1r, imgr1r, imgl1b, imgr1b)
res = {}
if not valid:
self.__init_left_corr(imgl1b)
valid, pt_l_x, pt_l_y, pt_r_x, pt_r_y = self.__track_and_validate(
imgl1r, imgr1r, imgl1b, imgr1b)
res['valid'] = valid
res['pt_l'] = (pt_l_x, pt_l_y)
res['pt_r'] = (pt_r_x, pt_r_y)
res['range'] = -1
res['range_f'] = -1
if not valid:
self.__init_left_corr(imgl1b)
#self.t_pt=None
self.last_t_pt = None
else:
t_pt = triangulate(self.proj_cams[0], self.proj_cams[1], pt_l_x,
pt_l_y, pt_r_x, pt_r_y)
res['range'] = t_pt[0] # range in the x direction
#res['range_z']=t_pt[2] # range in the z direction
#if valid:#abs(range_f-res['range']) < 0.20: #range jumps less then 2m per sec (0.2/0.1)
#if self.new_ref:
#if self.new_ref or self.t_pt is None:
if self.new_ref or self.dx_filt is None:
self.t_pt = t_pt #save reference point
self.last_t_pt = None
#self.dx_filt = ab_filt((0,0))
#self.range_filt = ab_filt((res['range'],0))
#self.range_filt_z = ab_filt((res['range_z'],0))
self.dx_filt = ab_filt((t_pt[0], 0))
self.dy_filt = ab_filt((t_pt[1], 0))
self.dz_filt = ab_filt((t_pt[2], 0))
#range_f , d_range_f = self.range_filt(res['range'])
#range_z_f , d_range_z_f = self.range_filt_z(res['range_z'])
#res['range_f'], res['d_range_f'] = range_f , d_range_f
#res['range_z_f'], res['d_range_z_f'] = range_z_f , d_range_z_f
dx = t_pt[0]
dy = t_pt[1]
#print('----',dy)
dz = t_pt[2]
if not self.new_ref:
res['dx'] = dx
res['dy'] = dy
res['dz'] = dz
res['dx_f'] = self.dx_filt(dx)
res['dy_f'] = self.dy_filt(dy)
res['dz_f'] = self.dz_filt(dz)
res['range_f'] = res['dx_f'][0]
else:
print('new ref flag 1')
#else:
# print('new range filt')
# self.range_filt = ab_filt((res['range'],0))
self.last_t_pt = t_pt
res['new_ref'] = self.new_ref
res['ref_cnt'] = self.ref_cnt
return res