import sys # NOQA: E402
|
import os
|
import threading
|
import time
|
|
from .DustCleaner import DustCleaner
|
|
# add python path of src to sys.path
|
src_path = os.path.join(__file__, *(['..'] * 2))
|
src_path = os.path.abspath(src_path)
|
sys.path.insert(0, src_path)
|
|
from protos import aerial_pb2 as pb
|
|
|
|
class DustCleanerThread (threading.Thread):
|
"""
|
dust cleaner threads
|
"""
|
def __init__(self, cmd_queue, config):
|
threading.Thread.__init__(self)
|
self.config = config
|
self.cmd_queue = cmd_queue
|
self.stop = False
|
self.cleaner = DustCleaner("com11", 115200, 1)
|
self._init_dust_cleaner()
|
|
def _init_dust_cleaner(self):
|
"""init dust cleaner"""
|
# send dust cleaner self clean command
|
self.cleaner.Send_self_clean_params(
|
self.config.clean_self_start_angle,
|
self.config.clean_self_end_angle,
|
self.config.clean_self_num)
|
# send dust cleaner period clean self command
|
self.cleaner.Send_period_clean_self(self.config.clean_self_period)
|
# send dust cleaner camera clean command
|
self.cleaner.Send_camera_clean_params(
|
self.config.clean_lens_start_angle,
|
self.config.clean_lens_end_angle,
|
self.config.clean_lens_num)
|
# send dust cleaner period clean camera command
|
self.cleaner.Send_period_clean_camera(self.config.clean_lens_period)
|
|
def run(self):
|
print("DustCleanerThread started")
|
while not self.stop:
|
# check if there is a command in the queue
|
if not self.cmd_queue.empty():
|
cmd = self.cmd_queue.get()
|
dust_cleaner_msg = pb.DustCleanerMessage()
|
dust_cleaner_msg.ParseFromString(cmd.data1.data)
|
if cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_ONETIME_CLEAN_SELF:
|
self.cleaner.Send_onetime_clean_self()
|
elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_ONETIME_CLEAN_CAMERA:
|
self.cleaner.Send_onetime_clean_camera()
|
elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_CLOSE_PERIOD_CLEAN_SELF:
|
self.cleaner.Send_close_period_clean_self()
|
elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_CLOSE_PERIOD_CLEAN_CAMERA:
|
self.cleaner.Send_close_period_clean_camera()
|
elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_PERIOD_CLEAN_SELF:
|
self.cleaner.Send_close_period_clean_self(dust_cleaner_msg.period)
|
elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_PERIOD_CLEAN_CAMERA:
|
self.cleaner.Send_period_clean_camera(dust_cleaner_msg.period)
|
time.sleep(0.1)
|
print("DustCleanerThread stopped")
|
