def __init__(self):
self.lasttime = time.clock()
super(GroundStation, self).__init__(parent = None)
self.SetIcon(wx.Icon(r'resources/gs.ico', wx.BITMAP_TYPE_ICO))
#---- init status bar system ----
self.sbar = StatusBarSystem(self.m_statusBar)
#---- add Google Earth Components ----
sizer_ge = self.m_panel_route.GetSizer()
self.browser_ge = wx.html2.WebView.New(self.m_panel_route, size=(330,330))
sizer_ge.Add(self.browser_ge, 1, wx.ALIGN_CENTER, 0)
self.browser_ge.LoadURL(r'file:///%s/resources/ge.html'%os.getcwd().replace('\\','/'))
self.GE_uninited = True
#---- set record labels ----
self.m_filePicker_output.GetPickerCtrl().SetLabel(u'设置视频参数')
#---- add component attributes ----
for comp in [self.m_button_toggle_track,
self.m_button_toggle_track_video,
self.m_button_toggle_video,
self.m_button_toggle_xbee,
self.m_button_update_uavinfo,
self.m_button_select_object,
self.m_button_record,
self.m_button_toggle_joystick,
]:
comp.__setattr__('is_running', False)
#---- init comm block ----
self.comm = XBee(self)
self.comm_options = {}
self.load_default_comm_options()
self.history = InputHistory(self)
self.enable_comm_relative_components(False)
#---- init video block ----
self.cap_dev_num = 1
self.enable_video_components(False)
self.video_window = None
#---- init track block ----
self.enable_track_components(False)
self.display_track_state = None
#---- init parameter adjust block ----
self.init_para_block()
#---- init size variables ----
self.bitmap_track_size = tuple(self.m_bitmap_track.GetSize())
self.bitmap_video_size = tuple(self.m_bitmap_video.GetSize())
#---- init UAVInformation ----
self.UAVinfo = UAVInfomation()
#---- init UAV Control ----
self.init_uav_control()
# --- dc init ---
self.dc_video = wx.ClientDC(self.m_bitmap_video)
self.dc_track = wx.ClientDC(self.m_bitmap_track)
self.memory = wx.MemoryDC()
self.dc_attitude = wx.ClientDC(self.m_bitmap_attitude)
self.dc_uavinfo = wx.ClientDC(self.m_bitmap_uavinfo)
# --- test ---
self.multimean_arg = None
self.edge_arg = None
self.init_worklist()
#self.Bind(wx.EVT_IDLE, self.main_work)
self.timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.main_work, self.timer)
self.timer.Start(1000.0/MAIN_TASK_FREQ)
self.timer.last_time=time.clock()
# self.timer1 = wx.Timer(self)
# self.Bind(wx.EVT_TIMER, self.main_work1, self.timer1)
# self.timer1.Start(1000/10)
self.Bind(wx.EVT_CLOSE, self.OnClose)
class GroundStation(FrameGroundStationBase, WorkBlock ,TrackBlock, VideoBlock,
ButtonBlock, MenuBlock, CommBlock, ParameterAdjustBlock,
UAVCtrlBlock
):
def __init__(self):
self.lasttime = time.clock()
super(GroundStation, self).__init__(parent = None)
self.SetIcon(wx.Icon(r'resources/gs.ico', wx.BITMAP_TYPE_ICO))
#---- init status bar system ----
self.sbar = StatusBarSystem(self.m_statusBar)
#---- add Google Earth Components ----
sizer_ge = self.m_panel_route.GetSizer()
self.browser_ge = wx.html2.WebView.New(self.m_panel_route, size=(330,330))
sizer_ge.Add(self.browser_ge, 1, wx.ALIGN_CENTER, 0)
self.browser_ge.LoadURL(r'file:///%s/resources/ge.html'%os.getcwd().replace('\\','/'))
self.GE_uninited = True
#---- set record labels ----
self.m_filePicker_output.GetPickerCtrl().SetLabel(u'设置视频参数')
#---- add component attributes ----
for comp in [self.m_button_toggle_track,
self.m_button_toggle_track_video,
self.m_button_toggle_video,
self.m_button_toggle_xbee,
self.m_button_update_uavinfo,
self.m_button_select_object,
self.m_button_record,
self.m_button_toggle_joystick,
]:
comp.__setattr__('is_running', False)
#---- init comm block ----
self.comm = XBee(self)
self.comm_options = {}
self.load_default_comm_options()
self.history = InputHistory(self)
self.enable_comm_relative_components(False)
#---- init video block ----
self.cap_dev_num = 1
self.enable_video_components(False)
self.video_window = None
#---- init track block ----
self.enable_track_components(False)
self.display_track_state = None
#---- init parameter adjust block ----
self.init_para_block()
#---- init size variables ----
self.bitmap_track_size = tuple(self.m_bitmap_track.GetSize())
self.bitmap_video_size = tuple(self.m_bitmap_video.GetSize())
#---- init UAVInformation ----
self.UAVinfo = UAVInfomation()
#---- init UAV Control ----
self.init_uav_control()
# --- dc init ---
self.dc_video = wx.ClientDC(self.m_bitmap_video)
self.dc_track = wx.ClientDC(self.m_bitmap_track)
self.memory = wx.MemoryDC()
self.dc_attitude = wx.ClientDC(self.m_bitmap_attitude)
self.dc_uavinfo = wx.ClientDC(self.m_bitmap_uavinfo)
# --- test ---
self.multimean_arg = None
self.edge_arg = None
self.init_worklist()
#self.Bind(wx.EVT_IDLE, self.main_work)
self.timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.main_work, self.timer)
self.timer.Start(1000.0/MAIN_TASK_FREQ)
self.timer.last_time=time.clock()
# self.timer1 = wx.Timer(self)
# self.Bind(wx.EVT_TIMER, self.main_work1, self.timer1)
# self.timer1.Start(1000/10)
self.Bind(wx.EVT_CLOSE, self.OnClose)
#------ Menu binding function ------
def on_about(self, event):
self.about()
def on_save_comm_option(self, event):
self.save_comm_option(self.comm_options)
def on_load_comm_option(self, event):
self.load_comm_option()
def on_check_comm_option(self, event):
self.load_default_comm_options()
self.display_comm_option(self.comm_options)
#------ Button binding function ------
def on_xbee_option(self, event):
self.show_xbee_option()
def on_toggle_xbee(self, event):
if event.GetEventObject().is_running:
self.close_xbee(event.GetEventObject())
else:
self.open_xbee(event.GetEventObject())
def on_toggle_video(self, event):
if event.GetEventObject().is_running:
self.close_video(event.GetEventObject())
else:
self.open_video(event.GetEventObject())
def on_video_comm_option(self, event):
self.show_video_option()
def on_update_uavinfo(self, event):
if event.GetEventObject().is_running:
self.unshow_uavinfo(event.GetEventObject())
else:
self.show_uavinfo(event.GetEventObject())
def on_save_uav_info(self, event):
self.UAVinfo.save_to_file(self, pidpara = self.get_para_from_table())
if self.m_menuItem_clear_uav_info_after_save.IsChecked():
self.UAVinfo.clear_buf()
self.update_GUI_UAVinfo(self.UAVinfo.get())
def on_clear_uav_info(self, event):
self.UAVinfo.clear_buf()
self.update_GUI_UAVinfo(self.UAVinfo.get())
#------ Video Binding Function ------
def on_video_window_show(self, event):
self.show_independent_video()
def on_record(self, event):
if event.GetEventObject().is_running:
self.stop_record(event.GetEventObject())
else:
self.start_record(event.GetEventObject())
def on_record_file_changed(self, event):
self.init_record(self.m_filePicker_output.GetPath())
def on_enter_bitmap_video(self, event):
self.sbar.update(u'提示：单击右键可选择OSD选项')
def on_leave_bitmap_video(self, event):
self.sbar.backward()
#------ UAV Control Binding Function ------
def on_PT_send(self, event):
rtn = self.send_pt_reference()
self.sbar.update(u'云台控制信息已经发送(%d)'%rtn)
def on_toggle_joystick(self, event):
if event.GetEventObject().is_running:
self.close_joystick(event.GetEventObject())
else:
self.open_joystick(event.GetEventObject())
def on_toggle_smart_direction(self, event):
self.toggle_smart_direction(self.m_checkBox_smart_direction.IsChecked())
#------ Track Binding Function ------
def on_toggle_track_video(self, event):
if event.GetEventObject().is_running:
self.close_track_video(event.GetEventObject())
else:
self.open_track_video(event.GetEventObject())
def on_radiobox_adjust(self, event):
self.change_adjust_type()
def on_slider_adjust_changed(self, event):
self.change_adjust_value()
def on_select_object(self, event):
self.toggle_drag_selection(True)
def on_toggle_track(self, event):
if event.GetEventObject().is_running:
self.stop_track(event.GetEventObject())
else:
self.start_track(event.GetEventObject())
def on_track_bitmap_left_down(self, event):
self.start_drag(event)
def on_track_bitmap_left_up(self, event):
self.end_drag(event)
def on_track_bitmap_motion(self, event):
self.on_drag(event)
def on_track_arg_enter(self, event):
self.set_track_arg(event.GetEventObject().GetValue())
def on_enter_bitmap_track(self, event):
self.sbar.update(u'提示：单击右键可选择显示模式和分辨模式')
def on_leave_bitmap_track(self, event):
self.sbar.backward()
#------ Parameter Adjust Binding Function ------
def on_send_para(self, event):
self.send_para((self.m_checkBox_para_x.IsChecked(),
self.m_checkBox_para_y.IsChecked(),
self.m_checkBox_para_z.IsChecked(),
self.m_checkBox_para_h.IsChecked(),
self.m_checkBox_para_p.IsChecked(),
)
)
def on_load_para(self, event):
self.load_para()
def on_save_para(self, event):
self.save_para()
def on_set_down_para(self, event):
self.set_down_para()
#------ Communication Binding Function ------
def on_send_area_enter(self, event):
self.send_data_by_GUI()
def on_send_comm_click(self, event):
self.send_data_by_GUI()
def on_send_area_char(self, event):
keycode = event.GetKeyCode()
if keycode == wx.WXK_UP:
self.m_textCtrl_comm_send.SetValue(self.history.getByRel(-1))
self.m_textCtrl_comm_send.SetInsertionPoint(len(self.m_textCtrl_comm_send.GetValue()))
elif keycode == wx.WXK_DOWN:
self.m_textCtrl_comm_send.SetValue(self.history.getByRel(1))
self.m_textCtrl_comm_send.SetInsertionPoint(len(self.m_textCtrl_comm_send.GetValue()))
else:
FrameGroundStationBase.on_send_comm_char(self, event)
def on_clear_receive(self, event):
self.comm.clear_rcvbuf()
self.update_rcv_area(refresh = True)
def on_recv_style_choice(self, event):
self.update_rcv_area(refresh = True)
#------ CallAfter Function ------
def on_xbee_receive_char(self,rcvchr):
self.comm.on_receive_char(rcvchr)
msgtype, data = self.get_backdata()
if data:
self.process_backdata(msgtype, data)
#------ Work Function ------
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
#------ Tool Function ------
def update_GUI_UAVinfo(self, info):
# update text info
for k,v in info.iteritems():
if v is None:
info[k] = float('nan')
mem = wx.MemoryDC()
mem.SetFont(util.WXFONT)
csz = mem.GetTextExtent(' ')
sz = (45*csz[0], 9*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):
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):
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'], None)
pos = writeln('', None)
pos = writeln(' pitch =%9.3Fd Rpitch =%9.3Fd'%(info['pitch'], info['ref_pitch']), None)
pos = writeln(' roll =%9.3Fd Rroll =%9.3Fd'%(info['roll'], info['ref_roll']), None)
pos = writeln(' yaw =%9.3Fd Ryaw =%9.3Fd'%(info['yaw'], info['ref_yaw']), None)
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']), None)
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']), None)
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()
self.dc_attitude.DrawBitmap(util.cvimg_to_wxbmp(attiimg), 0, 0)
def update_GE(self, info):
la = info['la']
lo = info['lo']
if self.GE_uninited:
js = util.JSCODESINIT%(la,lo, info['height']*1.2)
self.GE_uninited = False
else:
js = util.JSCODES%(la,lo, info['height']*1.2)
self.browser_ge.RunScript(js)
def get_hist_channel(self):
if self.m_menuItem_track_hist_h.IsChecked():
return [0]
elif self.m_menuItem_track_hist_s.IsChecked():
return [1]
elif self.m_menuItem_track_hist_l.IsChecked():
return [2]
def enable_video_components(self, switch):
for each in [self.m_button_video_window_show,
self.m_bitmap_video,
self.m_button_record,
self.m_filePicker_output
]:
each.Enable(switch)
self.m_button_record.Enable(False)
def OnClose(self, event):
print('Close window...')
self.timer.Stop()
# try:
# self.camcap.release()
# except AttributeError:
# print('No camcap, release failed.')
if self.m_button_toggle_xbee.is_running:
self.close_xbee(self.m_button_toggle_xbee)
if self.m_button_toggle_track_video.is_running:
self.close_video(self.m_button_toggle_track_video)
self.Destroy()
