mi-task/main.py

'''
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
'''
# sudo chown -R $USER:$USER /home/mi-task/
import lcm
import sys
import os
import time
from threading import Thread, Lock
import sys
import string
from datetime import datetime 
import rclpy
import cv2

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.speech_demo                 import speak
# from utils.marker_client import MarkerRunner

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, run_task_2_demo
from task_2.xiesi2         import xiesi2, xiesi2_back
from task_2_5.task_2_5     import run_task_2_5, run_task_2_5_back
from task_3.task_3         import run_task_3, run_task_3_back
from task_4.task_4         import run_task_4, run_task_4_back
from task_5.task_5         import run_task_5
from base_move.turn_degree import turn_degree_v2
from base_move.go_to_xy    import go_to_x_v2, go_to_y_v2
from utils.log_helper      import info
from utils.speech_demo     import speak
from Circle1.circle1       import circle1
from Circle2.circle2       import circle2

pass_marker = True
TIME_SLEEP = 5000  # TODO 比赛时改成 5000

from enum import Enum, auto

class TaskType(Enum):
    TASK = auto()           # 默认任务
    PASS_BAR = auto()       # TODO 测试低头高度；可以修改代码，和 RED-BAR 一块测试。
    YELLOW_LIGHT = auto()   # TODO 设定 yellow threshold
    RED_BAR = auto()        # TODO 设定 red threshold
    UP_AND_DOWN = auto()    # TODO 调整其完成从上坡到下坡
    STONE_ROAD = auto()     # TODO 调整其刚好走完石板路，不浪费时间。目前估测 distance 就是 4（m）
    #
    MOVE_TO_LINE = auto()   # TODO 直走逼近直线测试
    CENTER_ON_DUAL_TRACKS = auto()   # TODO 双轨道居中测试

TASK = TaskType.TASK

def main():
    rclpy.init()  # 新增：在主程序中统一初始化 ROS 2 上下文
    Ctrl = Robot_Ctrl()
    Ctrl.run()
    print('1')
    msg = robot_control_cmd_lcmt()
    print('2')
    Ctrl.voice.play_audio('ok')
    demo.play_audio(text)
    
    # try:
    #     # INFO Real Task
    #     # TAG task - 0
    #     info("Recovery stand", "info")
    #     Ctrl.base_msg.stand_up()
    #     Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat # INFO 但是正式比赛或许也有用
        
    #     if TASK == TaskType.PASS_BAR:
    #         from task_4.pass_bar import pass_bar
    #         # TEST #1: pass-bar
    #         pass_bar(Ctrl, msg)
    #     elif TASK == TaskType.YELLOW_LIGHT: # TODO image
    #         from task_3.task_3 import go_until_yellow_area
    #         # turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)
    #         go_until_yellow_area(Ctrl, msg, yellow_ratio_threshold=0.012, speed=0.3, max_time=30, observe=True)
    #     elif TASK == TaskType.RED_BAR:
    #         from task_4.task_4 import go_straight_until_red_bar
    #         go_straight_until_red_bar(Ctrl, msg, red_ratio_threshold=0.18, step_distance=0.3)
    #     elif TASK == TaskType.UP_AND_DOWN:
    #         from task_3.task_3 import go_straight_with_enhanced_calibration
    #         go_straight_with_enhanced_calibration(Ctrl, msg, distance = 5, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21])
    #     elif TASK == TaskType.STONE_ROAD:
    #         from task_3.task_3 import pass_stone
    #         pass_stone(Ctrl, msg, distance = 4.5, observe=False)
    #     elif TASK == TaskType.MOVE_TO_LINE:
    #         from base_move.move_base_hori_line import move_to_hori_line
    #         move_to_hori_line(Ctrl, msg, target_distance = 1.1, observe=False)
    #     elif TASK == TaskType.CENTER_ON_DUAL_TRACKS:
    #         from base_move.center_on_dual_tracks import center_on_dual_tracks
    #         center_on_dual_tracks(Ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3)
    #     else:
    #         pass
        
    #     if TASK != TaskType.TASK:
    #         # 如果不是 task 类型，直接返回
    #         pass
        
    #     # TAG task - 1
    #     run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)       

    #     # TAG task - 2
    #     arrow_direction = xiesi2(Ctrl, msg)

    #     # TAG task - 3 / 4 - part I
    #     # arrow_direction = 'left'       
    #     if arrow_direction == 'left':
    #         circle1(Ctrl, msg)
    #     else:
    #         circle2(Ctrl, msg)


    #     # TAG task - 5
    #     # Ctrl.base_msg.stand_up()          
    #     # turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
    #     # run_task_5(Ctrl, msg, direction=arrow_direction, observe=True, time_sleep=TIME_SLEEP)  # B区任务
    #     # Ctrl.base_msg.lie_down(wait_time=10000)
    #     # print('wait 30s')


    #     # TAG task - 3 / 4 - part II
    #     # Ctrl.base_msg.stand_up()   
    #     # if arrow_direction == 'left':
    #     #     from task_3.task_3 import go_straight_with_enhanced_calibration
    #     #     go_straight_with_enhanced_calibration(Ctrl, msg, distance = 5, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21])
    #     # else:
    #     #     from task_3.task_3 import pass_stone
    #     #     pass_stone(Ctrl, msg, distance = 4.5, observe=False)

    #     # TAG task - 2.5 - back
    #     #nrun_task_2_5_back(Ctrl, msg, direction=arrow_direction)
    #     # Ctrl.base_msg.lie_down(wait_time=10000)
    #     # print('wait 30s')

    #     # Ctrl.base_msg.stand_up()  
    #     # TAG task - 2 - back
    #     # run_task_2_back(Ctrl, msg)
    #     xiesi2_back(Ctrl, msg)
    #     # Ctrl.base_msg.lie_down(wait_time=10000)
    #     # print('wait 30s')

    #     # Ctrl.base_msg.stand_up()  
    #     # TAG task - 1 - back
    #     # run_task_1_back(Ctrl, msg, time_sleep=TIME_SLEEP)

         
    #     print('🏁 Task finished.')
    #     try:
    #         while rclpy.ok():
    #             executor.spin_once(timeout_sec=0.1)
    #             if getattr(node, '_shutdown_flag', False):
    #                 break
    #     except KeyboardInterrupt:
    #         pass
        
    except KeyboardInterrupt:
        print("\n程序被用户中断")
    except Exception as e:
        import traceback
        exc_type, exc_value, exc_tb = sys.exc_info()
        tb = traceback.extract_tb(exc_tb)
        if tb:
            last_call = tb[-1]
            print(f"发生错误: {e}（文件: {last_call.filename}, 行号: {last_call.lineno}, 函数: {last_call.name}）")
        else:
            print(f"发生错误: {e}")
        traceback.print_exc()
    finally:
        print("正在清理资源...")
        Ctrl.quit()
        print('3')
        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(save_flag=False) # INFO 这里控制是否开始保存图片
        # DEBUG
        # 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)
        self.voice=AudioDemo(self)

    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
        """调用 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()