def __init__(self):
logging.basicConfig(filename='LiDAR.log', filemode='w', level=logging.INFO)
logging.info("Initializing RPLidar")
###############
self.lidar = None
self.lidar_client = None
self.lidar_thread_client = None
self.lidar_run_flag = False
self.lidar_state = []
self.lidar_data = []
self.lidar_connect = False
###############
try:
logging.info("Initializing Lidar_client")
self.lidar_client = rover_socket.TCP_client(50004)
self.lidar_thread_client = rover_socket.UDP_client(50005)
self.lidar_scan_port()
if self.lidar_connect:
self.lidar_client.send_list(['L', 'status', str(self.lidar_state[0])])
self.lidar_run_flag = True
self.lidar_main()
else:
print(("LiDAR is not initialized"))
logging.info("LiDAR is not initialized")
except:
traceback.print_exc()
logging.exception("Got error\n")
if self.lidar_client != None:
self.lidar_client.close()
if self.lidar_thread_client != None:
self.lidar_thread_client.close()
def get_lidar_data(lidar_data, get_lidar_data_run):
lidar = rover_socket.UDP_client(50010, 0, '192.168.5.2')
run = True
while run:
try:
lidar.send_list([1])
data = lidar.recv_list(65535)
if data is not None:
if lidar_data.qsize() > 0:
lidar_data.get()
lidar_data.put(data)
else:
lidar_data.put(data)
# Check if queue, get_lidar_data_run has an input of Flase.
try:
run = get_lidar_data_run.get(False)
if not run:
lidar.stop()
print("Lidar stop")
break
except queue.Empty:
pass
except KeyboardInterrupt:
lidar.close
print("Lidar stopped by KeyboardInterrupt")
except:
traceback.print_exc()
lidar.close()
finally:
run = False
print("Lidar disconnect")
def init(self):
''' Initialize communication with vision module and tcn_bridge'''
try:
time.sleep(0.2) # Make sure server initialize first
logging.basicConfig(filename='Vision_main.log',
filemode='w',
level=logging.INFO)
self.vision = xmlrpclib.ServerProxy("http://{}:8080".format(
self.ip))
self.vision_thread_client = rover_socket.UDP_client(50003)
self.vision_client = rover_socket.TCP_client(50002)
if self.vision.alive() == [0, 'Alive']:
logging.info(
'Connection to Vision module establiished , Vision module status : {}\n'
.format(self.vision.alive()))
self.vision_client.send_list(['V', 'next'])
self.vision_client_run = True
else:
logging.info('Vision module is not Alive\n')
raise KeyboardInterrupt
except:
self.end()
self.end_background_thread()
traceback.print_exc()
logging.exception('Got error : ')
def KeyboardControlBtn_click(self):
def controller():
if keyboard.is_pressed('w') and not keyboard.is_pressed(
'a') and not keyboard.is_pressed('d'):
self.gui_keyboard_control_client.send_list(['w'])
elif keyboard.is_pressed('w') and keyboard.is_pressed(
'a') and not keyboard.is_pressed('d'):
self.gui_keyboard_control_client.send_list(['wa'])
elif keyboard.is_pressed('w') and not keyboard.is_pressed(
'a') and keyboard.is_pressed('d'):
self.gui_keyboard_control_client.send_list(['wd'])
elif keyboard.is_pressed('s') and not keyboard.is_pressed(
'a') and not keyboard.is_pressed('d'):
self.gui_keyboard_control_client.send_list(['s'])
elif keyboard.is_pressed('s') and keyboard.is_pressed(
'a') and not keyboard.is_pressed('d'):
self.gui_keyboard_control_client.send_list(['sa'])
elif keyboard.is_pressed('s') and not keyboard.is_pressed(
'a') and keyboard.is_pressed('d'):
self.gui_keyboard_control_client.send_list(['sd'])
elif keyboard.is_pressed('d') and not keyboard.is_pressed(
'w') and not keyboard.is_pressed('s'):
self.gui_keyboard_control_client.send_list(['d'])
elif keyboard.is_pressed('a') and not keyboard.is_pressed(
'w') and not keyboard.is_pressed('s'):
self.gui_keyboard_control_client.send_list(['a'])
self.gui.KeyUp.setStyleSheet("background-color: rgb(0, 255, 0);") \
if keyboard.is_pressed('w') else \
self.gui.KeyUp.setStyleSheet("background-color: rgb(255, 255, 255);")
self.gui.KeyDown.setStyleSheet("background-color: rgb(0, 255, 0);") \
if keyboard.is_pressed('s') else \
self.gui.KeyDown.setStyleSheet("background-color: rgb(255, 255, 255);")
self.gui.KeyRight.setStyleSheet("background-color: rgb(0, 255, 0);") \
if keyboard.is_pressed('d') else \
self.gui.KeyRight.setStyleSheet("background-color: rgb(255, 255, 255);")
self.gui.KeyLeft.setStyleSheet("background-color: rgb(0 255, 0);") \
if keyboard.is_pressed('a') else \
self.gui.KeyLeft.setStyleSheet("background-color: rgb(255, 255, 255);")
self.gui_keyboard_control_client = rover_socket.UDP_client(
50011, 0, '192.168.5.2')
self.KeyboardControlTimer = QtCore.QTimer()
self.KeyboardControlTimer.timeout.connect(controller)
if not self.keyboard_control_run:
self.CALCULATOR.show_message('Keyboard Control Start')
self.KeyboardControlTimer.start(50)
self.keyboard_control_run = True
else:
self.KeyboardControlTimer.stop()
self.gui_keyboard_control_client.close()
self.CALCULATOR.show_message('Keyboard Control Stop')
self.keyboard_control_run = False
def __init__(self):
os.environ["QT_AUTO_SCREEN_SCALE_FACTOR"] = "1"
app = QtWidgets.QApplication(sys.argv)
app.setAttribute(QtCore.Qt.AA_EnableHighDpiScaling)
MainWindow = QtWidgets.QMainWindow()
self.gui = GUI.Ui_MainWindow()
self.gui.setupUi(MainWindow)
self.SV = SharedVariables()
self.SV.gui = self.gui
self.CALCULATOR = Calculator(self.SV)
self.current_speed = 0
self.show_vision_status_dict = {
0: "Booting",
1: "Waiting for command",
2: "Loading data",
3: ":Locating, please move around",
4: ":Working normally",
5: ":Lost current position"
}
self.rover_run, self.lidar_server_run, self.vision_server_run, \
self.animation_run, self.vision_build_map_mode, self.vision_use_map_mode, \
self.SV.calibration_run, self.keyboard_control_run, self.vision_idle \
= False, False, False, False, False, False, False, False, False
self.check_status_rover_run_list = [
self.rover_run, self.vision_server_run, self.lidar_server_run
]
self.check_status_rover_Btn_list = [
self.gui.RoverMainOnOffBtn, self.gui.VisionOnOffBtn,
self.gui.LidarOnOffBtn
]
self.check_status_func_run_list = [
self.animation_run, self.vision_build_map_mode,
self.vision_use_map_mode, self.SV.calibration_run,
self.keyboard_control_run
]
self.check_status_func_Btn_list = [
self.gui.ShowMapBtn, self.gui.VisionBuildMapBtn,
self.gui.VisionUseMapBtn, self.gui.CalibrationBtn,
self.gui.KeyboardControlBtn
]
self.gui.StopAllBtn.clicked.connect(self.StopAllBtn_click)
self.gui.KeyboardControlBtn.clicked.connect(
self.KeyboardControlBtn_click)
self.gui.VisionBuildMapBtn.clicked.connect(
self.VisionBuildMapBtn_click)
self.gui.VisionBuildMapStopBtn.clicked.connect(
self.VisionBuildMapStopBtn_click)
self.gui.VisionUseMapBtn.clicked.connect(self.VisionUseMapBtn_click)
self.gui.VisionUseMapStopBtn.clicked.connect(
self.VisionUseMapStopBtn_click)
self.gui.ShowMapBtn.clicked.connect(self.show_map)
self.gui.ShowMap_AddBtn.clicked.connect(self.ShowMap_AddBtn_click)
self.gui.KeyboardControl_SetSpeedBtn.clicked.connect(
self.KeyboardControl_SetSpeedBtn_click)
self.gui.KeyBoardControl_speed.valueChanged.connect(
self.KeyBoardControl_speed_value_change)
self.gui.CalibrationBtn.clicked.connect(self.calibration)
self.gui_get_lidar_vision_client = rover_socket.UDP_client(
50010, 0, '192.168.5.2')
self.gui_get_rover_status_client = rover_socket.UDP_client(
50012, 0, '192.168.5.2')
self.gui_rover_command_client = rover_socket.UDP_client(
50013, 0, '192.168.5.2')
self.get_data_retry = 0
self.get_data_timer = QtCore.QTimer()
self.get_data_timer.timeout.connect(self.get_data_from_rover)
self.get_data_timer.start(40)
self.get_rover_status_retry = 0
self.get_rover_status_timer = QtCore.QTimer()
self.get_rover_status_timer.timeout.connect(self.get_rover_status)
self.get_rover_status_timer.start(40)
self.check_status_timer = QtCore.QTimer()
self.check_status_timer.timeout.connect(self.check_status)
self.check_status_timer.start(100)
self.show_map()
self.gui.tabWidget.setCurrentIndex(0)
# self.gui.tabWidget.currentChanged.connect(self.show_map)
MainWindow.show()
sys.exit(app.exec_())