class SmartCar:
# 自动模式
auto_mode = True
current_status = ''
jpeg_quality = 80
_human_boxes = []
draw_human_boxes = True
def __init__(self, *, debug_show=False):
self.car_op = CarOp()
self.auto_nav_er = AutoNav(self.car_op)
# self.orbslam2_connect = OrbSlam2Connector()
self.debug_show = debug_show
self.human_detect = Yolov3HumanDetector()
self.auto_watch_human = AutoWatchHuman(self.car_op)
self.recorder = Recorder()
self.notifier = Notifier(self.recorder)
self.work_thread = Thread(target=self.work_run)
self.work_thread.start()
self.record_thread = Thread(target=self.record_run)
self.record_thread.start()
def work_run(self):
while True:
time.sleep(1 / 3)
img = self.get_current_img(only_left=True)
if img is None:
continue
self.human_detect.push_frame(img)
boxes = self.human_detect.detect()
if len(boxes) > 0:
boxes = np.array(boxes, np.float32)
self._human_boxes = boxes.copy()
self.auto_watch_human.detect(img, boxes)
self.notifier.notice(notify_type.type_human, '发现人')
self.notifier.do_interest_mark()
else:
self._human_boxes = []
if self.debug_show:
img = self.get_current_img(only_left=True, draw_box=True)
img2 = img[:, :, ::-1]
cv2.imshow('viewerL', img2)
cv2.waitKey(1000 // 60)
def record_run(self):
while True:
time.sleep(1 / 20)
img = self.get_current_img(only_left=True)
if img is None:
continue
if self.notifier.can_record:
self.recorder.start_video_record(20)
else:
self.recorder.stop_video_record()
self.recorder.next_frame(img)
def what_i_need_do(self):
p = np.random.uniform(0, 1)
if p < 0.1:
a = 'patrol'
elif p > 0.9:
a = 'stop'
else:
a = 'idle'
return a
def ctrl_auto_mode(self, b=None):
'''
开启与关闭自动模式
:param b:
:return:
'''
if b is not None:
self.auto_mode = b
if self.auto_mode:
self.auto_nav_er.start()
else:
self.auto_nav_er.stop()
self.car_op.move_stop()
return self.auto_mode
def handle_motion_system(self, action=''):
if self.auto_mode:
print('please close auto mode to use handle mode')
return
if action == 'forward':
self.car_op.move_forward()
elif action == 'back':
self.car_op.move_back()
elif action == 'left':
self.car_op.move_left()
elif action == 'right':
self.car_op.move_right()
elif action == 'stop':
self.car_op.move_stop()
def handle_servohead(self, action=''):
if self.auto_mode:
print('please close auto mode to use handle mode')
return
if action == 'turn_left':
self.car_op.ctrl_servohead('turn_left')
elif action == 'turn_right':
self.car_op.ctrl_servohead('turn_right')
elif action == 'turn_up':
self.car_op.ctrl_servohead('turn_up')
elif action == 'turn_down':
self.car_op.ctrl_servohead('turn_down')
elif action == 'watch_forward':
self.car_op.ctrl_servohead('forward')
elif action == 'watch_left':
self.car_op.ctrl_servohead('left')
elif action == 'watch_right':
self.car_op.ctrl_servohead('right')
elif action == 'watch_top':
self.car_op.ctrl_servohead('top')
def run(self):
while True:
time.sleep(1.)
# if imgLR is not None:
# self.orbslam2_connect.get_pos(imgLR)
if self.car_op.offline:
print('offline...')
continue
if not self.auto_mode:
print('handling...')
continue
# 自动控制
cmd_id = self.what_i_need_do()
if self.current_status != cmd_id:
self.current_status = cmd_id
if cmd_id == 'idle':
print('idle...')
elif cmd_id == 'patrol':
print('patrol...')
self.auto_nav_er.start()
self.auto_nav_er.to('any')
elif cmd_id == 'stop':
print('stop...')
self.auto_nav_er.to('neighbor')
else:
raise AttributeError('Please check cmd code !')
def get_current_img(self, only_left=True, draw_box=False):
'''
返回当前图像，如果没有初始化成功，返回None
:return: np.array
'''
img = self.car_op.get_cam_img()
if img is None:
return None
if only_left:
img = np.split(img, 2, 1)[0]
if draw_box:
human_boxes = self._human_boxes.copy()
for box in human_boxes:
cv2.rectangle(img, tuple(box[:2]), tuple(box[2:]), (0, 255, 0))
return img
def get_current_jpg(self, only_left=True):
'''
返回当前图像的JPEG编码格式的字节串，如果没有初始化成功，返回None
:return: None or bytes
'''
img = self.get_current_img(only_left, draw_box=self.draw_human_boxes)
if img is not None:
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
_, data = cv2.imencode(
'.jpg', img, (cv2.IMWRITE_JPEG_QUALITY, self.jpeg_quality))
return bytes(data)
return None
