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