def tcpThread(): global glider_data, tape_data, tcp_dict, data_init, tape_init, tape_x, tape_y, glider_x, glider_y, glider_heading, glider_roll, glider_altitiude s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((tcp_ip, tcp_port)) while tcp_loop: data = s.recv(buffer_size) data = data.splitlines()[0] try: tcp_dict = json.loads(str(data)) channel = tcp_dict['CHANNEL'].upper() if channel == "NEW_TAPE": if not tape_init: tape_init = True tape_data = tcp_dict tape_x = [item for sublist in tape_data['x'] for item in sublist] tape_y = [item for sublist in tape_data['y'] for item in sublist] elif channel == "GLIDER_STATE": if not data_init: data_init = True glider_data = tcp_dict glider_x, glider_y = lla2flatearth(glider_data['LAT'], glider_data['LON']) glider_heading = glider_data["HEADING"]*180/math.pi glider_roll = glider_data["PHI"]*180/math.pi glider_altitiude = glider_data["ALT"] except: pass s.close() print "Closing Visualization TCP Thread"
def tcpThread(): global glider_data, tape_data, tcp_dict, data_init, tape_init, tape_x, tape_y, glider_x, glider_y, glider_heading, glider_roll, glider_altitiude, desired_phi, desired_phi_tape global cur_wpt_x, cur_wpt_y, nxt_wpt_x, nxt_wpt_y, wall_l_x, wall_l_y, wall_r_x, wall_r_y, dist_from_line, dist_from_wall, tape_count, glider_vdown, prev_tape_count global old_tape_x, old_tape_y, old2_tape_x, old2_tape_y, trigger_wall_l_x, trigger_wall_l_y, trigger_wall_r_x, trigger_wall_r_y s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((tcp_ip, tcp_port)) while tcp_loop: data = s.recv(buffer_size) data = data.splitlines()[0] try: tcp_dict = json.loads(str(data)) channel = tcp_dict['CHANNEL'].upper() if channel == "NEW_TAPE": if not tape_init: tape_init = True tape_data = tcp_dict tape_count = tape_data['count'] if prev_tape_count != tape_count: prev_tape_count = tape_count if tape_count > 1: old2_tape_x = old_tape_x old2_tape_y = old_tape_y if tape_count > 0: old_tape_x = tape_x old_tape_y = tape_y tape_x = [item for sublist in tape_data['x'] for item in sublist] tape_y = [item for sublist in tape_data['y'] for item in sublist] cur_wpt_x = tape_data['CUR_WPT'][0] cur_wpt_y = tape_data['CUR_WPT'][1] nxt_wpt_x = tape_data['NXT_WPT'][0] nxt_wpt_y = tape_data['NXT_WPT'][1] wall_l_x = tape_data['WALL_LEFT'][0] wall_l_y = tape_data['WALL_LEFT'][1] wall_r_x = tape_data['WALL_RIGHT'][0] wall_r_y = tape_data['WALL_RIGHT'][1] trigger_wall_l_x = tape_data['TRIGGER_WALL_LEFT'][0] trigger_wall_l_y = tape_data['TRIGGER_WALL_LEFT'][1] trigger_wall_r_x = tape_data['TRIGGER_WALL_RIGHT'][0] trigger_wall_r_y = tape_data['TRIGGER_WALL_RIGHT'][1] dist_from_line = tape_data['DIST_FROM_LINE'] dist_from_wall = tape_data['DIST_FROM_WALL'] desired_phi = tape_data['DESIRED_PHI'] desired_phi_tape = tape_data['DESIRED_PHI_TAPE'] elif channel == "GLIDER_STATE": if not data_init: data_init = True glider_data = tcp_dict #glider_x, glider_y = lla2flatearth(glider_data['LAT'], glider_data['LON']) glider_x, glider_y = helper.lla2flatearth(glider_data['LAT'], glider_data['LON']) glider_heading = glider_data["HEADING"]*180/math.pi glider_roll = glider_data["PHI"]*180/math.pi glider_altitiude = glider_data["ALT"] glider_vdown = glider_data["V_DOWN"] except: pass s.close() print "Closing Visualization TCP Thread"
def calcCurLine(self, x, y, u): x1 = x[self.cur_idx][0] y1 = y[self.cur_idx][0] x2 = x[self.nxt_idx][0] y2 = y[self.nxt_idx][0] uav_x, uav_y = lla2flatearth(self.state_estimation.x[0,0], self.state_estimation.x[1,0]) #for calculating the trigger wall (index 29) xt = x[29][0] yt = y[29][0] xt_p = x[28][0] yt_p = y[28][0] dxt = (xt-xt_p) dyt = (yt-yt_p) anglet = math.atan2(dyt, dxt) * 180.0/math.pi perpanglet = (anglet + 90) * math.pi/180.0 self.trigger_wall_l_x = xt + 100*math.cos(perpanglet) self.trigger_wall_r_x = xt - 100*math.cos(perpanglet) self.trigger_wall_l_y = yt + 100*math.sin(perpanglet) self.trigger_wall_r_y = yt - 100*math.sin(perpanglet) #for calcuating the current line dx = (x2-x1) dy = (y2-y1) #for calculating error to and side of current line of tape err_num = abs(dy*uav_x - dx*uav_y - x1*y2 + x2*y1) err_den = np.sqrt(dx**2 + dy**2) self.error_tape = err_num/err_den self.side_tape = (uav_x - x1)*dy - (uav_y - y1)*dx if self.side_tape > 0: self.side_tape = -1 #right of line else: self.side_tape = 1 #left of line self.error_tape = self.error_tape*self.side_tape #for calculating the current wall angle = math.atan2(dy, dx) * 180.0/math.pi perpangle = (angle + 90) * math.pi/180.0 xperpleft = x2 + 50*math.cos(perpangle) xperpright = x2 - 50*math.cos(perpangle) yperpleft = y2 + 50*math.sin(perpangle) yperpright = y2 - 50*math.sin(perpangle) #for calculating the error to and side of current wall dx = (xperpright-xperpleft) dy = (yperpright-yperpleft) err_num = abs(dy*uav_x - dx*uav_y - xperpleft*yperpright + xperpright*yperpleft) err_den = np.sqrt(dx**2 + dy**2) self.error_wall = err_num/err_den self.side_wall = (uav_x - xperpleft)*dy - (uav_y - yperpleft)*dx if self.side_wall > 0: self.side_wall = -1 #behind wall else: self.side_wall = 1 #in front of wall if self.cur_idx < (len(x)-2): self.cur_idx = self.nxt_idx self.nxt_idx = self.cur_idx + 1 self.cur_wpt_x = x1 self.cur_wpt_y = y1 self.nxt_wpt_x = x2 self.nxt_wpt_y = y2 self.wall_l_x = xperpleft self.wall_l_y = yperpleft self.wall_r_x = xperpright self.wall_r_y = yperpright #index into current control from tape # TODO make this a linear interpolation look up frac = calcPerpendicularFrac([x1,y1],[x2,y2],[uav_x,uav_y]) interpu = u[self.cur_idx][0] #if (frac<1): # interpu = u[self.cur_idx][0] + frac*(u[self.nxt_idx][0] - u[self.cur_idx][0]) if (self.side_wall==1): interpu = 0.0 self.phi_desired_tape = interpu #calculate delta psi (difference between line angle and uav angle) uav_angle = self.heading2XY(self.state_estimation.x[8,0]*180.0/math.pi) vd = [0.0,0.0,0.0] vr = [0.0,0.0,0.0] vd[0] = math.cos(angle*math.pi/180.0) vd[1] = math.sin(angle*math.pi/180.0) vr[0] = math.cos(uav_angle) vr[1] = math.sin(uav_angle) #(angle*math.pi/180.0 - uav_angle) self.psi_delta = math.acos(vd[0]*vr[0] + vd[1]*vr[1]) cr = cross(vd,vr) if cr[2]>0: self.psi_delta = -self.psi_delta