def do_joy_control(self): rst = self.joy.get_ctrl() p = rst['pitch'] r = rst['roll'] y = rst['yaw'] t = rst['throttle'] ctrltype = rst['ctrltype'] sd = rst['sd'] self.state_smart_direction = bool(sd) self.m_checkBox_smart_direction.SetValue(self.state_smart_direction) self.update_joy_status(rst) return self.send_data_by_frame(MsgPrcs.pack_control(ctrltype,sd,p,r,y,t))
def main_work(self, event): worklist = self.worklist a = time.clock() # 5 Hz Tasks if (time.clock()-self.timer.last_time) > 1.0/5: self.timer.last_time=time.clock() if DISPLAY_XBEE_DATA in worklist: self.update_rcv_area() if USING_JOYSTICK in self.worklist: self.do_joy_control() # 发送MID=0x09时自动返回,不需重复发送。 if DISPLAY_UAVINFO in self.worklist and USING_JOYSTICK not in self.worklist: self.send_data_by_frame(MsgPrcs.pack_control(0, self.state_smart_direction)) if DISPLAY_UAVINFO in self.worklist: self.update_GUI_UAVinfo(self.UAVinfo.get()) # MAIN_TASK_FREQ Hz Tasks if USING_JOYSTICK in self.worklist: self.update_joy_status() if DISPLAY_VIDEO in worklist: srcimg = self.camcap.get_frame() wxbmp = util.cvimg_to_wxbmp(srcimg) wximg = wx.ImageFromBitmap(wxbmp) memvideo = wx.MemoryDC() memvideo.SelectObject(wxbmp) if self.m_menuItem_video_osd.IsChecked(): # draw OSD information on bitmap_video memvideo.SetTextForeground( wx.BLUE ) memvideo.SetFont( util.WXFONT ) pos = (srcimg.shape[1] - util.PADDING - util.TIME_TEXT_WIDTH, util.PADDING) memvideo.DrawText(util.get_now(), pos[0], pos[1]) # 设置缩放比例 memvideo.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_video_size[0]), float(srcimg.shape[0])/float(self.bitmap_video_size[1]) ) self.dc_video.Blit(0, 0, self.bitmap_video_size[0], self.bitmap_video_size[1], memvideo, 0, 0) memvideo.SelectObject(wx.NullBitmap) if RECORD_VIDEO in worklist: self.mov_rec.save_frame(wxbmp) if DISPLAY_INDEPENDENT_VIDEO in worklist: self.video_window.update_image_with_info1(wximg, self.UAVinfo.get_information_in_InfoEntries()) # 结束图像传输需要先停止track if DISPLAY_TRACK_VIDEO in worklist: memtrack = wx.MemoryDC() # 显示原始图像 if self.display_track_state == DISPLAY_TRACK_STATE_RAW: #rstimg = self.get_adjusted_image(srcimg) rstimg = srcimg rstbmp = util.cvimg_to_wxbmp(rstimg) # 正在框选状态 elif self.display_track_state == DISPLAY_TRACK_STATE_SELECTION: assert self.frozen_frame is not None, 'Frozen frame is none.' rectimg = self.get_dragging_image(self.frozen_frame,self.drag_info.get_drag_data()) rstbmp = util.cvimg_to_wxbmp(rectimg) # 显示目标追踪结果 elif self.display_track_state == DISPLAY_TRACK_STATE_RESULT: track_mode = self.m_choice_track_mode.GetStringSelection() display_process = self.m_menuItem_track_display_process.IsChecked() if track_mode == 'template': method = METHOD.TEMPLATEMATCH elif track_mode == 'meanshift': method = METHOD.MEANSHIFT elif track_mode == 'gray-meanshift': method = METHOD.GRAYMEANSHIFT else: method = METHOD.OPTICALFLOW matchimg, center, res = self.objmatch.process(method, srcimg) if display_process: rstbmp = util.cvimg_to_wxbmp(res) else: rstbmp = util.cvimg_to_wxbmp(matchimg) # # 模板匹配模式 # if track_mode == 'template': # matchimg, center, res = self.objmatch.do_tpl_match(srcimg) # if display_process: # rstbmp = util.cvimg_to_wxbmp(res) # else: # rstbmp = util.cvimg_to_wxbmp(matchimg) # # 边缘检测-模板匹配模式 # elif track_mode == 'edge-tpl': # matchimg, center, edgeimg = self.objmatch.do_edge_match(srcimg, arg=self.edge_arg) # if display_process: # rstbmp = util.cvimg_to_wxbmp(edgeimg) # else: # rstbmp = util.cvimg_to_wxbmp(matchimg) # # MeanShift匹配模式 # elif track_mode == 'meanshift': # matchimg, center, prj_img = self.objmatch.do_meanshift(srcimg) # if display_process: # rstbmp = util.cvimg_to_wxbmp(prj_img) # else: # rstbmp = util.cvimg_to_wxbmp(matchimg) # # 多目标MeanShift匹配模式 # elif track_mode == 'multi-meanshift': # matchimg, center, prj_img = self.objmatch.do_multi_meanshift(srcimg, arg=self.multimean_arg) # if display_process: # rstbmp = util.cvimg_to_wxbmp(prj_img) # else: # rstbmp = util.cvimg_to_wxbmp(matchimg) # elif track_mode == 'gray-meanshift': # matchimg, center, prj_img = self.objmatch.do_gray_meanshift(srcimg) # if display_process: # rstbmp = util.cvimg_to_wxbmp(prj_img) # else: # rstbmp = util.cvimg_to_wxbmp(matchimg) # elif track_mode == 'optical-flow': # matchimg, center, prj_img = self.objmatch.do_optical_flow(srcimg) # if display_process: # rstbmp = util.cvimg_to_wxbmp(prj_img) # else: # rstbmp = util.cvimg_to_wxbmp(matchimg) # # 混合匹配模式 # elif track_mode == 'mix': # matchimg, center, _ = self.objmatch.do_mix(srcimg, multimean_arg=self.multimean_arg, edgetpl_arg=self.edge_arg) # rstbmp = util.cvimg_to_wxbmp(matchimg) # 更新track bitmap 界面 memtrack.SelectObject(rstbmp) memtrack.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_track_size[0]), float(srcimg.shape[0])/float(self.bitmap_track_size[1])) self.dc_track.Blit(0, 0, self.bitmap_track_size[0], self.bitmap_track_size[1], memtrack, 0, 0) a = time.clock() if TRACK_OBJECT in worklist: self.trackctrl.add_pt(center) self.trackctrl.update_h(self.UAVinfo.get()['height']) self.trackctrl.get_u() rstimg = self.objmatch.draw_circles(matchimg, self.trackctrl.pts[-1], color='GREEN', radius=10) rstbmp = util.cvimg_to_wxbmp(rstimg) memtrack.SelectObject(rstbmp) memtrack.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_track_size[0]), float(srcimg.shape[0])/float(self.bitmap_track_size[1])) self.dc_track.Blit(0, 0, self.bitmap_track_size[0], self.bitmap_track_size[1], memtrack, 0, 0) n = time.clock() # print('[work time]%4.4f [cir time]%4.4f'%((n-a)*1000,(n-self.lasttime)*1000)) self.lasttime = n
def main_work(self, event): worklist = self.worklist a = time.clock() # MAIN_TASK_FREQ Hz Tasks if USING_JOYSTICK in self.worklist: self.update_joy_status() if DISPLAY_VIDEO in worklist: srcimg = self.camcap.get_frame() if len(self.cambuf)==0: self.cambuf.append(srcimg) self.cambuf.append(srcimg) # self.cambuf.append(srcimg) # print('cambuf length:%d'%len(self.cambuf)) wxbmp = util.cvimg_to_wxbmp(srcimg) wximg = wx.ImageFromBitmap(wxbmp) memvideo = wx.MemoryDC() memvideo.SelectObject(wxbmp) if self.m_menuItem_video_osd.IsChecked(): # draw OSD information on bitmap_video memvideo.SetTextForeground( wx.BLUE ) memvideo.SetFont( util.WXFONT ) pos = (srcimg.shape[1] - util.PADDING - util.TIME_TEXT_WIDTH, util.PADDING) memvideo.DrawText(util.get_now(), pos[0], pos[1]) # 设置缩放比例 memvideo.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_video_size[0]), float(srcimg.shape[0])/float(self.bitmap_video_size[1]) ) self.dc_video.Blit(0, 0, self.bitmap_video_size[0], self.bitmap_video_size[1], memvideo, 0, 0) memvideo.SelectObject(wx.NullBitmap) if RECORD_VIDEO in worklist: self.mov_rec.save_frame(wxbmp) if DISPLAY_INDEPENDENT_VIDEO in worklist: self.video_window.update_image(wximg, self.UAVinfo.get_information_in_InfoEntries()) # 结束图像传输需要先停止track if DISPLAY_TRACK_VIDEO in worklist: memtrack = wx.MemoryDC() #图像滤波 self.cambuf.append(srcimg) self.cambuf.pop(0) a=time.clock() srcimg = img_filter(self.cambuf) # print '%.6f'%((time.clock()-a)*1000) # self.cambuf.append(srcimg) # self.cambuf.pop(0) # 显示原始图像 if self.display_track_state == DISPLAY_TRACK_STATE_RAW: #rstimg = self.get_adjusted_image(srcimg) rstimg = srcimg rstbmp = util.cvimg_to_wxbmp(rstimg) # 正在框选状态 elif self.display_track_state == DISPLAY_TRACK_STATE_SELECTION: assert self.frozen_frame is not None, 'Frozen frame is none.' rectimg = self.get_dragging_image(self.frozen_frame,self.drag_info.get_drag_data()) rstbmp = util.cvimg_to_wxbmp(rectimg) # 显示目标追踪结果 elif self.display_track_state == DISPLAY_TRACK_STATE_RESULT: track_mode = self.m_choice_track_mode.GetStringSelection() display_process = self.m_menuItem_track_display_process.IsChecked() if track_mode == 'template': method = METHOD.TEMPLATEMATCH elif track_mode == 'meanshift': method = METHOD.MEANSHIFT elif track_mode == 'gray-meanshift': method = METHOD.GRAYMEANSHIFT else: method = METHOD.OPTICALFLOW matchimg, center, res = self.objmatch.process(method, srcimg) if display_process: rstbmp = util.cvimg_to_wxbmp(res) tmpimg = res else: rstbmp = util.cvimg_to_wxbmp(matchimg) tmpimg = matchimg if TRACK_OBJECT in worklist: self.trackctrl.add_pt(center) # TODO:MeanShift-OpticalFlow 卡尔曼 # 更新track bitmap 界面 memtrack.SelectObject(rstbmp) memtrack.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_track_size[0]), float(srcimg.shape[0])/float(self.bitmap_track_size[1])) self.dc_track.Blit(0, 0, self.bitmap_track_size[0], self.bitmap_track_size[1], memtrack, 0, 0) a = time.clock() # 5 Hz Tasks if (time.clock()-self.timer.last_time) > (1.0/TASK_LOW_FREQ): self.timer.last_time=time.clock() if DISPLAY_XBEE_DATA in worklist: self.update_rcv_area() if USING_JOYSTICK in self.worklist: self.do_joy_control() # 发送MID=0x09时自动返回,不需重复发送。 elif DISPLAY_UAVINFO in self.worklist: self.send_data_by_frame(MsgPrcs.pack_control(0, self.state_smart_direction)) if DISPLAY_UAVINFO in self.worklist: self.update_GUI_UAVinfo(self.UAVinfo.get(-2)) if TRACK_OBJECT in worklist: now_height = self.UAVinfo.get().height self.trackctrl.update_h(3 if math.isnan(now_height) else now_height) nt = self.UAVinfo.get(-2).uavtime du = self.trackctrl.get_u(nt, self.camera_pt_pitch) rstimg = self.objmatch.draw_circles(tmpimg, self.trackctrl.pts[-1], color='GREEN', radius=10) rstbmp = util.cvimg_to_wxbmp(rstimg) memtrack.SelectObject(rstbmp) memtrack.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_track_size[0]), float(srcimg.shape[0])/float(self.bitmap_track_size[1])) self.dc_track.Blit(0, 0, self.bitmap_track_size[0], self.bitmap_track_size[1], memtrack, 0, 0) self.send_ref(du) n = time.clock() # print('[work time]%4.4f [cir time]%4.4f'%((n-a)*1000,(n-self.lasttime)*1000)) self.lasttime = n