Compare commits
1 Commits
main
...
no-ros-mar
Author | SHA1 | Date | |
---|---|---|---|
a36ce7ebb0 |
42
main.py
42
main.py
@ -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__':
|
||||||
|
Loading…
x
Reference in New Issue
Block a user