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