Esempio n. 1
0
    def update_GUI_UAVinfo(self, info):
        # update text info
        mem = wx.MemoryDC()
        mem.SetFont(util.WXFONT)
        
        csz = mem.GetTextExtent(' ')
        sz = (45*csz[0], 10*csz[1])
        
        self.m_bitmap_uavinfo.SetSize(sz)
        self.m_bitmap_uavinfo.CenterOnParent()
        mem.SelectObject(wx.BitmapFromImage(wx.ImageFromData(sz[0],sz[1],'\xf0'*sz[0]*sz[1]*3)))
        
        pos = (0,5)
        padding = csz[0]*4
        
        def write(s, color=None):
            if color is None:
                color = wx.BLUE
            mem.SetTextForeground(color)
            mem.DrawText(s, pos[0], pos[1])
            return (pos[0] + mem.GetTextExtent(s)[0] + padding, pos[1])
        
        def writeln(s, color=None):
            rtn =  write(s, color)
            return (0, rtn[1]+csz[1])
            
        def get_st_color(value):
            if value == 0:
                return wx.Colour(255,0,0)
            elif value == 2:
                return wx.Colour(255,255,0)
            elif value == 1:
                return wx.Colour(20,215,0)
            else:
                return wx.Colour(160,160,160)
        
        pos = writeln('--UAVInfo Display--    UAVTime :%9.3Fs'%info.uavtime)
        pos = writeln('', None)
        pos = writeln(' pitch  =%9.3Fd    Rpitch  =%9.3Fd'%(info.pitch, info.ref_pitch))
        pos = writeln(' roll   =%9.3Fd    Rroll   =%9.3Fd'%(info.roll, info.ref_roll))
        pos = writeln(' yaw    =%9.3Fd    Ryaw    =%9.3Fd'%(info.yaw, info.ref_yaw))
        pos = write(' height =%9.3Fm'%info.height,
                    wx.RED if self.UAVinfo.need_warning('height', info.height) else None)
        pos = writeln('Rheight =%9.3Fm\n'%(info.ref_height))
        pos = write(' volt   =%9.3FV'%info.volt,
                    wx.RED if self.UAVinfo.need_warning('volt', info.volt) else None)
        pos = writeln('Rthrust =%9.3F'%(info.ref_thrust))
        pos = writeln(' Spd(%5.2f,%5.2f)  RSpd(%5.2f,%5.2f)'%(info.spdx,info.spdy,info.rspdx,info.rspdy))
        pos = write(' JOY', get_st_color(info.st_ct))
        pos = write('MOTOR', get_st_color(info.st_mt))
        pos = write('AutoHeight', get_st_color(info.st_ah))
        pos = write('Smart Dir.', get_st_color(info.st_sd))

        self.dc_uavinfo.Blit(0, 0, sz[0],sz[1], mem, 0, 0)
        
        # update bitmap_atti
        attiimg = self.UAVinfo.get_attitude_img(info.pitch, info.roll, info.yaw)
        self.dc_attitude.DrawBitmap(util.cvimg_to_wxbmp(attiimg), 0, 0)        
    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
Esempio n. 3
0
    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