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
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