''' This demo show the communication interface of MR813 motion control board based on Lcm. Dependency: - robot_control_cmd_lcmt.py - robot_control_response_lcmt.py ''' import lcm import sys import os import time from threading import Thread, Lock import sys import rclpy 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.base_msg import BaseMsg # from utils.marker_client import MarkerRunner from task_1.task_1 import run_task_1 from task_2_5.task_2_5 import run_task_2_5 from task_3.task_3 import run_task_3, run_task_3_back from task_1.task_1 import run_task_1, run_task_1_back from task_2.task_2 import run_task_2, run_task_2_back from task_2_5.task_2_5 import run_task_2_5, run_task_2_5_back from task_4.task_4 import run_task_4, run_task_4_back from task_test.task_left_line import run_task_test from task_5.task_5 import run_task_5 from base_move.turn_degree import turn_degree, turn_degree_v2 from base_move.center_on_dual_tracks import center_on_dual_tracks from utils.log_helper import info pass_marker = True def main(): rclpy.init() # 新增：在主程序中统一初始化 ROS 2 上下文 Ctrl = Robot_Ctrl() Ctrl.run() msg = robot_control_cmd_lcmt() try: info("Recovery stand", "info") Ctrl.base_msg.stand_up() Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # time.sleep(100) # TEST # run_task_1(Ctrl, msg) # arrow_direction = run_task_2(Ctrl, msg, xy_flag=False) arrow_direction = 'right' # TEST # info(f"识别到箭头方向: {arrow_direction}", "info") # run_task_2_5(Ctrl, msg, direction=arrow_direction) # if arrow_direction == 'left': # run_task_4(Ctrl, msg) # else: # run_task_3(Ctrl, msg) turn_degree_v2(Ctrl, msg, degree=90, absolute=True) center_on_dual_tracks(Ctrl, msg, max_deviation=10.0, observe=False) run_task_5(Ctrl, msg, direction=arrow_direction) if arrow_direction == 'left': run_task_3_back(Ctrl, msg) else: run_task_4_back(Ctrl, msg) # run_task_2_5_back(Ctrl, msg, direction=arrow_direction) # run_task_2_back(Ctrl, msg) # run_task_1_back(Ctrl, msg) except KeyboardInterrupt: print("\n程序被用户中断") except Exception as e: print(f"发生错误: {e}") finally: print("正在清理资源...") Ctrl.quit() rclpy.shutdown() # 新增：在主程序中统一销毁 ROS 2 上下文 print("程序已退出") sys.exit() class Robot_Ctrl(object): def __init__(self): self.rec_thread = Thread(target=self.rec_responce) self.send_thread = Thread(target=self.send_publish) self.odo_thread = Thread(target=self.rec_responce_o) self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255") self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") self.lc_o = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") self.cmd_msg = robot_control_cmd_lcmt() self.rec_msg = robot_control_response_lcmt() 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 self.gait_ok = 0 self.runing = 1 # 新增: 校准相关变量 self.is_calibrated = False # 是否已完成校准 self.calibration_offset = [0, 0, 0] # 校准偏移量 # 新增: 基础消息 self.base_msg = BaseMsg(self, self.cmd_msg) def run(self): self.lc_r.subscribe("robot_control_response", self.msg_handler) self.lc_o.subscribe("global_to_robot", self.msg_handler_o) self.send_thread.start() self.rec_thread.start() self.odo_thread.start() self.image_processor.run() # self.marker_runner.run() def msg_handler(self, channel, data): self.rec_msg = robot_control_response_lcmt().decode(data) if self.rec_msg.order_process_bar >= 95: self.mode_ok = self.rec_msg.mode else: self.mode_ok = 0 def msg_handler_o(self, channel, data): self.odo_msg = localization_lcmt().decode(data) # 如果尚未校准，记录第一帧数据作为校准基准 if not self.is_calibrated: self.calibration_offset = self.odo_msg.xyz self.is_calibrated = True print(f"校准完成，基准值: {self.calibration_offset}") # 将接收到的 odo 数据减去校准基准值 self.odo_msg.xyz = [ self.odo_msg.xyz[0] - self.calibration_offset[0], self.odo_msg.xyz[1] - self.calibration_offset[1], self.odo_msg.xyz[2] - self.calibration_offset[2] ] # if self.odo_msg.timestamp % 100 == 0: # print(self.odo_msg.xyz, self.odo_msg.rpy, self.odo_msg.vxyz, self.odo_msg.omegaBody, self.odo_msg.vBody) def odo_reset(self): self.calibration_offset = self.odo_msg.xyz def rec_responce(self): while self.runing: self.lc_r.handle() time.sleep(0.002) def rec_responce_o(self): while self.runing: self.lc_o.handle() time.sleep(0.002) def Wait_finish(self, mode, gait_id): count = 0 while self.runing and count < 2000: # 10s if self.mode_ok == mode and self.gait_ok == gait_id: return True else: time.sleep(0.005) count += 1 print("Wait_finish timeout.") return False def send_publish(self): while self.runing: self.send_lock.acquire() if self.delay_cnt > 20: # Heartbeat signal 10HZ self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode()) self.delay_cnt = 0 self.delay_cnt += 1 self.send_lock.release() time.sleep(0.005) def Send_cmd(self, msg): self.send_lock.acquire() self.delay_cnt = 50 self.cmd_msg = msg self.cmd_msg.life_count %= 127 self.send_lock.release() def place_marker(self, x, y, z, color, observe=False): return None return None """调用 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}") # Main function if __name__ == '__main__': main()