IROS 任务代码复盘：人形机器人状态机与赛场执行链路

def run(self):
    class State:
        WAIT_FOR_COMMAND = "wait_for_command"
        MISSION_2 = "mission_2"
        M3_START = "m3_start"
        M3_NAV_A = "m3_nav_a"
        M3_PROCESS_A = "m3_process_a"
        M3_PREP_B = "m3_prep_b"
        M3_NAV_B = "m3_nav_b"
        M3_PROCESS_B = "m3_process_b"
        M3_FINISH = "m3_finish"
        FINISHED = "finished"

    current_state = State.WAIT_FOR_COMMAND
    while current_state != State.FINISHED:
        if current_state == State.WAIT_FOR_COMMAND:
            self.tasks = self._wait_for_vehicle_command()
            if SKIP_MISSION_2:
                current_state = State.M3_START
            else:
                current_state = State.MISSION_2

def _wait_for_vehicle_command(self):
    HOST = '0.0.0.0'
    PORT = 6000
    COLOR_MAP = {'。: '红色', '。: '绿色', '。: '蓝色'}
    tasks = []
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
        s.bind((HOST, PORT))
        s.listen(1)
        s.settimeout(60.0)
        conn, addr = s.accept()
        with conn:
            data = conn.recv(1024)
            received_string = data.decode('utf-8')
            instruction_pairs = [received_string[i:i+2] for i in range(0, len(received_string), 2)]
            for pair in instruction_pairs:
                color_char, target_char = pair[0], pair[1]
                if color_char in COLOR_MAP:
                    tasks.append((COLOR_MAP[color_char], target_char))
    if not tasks:
        tasks = [("红色", "A"), ("蓝色", "B")]
    return tasks

def mission_2_obstacle_crossing(self):
    if not self._execute_motion("向前走路1", 3, description="向前走两步，接近台阶", silent_tts=True):
        raise Exception("Sequence Failed")
    self.correct_heading(with_grab=False)
    if not self._execute_motion("climb3", 1, description="上第一级台。", speed="normal", silent_tts=True):
        raise Exception("Sequence Failed")
    self.correct_heading(with_grab=False)
    if not self._execute_motion("down3", 1, description="下第一级台。", speed="normal", silent_tts=True):
        raise Exception("Sequence Failed")
    self.correct_heading(with_grab=False)

    if HURDLE_POSITION == "LEFT":
        self._execute_motion("左走一。", 2, description="向左平移，对准左侧栏。", silent_tts=True)
    elif HURDLE_POSITION == "RIGHT":
        self._execute_motion("右走一。", 2, description="向右平移，对准右侧栏。", silent_tts=True)
    else:
        self._execute_motion("向前走路1", 1, description="向前走三步，对准中央栏架", speed="normal", silent_tts=True)
    self.correct_heading(with_grab=False)
    self._execute_motion("cross3", 1, description="执行跨越动作", speed="normal", silent_tts=True)

def _align_to_nav_tag(self, target_distance_m):
    target_pos = None
    all_tags = [{"id": tag_id, "size": self.APRILTAG_SIZE_M} for tag_id in [0, 1, 2, 3, 4]]
    YanAPI.start_aprilTag_recognition(tags=all_tags)
    status = YanAPI.get_aprilTag_recognition_status()
    for tag in status['data']['AprilTagStatus']:
        if tag['id'] == 0:
            target_pos = (tag['position-x'], tag['position-z'])
            break
    YanAPI.stop_aprilTag_recognition()

    pos_x, pos_z = target_pos
    if abs(pos_x) > 0.02:
        steps = max(1, int(abs(pos_x) / 0.06))
        direction = "left" if pos_x < 0 else "right"
        self._execute_motion("walk", steps, direction=direction, description="水平调整", silent_tts=True)
        self.correct_heading(with_grab=False)

def correct_heading(self, with_grab=False):
    return self.turn(self.target_heading, with_grab=with_grab)

def _scan_for_baskets(self):
    raw_measurements = {}
    scan_angles = [60, 90, 120]
    tags_to_detect = [{"id": tag_id, "size": self.APRILTAG_SIZE_M} for tag_id in [1, 2, 3]]
    YanAPI.start_aprilTag_recognition(tags=tags_to_detect)

    for angle_deg in scan_angles:
        self._set_head_angle(angle_deg)
        status = YanAPI.get_aprilTag_recognition_status()
        for tag in status['data']['AprilTagStatus']:
            tag_id = tag['id']
            if tag_id not in raw_measurements:
                cam_x, cam_z = tag['position-x'], tag['position-z']
                head_angle_rad = math.radians(angle_deg - 90)
                body_x = cam_x * math.cos(-head_angle_rad) - cam_z * math.sin(-head_angle_rad)
                body_z = cam_x * math.sin(-head_angle_rad) + cam_z * math.cos(-head_angle_rad)
                raw_measurements[tag_id] = {'x': body_x, 'z': body_z}

    prediction_weight = 0.6
    observation_weight = 0.4
    final_map = {}
    for tag_id in raw_measurements:
        final_x = predicted_map[tag_id]['x'] * prediction_weight + raw_measurements[tag_id]['x'] * observation_weight
        final_z = predicted_map[tag_id]['z'] * prediction_weight + raw_measurements[tag_id]['z'] * observation_weight
        final_map[tag_id] = {'x': final_x, 'z': final_z}

def is_fallen(self):
    gyro_data = YanAPI.get_sensors_gyro()
    current_euler_x = gyro_data['data']['gyro'][0]['euler-x']
    current_euler_y = gyro_data['data']['gyro'][0]['euler-y']
    delta_x = abs(current_euler_x - self.standing_euler_x)
    delta_y = abs(current_euler_y - self.standing_euler_y)
    if delta_x > self.FALL_DOWN_THRESHOLD or delta_y > self.FALL_DOWN_THRESHOLD:
        return True
    return False

while current_state != State.FINISHED:
    try:
        if current_state == State.WAIT_FOR_COMMAND:
            self.tasks = self._wait_for_vehicle_command()
            self.color_a, self.platform_a = self.tasks[0]
            self.color_b, self.platform_b = self.tasks[1]
    except FallDownException as e:
        time.sleep(15.0)
        self.calibrate_initial_heading()
        print("  - 恢复完成，将重试当前状。 {}".format(current_state))