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
