Compare commits

...

1 Commits

Author SHA1 Message Date
a36ce7ebb0 🎏 remove ros marker 2025-05-23 06:59:25 +00:00

42
main.py
View File

@ -17,7 +17,7 @@ from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt
from utils.robot_control_response_lcmt import robot_control_response_lcmt from utils.robot_control_response_lcmt import robot_control_response_lcmt
from utils.localization_lcmt import localization_lcmt from utils.localization_lcmt import localization_lcmt
from utils.image_raw import ImageProcessor from utils.image_raw import ImageProcessor
from utils.marker_client import MarkerRunner # from utils.marker_client import MarkerRunner
from utils.base_msg import BaseMsg from utils.base_msg import BaseMsg
from task_1.task_1 import run_task_1 from task_1.task_1 import run_task_1
@ -75,7 +75,7 @@ class Robot_Ctrl(object):
self.odo_msg = localization_lcmt() self.odo_msg = localization_lcmt()
self.image_processor = ImageProcessor() self.image_processor = ImageProcessor()
# DEBUG # DEBUG
self.marker_runner = MarkerRunner(pass_flag=pass_marker) # self.marker_runner = MarkerRunner(pass_flag=pass_marker)
self.send_lock = Lock() self.send_lock = Lock()
self.delay_cnt = 0 self.delay_cnt = 0
self.mode_ok = 0 self.mode_ok = 0
@ -95,7 +95,7 @@ class Robot_Ctrl(object):
self.rec_thread.start() self.rec_thread.start()
self.odo_thread.start() self.odo_thread.start()
self.image_processor.run() self.image_processor.run()
self.marker_runner.run() # self.marker_runner.run()
def msg_handler(self, channel, data): def msg_handler(self, channel, data):
self.rec_msg = robot_control_response_lcmt().decode(data) self.rec_msg = robot_control_response_lcmt().decode(data)
@ -160,30 +160,30 @@ class Robot_Ctrl(object):
self.cmd_msg = msg self.cmd_msg = msg
self.send_lock.release() self.send_lock.release()
def place_marker(self, x, y, z, color, observe=False): # def place_marker(self, x, y, z, color, observe=False):
"""调用 MarkerRunner 放置标志物""" # """调用 MarkerRunner 放置标志物"""
if self.marker_runner is None or self.marker_runner.marker_client is None: # if self.marker_runner is None or self.marker_runner.marker_client is None:
print("MarkerRunner 未初始化,无法放置标志物") # print("MarkerRunner 未初始化,无法放置标志物")
return None # return None
try: # try:
response = self.marker_runner.send_request(x, y, z, color, observe=observe) # response = self.marker_runner.send_request(x, y, z, color, observe=observe)
print(f"放置标志物结果: {response.success}, 消息: {response.message}") # print(f"放置标志物结果: {response.success}, 消息: {response.message}")
return response # return response
except Exception as e: # except Exception as e:
print(f"放置标志物时发生异常: {e}") # print(f"放置标志物时发生异常: {e}")
return None # return None
def quit(self): def quit(self):
self.runing = 0 self.runing = 0
self.rec_thread.join() self.rec_thread.join()
self.send_thread.join() self.send_thread.join()
self.image_processor.destroy() self.image_processor.destroy()
# 销毁 MarkerRunner # # 销毁 MarkerRunner
if hasattr(self, 'marker_runner') and self.marker_runner is not None: # if hasattr(self, 'marker_runner') and self.marker_runner is not None:
try: # try:
self.marker_runner.destroy() # self.marker_runner.destroy()
except Exception as e: # except Exception as e:
print(f"MarkerRunner 销毁失败: {e}") # print(f"MarkerRunner 销毁失败: {e}")
# Main function # Main function
if __name__ == '__main__': if __name__ == '__main__':