Embodied Intelligent Functions Core Source Code

Please ensure that the web control program backend process is not running in the background. To close it, refer to the [Get Started Quickly - 2. Access the Terminal] section in [Disable the Auto-Start Web Control Backend Process]. After closing, please restart DofbotUltra!!!

Before starting this feature, you need to close the web control program backend process.

1. Course Content

1. The AI large model embodied intelligent gameplay is a composite feature involving the coordinated cooperation of multiple node programs. This course explains the core programs.

2. Source Code Package Structure

2.1 Package File Structure

The ROS package for AI large model embodied intelligence is largemodel. The package path is:

Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel
RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel

The package file structure is as follows:

Description of folders and files:

2.1.1 config

Configuration folder, stores configuration files.

• large_model_interface.yaml

  Large model interface configuration file, used to configure the model supplier API Key, Dify application API Key, large model parameters, and the Dify backend address.

• yahboom.yaml

  Node configuration file: used to configure parameters for core nodes.

2.1.2 largemodel

Source code folder, the core program for AI large model embodied intelligence.

• asr.py

  Speech recognition program file, used to run the speech recognition model and interact with the user.

• model_service.py

  Model server program file, used to call various model interfaces to implement the model inference architecture.

• action_service.py

  Action server program file, used to receive the action list requested by the model server, control the robot to move and play sounds.

2.1.3 launch

Launch file folder, used to store ROS2 node launch files.

• largemodel_control.launch.py

  Launch file for AI large model embodied intelligence: starts multiple nodes, supports voice interaction mode and text interaction mode.

2.1.4 resources_file

Stores system sound audio files.

2.1.5 utils

Components folder, stores program files for non-core functions.

• large_model_interface.py

  Large model interface program file, contains the underlying interfaces for calling large models on various platforms. Different platforms and models have different calling programs, which are uniformly managed in the interface file.

• mic_serial.py

  Voice module driver program file, used to drive the wake word function of the voice module.

• promot.py

  Large model prompt file: used to generate prompts for the execution layer large model.

• dify_client2.py

  Dify API functions, used for the robotic arm program to request local decision layer/execution layer Dify applications via the Dify application API Key.

2.2 Program Call Relationship Diagram

3. Speech Recognition Function

The speech recognition function includes VAD voice activity detection and speech-to-text conversion. The source code path is:

Raspberry Pi (inside Docker container), source path:

xxxxxxxxxx
/home/yahboom/dofbot_ultra_ws/src/llm/largemodel/largemodel/asr.py

RDK (running natively), source path:

xxxxxxxxxx
/home/sunrise/dofbot_ultra_ws/src/llm/largemodel/largemodel/asr.py

3.1 Program Flowchart

asr program flowchart.

3.2 VAD Voice Activity Detection

Implementation program: The listen_for_speech method in the ASRNode class.

Program explanation: Records audio in real-time through the specified microphone, uses VAD (Voice Activity Detection) to determine if the user is speaking. When a speech segment ends (continuous silence exceeds the set number of frames), stops recording and saves the valid speech content to a file.

Detailed logic:

1. Initialize the audio stream and configure parameters (sample rate, channels, etc.).

2. Continuously read audio frames and perform voice activity detection.

3. If speech start is detected, add audio frames to the buffer; if continuous silence exceeds the threshold (90 frames, approximately 1 second), end recording.

4. After recording ends, remove trailing silence and save valid speech as a WAV file.

5. If no valid speech is detected, do not save the file.

​x
  def listen_for_speech(self, mic_index=0):
        p = pyaudio.PyAudio()
        audio_buffer = []
        silence_counter = 0
        MAX_SILENCE_FRAMES = 90  # 30帧*30ms=900ms静音后停止 / Stop after 900ms of silence (30 frames * 30ms)
        speaking = False  # 语音活动标志 / Flag indicating speech activity
        frame_counter = 0  # 计数器 / Frame counter
        stream_kwargs = {
            "format": pyaudio.paInt16,
            "channels": 1,
            "rate": self.sample_rate,
            "input": True,
            "frames_per_buffer": self.frame_bytes,
        }
        if mic_index != 0:
            stream_kwargs["input_device_index"] = mic_index

        # 通过蜂鸣器提示用户讲话 / Prompt the user to speak via the buzzer
        self.pub_beep.publish(UInt16(data=1))
        time.sleep(0.5)
        self.pub_beep.publish(UInt16(data=0))

        try:
            # 打开音频流 / Open audio stream
            stream = p.open(**stream_kwargs)
            while True:
                if self.stop_event.is_set():
                    return False

                frame = stream.read(
                    self.frame_bytes, exception_on_overflow=False
                )  # 读取音频数据 / Read audio data
                is_speech = self.vad.is_speech(
                    frame, self.sample_rate
                )  # VAD检测 / VAD detection

                if is_speech:
                    # 检测到语音活动 / Detected speech activity
                    speaking = True
                    audio_buffer.append(frame)
                    silence_counter = 0
                else:
                    if speaking:
                        # 在语音活动后检测静音 / Detect silence after speech activity
                        silence_counter += 1
                        audio_buffer.append(
                            frame
                        )  # 持续记录缓冲 / Continue recording buffer

                        # 静音持续时间达标时结束录音 / End recording when silence duration meets the threshold
                        if silence_counter >= MAX_SILENCE_FRAMES:
                            break
                frame_counter += 1
                if frame_counter % 2 == 0:
                    self.get_logger().info("1" if is_speech else "-")
                    # print('1-' if is_speech else '0-', end='', flush=True)  # 实时状态显示方式 / Real-time status display
        finally:
            stream.stop_stream()
            stream.close()
            p.terminate()

        # 保存有效录音（去除尾部静音） / Save valid recording (remove trailing silence)
        if speaking and len(audio_buffer) > 0:
            # 裁剪最后静音部分 / Trim the last silent part
            clean_buffer = (
                audio_buffer[:-MAX_SILENCE_FRAMES]
                if len(audio_buffer) > MAX_SILENCE_FRAMES
                else audio_buffer
            )

            with wave.open(self.user_speechdir, "wb") as wf:
                wf.setnchannels(1)
                wf.setsampwidth(p.get_sample_size(pyaudio.paInt16))
                wf.setframerate(self.sample_rate)
                wf.writeframes(b"".join(clean_buffer))
                return True

3.3 ASR Speech Recognition

Implementation program: The ASR_conversion method in the ASRNode class converts the recording file to text.

Calls the speech recognition model interface function in the large_model_interface.py large model interface file.

Note: Text recognized by speech recognition that is less than 4 characters is considered invalid. This is to prevent falsely triggered content from being treated as valid content.

Program explanation:

1. If using oline_asr, call the corresponding method and check if the result is valid text (length greater than 4). If successful, return the recognized content; otherwise, log the error and return 'error'.

2. Otherwise, use SenseVoiceSmall_ASR for recognition and perform the same result determination and handling.

   The SenseVoiceSmall_ASR is a local model speech recognition method (requires GPU computing power).

xxxxxxxxxx
def ASR_conversion(self, input_file):
    if self.use_oline_asr:
        result=self.modelinterface.oline_asr(input_file)
        if result[0] == 'ok' and len(result[1]) > 4:
            return result[1]
        else:
            self.get_logger().error(f'ASR Error:{result[1]}')
            return 'error'
    else:
        result=self.modelinterface.SenseVoiceSmall_ASR(input_file)
        if result[0] == 'ok' and len(result[1]) > 4:
            return result[1]
        else:
            self.get_logger().error(f'ASR Error:{result[1]}')
            return 'error' 

4. Model Server Function

The implementation program is model_service.py, used to receive speech recognition results from voice interaction mode or terminal input from text interaction mode, implementing the large model inference logic. The source code path is:

Raspberry Pi (inside Docker container), source path:

xxxxxxxxxx
/home/yahboom/dofbot_ultra_ws/src/llm/largemodel/largemodel/model_service.py

RDK (running natively), source path:

xxxxxxxxxx
/home/sunrise/dofbot_ultra_ws/src/llm/largemodel/largemodel/model_service.py

4.1 Program Flowchart

model_service program flowchart

4.2 Dual Model Inference (Domestic Version)

The default uses dual model inference mode. The implementation programs are the dual_large_model_mode and instruction_process methods in the LargeModelService class.

Program explanation:

1. If it is a new instruction cycle (self.new_order_cycle is True):

   • Initialize multimodal history context.

   • Use the task decision model to perform task planning on the user input prompt.

   • Pass the planning result to the instruction execution layer model for processing.

   • Set new_order_cycle to False, indicating the current instruction cycle has started execution.

2. Otherwise (not a new cycle):

   • Directly pass the input prompt and type to the instruction execution layer model for processing.

xxxxxxxxxx
def dual_large_model_mode(self, type, prompt=""):
    """
    此函数实现了双模型推理模式，即先由文本生成模型进行任务规划，然后由多模态大模型生成动作列表
    This function implements the dual model inference mode, where the text generation model first plans the task, and then the multimodal large model generates the action list.
    """
    if (
        self.new_order_cycle
    ):  # 判断是否是新任务周期 / Determine if it is a new task cycle
        # 判断上一轮对话指令是否完成如果完成就清空历史上下文，开启新的上下文 / Determine if the previous round of dialogue instructions are completed. If completed, clear the historical context and start a new context
        self.model_client.init_Multimodal_history(
            get_prompt()
        )  # 初始化执行层上下文历史 / Initialize execution layer context history
        execute_instructions = self.model_client.TaskDecision(
            prompt
        )  # 调用决策层大模型进行任务规划 / Call the decision layer large model for task planning

        if not execute_instructions == "error":
            if self.text_chat_mode:
                msg = String(
                    data=f"The upcoming task to be carried out：{execute_instructions}"
                )
                self.text_pub.publish(msg)
            else:
                self.get_logger().info(
                    f"The upcoming task to be carried out：{execute_instructions}"
                )  # 即将执行的任务：...
            self.instruction_process(
                type="text",
                prompt=f"用户：{prompt},决策层AI规划:{execute_instructions}",
            )  # 传递决策层模型规划好的执行步骤给执行层模型 / Pass the planned execution steps from the decision layer model to the execution layer model
            self.new_order_cycle = (
                False  # 重置指令周期标志位 / Reset the instruction cycle flag
            )
        else:
            self.get_logger().info(
                "The model service is abnormal. Check the large model account or configuration options"
            )  # 模型推理失败，请检查模型配额和账户是否正常！！！
    else:
        self.instruction_process(
            prompt, type
        )  # 调用执行层大模型生成成动作列表并执行 / Call the execution layer large model to generate an action list and execute

def instruction_process(self, prompt, type, conversation_id=None):
    """
    根据输入信息的类型（文字/图片），构建不同的请求体进行推理，并返回结果）
    Based on the type of input information (text/image), construct different request bodies for inference and return the result.
    """
    if self.regional_setting == "China":  # 国内版
        if type == "text":
            raw_content = self.model_client.multimodalinfer(prompt)
        elif type == "image":
            self.save_single_image()
            raw_content = self.model_client.multimodalinfer(
                "机器人反馈:执行seewhat()完成", image_path=self.image_save_path
            )
        json_str = self.extract_json_content(raw_content)

    elif self.regional_setting == "international":  # 国际版
        if type == "text":
            result = self.model_client.TaskExecution(
                input=prompt,
                map_mapping=self.map_mapping,
                language=self.language_dict[self.language],
                conversation_id=conversation_id,
            )
            if result[0]:
                json_str = self.extract_json_content(result[1])
                self.conversation_id = result[2]
            else:
                self.get_logger().info(f"ERROR:{result[1]}")
        elif type == "image":
            self.save_single_image()
            result = self.model_client.TaskExecution(
                input="机器人反馈:执行seewhat()完成",
                map_mapping=self.map_mapping,
                language=self.language_dict[self.language],
                image_path=self.image_save_path,
                conversation_id=conversation_id,
            )
            if result[0]:
                json_str = self.extract_json_content(result[1])
                self.conversation_id = result[2]
            else:
                self.get_logger().info(f"ERROR:{result[1]}")

    if json_str is not None:
        # 解析JSON字符串,分离"action"、"response"字段 / Parse JSON string, separate "action" and "response" fields
        action_plan_json = json.loads(json_str)
        action_list = action_plan_json.get("action", [])
        llm_response = action_plan_json.get("response", "")
    else:
        self.get_logger().info(
            f"LargeScaleModel return: {json_str},The format was unexpected. The output format of the AI model at the execution layer did not meet the requirements"
        )
        return

    if self.text_chat_mode:
        msg = String(data=f'"action": {action_list}, "response": {llm_response}')
        self.text_pub.publish(msg)
    else:
        self.get_logger().info(
            f'"action": {action_list}, "response": {llm_response}'
        )

    self.send_action_service(
        action_list, llm_response
    )  # 异步发送动作列表、回复内容给ActionServer / Asynchronously send action list and response content to ActionServer

4.3 Dual Model Inference (International Version)

7. The program implementation logic is the same as the domestic version. The difference is that the international version requests the local Dify application, which then requests the cloud large model.

xxxxxxxxxx
def dual_large_model_international_model(self, type, prompt=""):
    """
    此函数适用于国际版双模型推理模式,使用dify作为中间件
    """
    if (
        self.new_order_cycle
    ):  # 判断是否是新任务周期 / Determine if it is a new task cycle
        self.conversation_id = None
        result = self.model_client.TaskDecision(prompt)
        if result[0]:
            if self.text_chat_mode:  # 文字交互模式 / Text interaction mode
                msg = String(
                    data=f"The upcoming task to be carried out:{result[1]}"
                )
                self.text_pub.publish(msg)
            else:  # 语音交互模式 / Voice interaction mode
                self.get_logger().info(
                    f"The upcoming task to be carried out:{result[1]}"
                )

            self.instruction_process(
                type="text", prompt=f"用户：{prompt},决策层AI规划:{result[1]}"
            )
            self.new_order_cycle = (
                False  # 重置指令周期标志位 / Reset the instruction cycle flag
            )
        else:
            self.get_logger().info(f"ERROR: {result[1]}")

    else:
        self.instruction_process(
            prompt, type, conversation_id=self.conversation_id
        )  # 调用执行层大模型生成成动作列表并执行 / Call the execution layer large model to generate an action list and execute

5. Action Server Function

The implementation program is action_service.py, used to receive the action list requested by the model server, parse the action list, and execute it. The source code path is:

Raspberry Pi (inside Docker container), source path:

xxxxxxxxxx
/home/yahboom/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

RDK (running natively), source path:

xxxxxxxxxx
/home/sunrise/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

5.1 Program Flowchart

Saction_service program flowchart

5.2 Action Function Library

The robot's action functions come from methods in the CustomActionServer class, containing various sub-action functions. Here, the function for controlling the robotic arm to move upward is used as an example for explanation. Other action functions will be explained when they first appear in subsequent chapters.

Program explanation:

1. Convert the input string parameters to floating point numbers.

2. Directly call the function, pass parameters, communicate with the underlying driver board, and control the six servos.

3. Decide whether to publish completion information based on the mode.

xxxxxxxxxx
def arm_up(self):
    """
    控制机械臂向上伸直 / Control the robotic arm to extend upward
    调用函数，直接与底层驱动板通讯，控制六个舵机向上伸直 / Call the function to communicate directly with the underlying driver board, controlling six servos to extend upward
    """
    Arm.Arm_serial_servo_write6(90,90,90,90,90,90,1000)
    #如果不是组合模式以及没有被打断，发布控制机械臂向上完成的信息 / If not in combo mode and not interrupted, publish the completion message for arm upward movement
    if not self.combination_mode and not self.interrupt_flag:
        self.action_status_pub("arm_up_done")

5.3 Parse Action List to Control the Robot

Parses the action list generated by the large model into functions that control the robot entity and executes them. The implementation program is the execute_callback method in the CustomActionServer class.

Program explanation:

1. Receive the action list (string format) sent by the client.

2. If the action list is empty, directly return a successful result.

3. If there is only one action, parse and execute the corresponding method; abort on failure.

4. If there are multiple actions, enter combination mode and execute sequentially, logging and providing feedback during the process.

5. After all actions are executed, call stop() to stop the robot.

6. Finally, return the execution success result to the client.

xxxxxxxxxx
def execute_callback(self, goal_handle):
    """
    执行动作列表回调函数 / Execute action list callback function
    解析大模型生成的动作列表，控制机器人实体执行相应动作 / Parse the action list generated by the large model and control the robot entity to execute corresponding actions
    """
    feedback_msg = Rot.Feedback()
    stop = getattr(self, "stop")  # 获取停止方法，通过发布话题的方法停止机器人运动 / Get the stop method to stop robot movement via topic publishing
    actions=goal_handle.request.actions
    if not actions: # 如果动作列表为空，直接返回成功 / If action list is empty, return success directly
        if not self.text_chat_mode:
           self.send_Audiorequest(goal_handle.request.llm_response)

        if self.use_double_llm:
            self.action_status_pub('机器人反馈：回复用户完成')  # Robot feedback: User reply completed

        goal_handle.succeed()
        result = Rot.Result()
        result.success = True
        return result
    elif len(actions) == 1:  # 动作列表只有一个动作，正常执行流程 / Action list has only one action, normal execution flow
        action = actions[0]
        if not self.text_chat_mode:
           self.send_Audiorequest(goal_handle.request.llm_response)

        match = re.match(r"(\w+)\((.*)\)", action)  # 使用正则表达式解析动作名称和参数 / Use regex to parse action name and parameters
        if not match:
            self.get_logger().warning(f'action_service: {action} is invalid action,skip execution')
            goal_handle.abort()
            result = Rot.Result()
            result.success = False
            return result
        else:
            action_name, args_str = match.groups()
            args = [arg.strip() for arg in args_str.split(",")] if args_str else []  # 解析参数列表 / Parse parameter list
            try:
                if hasattr(self, action_name):
                    method = getattr(self, action_name)  # 获取动作方法 / Get action method
                    method(*args)  # 执行动作 / Execute action
                    feedback_msg.status = f'action service execute  {action}  successed'
                    goal_handle.publish_feedback(feedback_msg)  # 发布反馈信息 / Publish feedback
                else:
                    self.get_logger().warning(f'action_service:inviald: {action_name},skip execution')
                    self.action_status_pub('机器人反馈:动作函数不存在，无法执行')  # Robot feedback: Action function does not exist, cannot execute
                    goal_handle.abort()
                    result = Rot.Result()
                    result.success = False
                    return result
            except Exception as e:
                self.get_logger().error(f'action_service:execute action: {action} failed,error-log: {str(e)}')
                goal_handle.abort()
                result = Rot.Result()
                result.success = False
                return result
    else:  # 如果动作列表有多个动作，使能组合模式 / If action list has multiple actions, enable combo mode
        if not self.text_chat_mode:
           self.send_Audiorequest(goal_handle.request.llm_response)

        self.combination_mode=True  # 使能组合模式 / Enable combo mode
        for action in actions:
            match = re.match(r"(\w+)\((.*)\)", action)  # 使用正则表达式解析动作名称和参数 / Use regex to parse action name and parameters
            if not match:
                self.get_logger().warning(f'action_service: {action} is invalid action，skip execution')
                continue

            action_name, args_str = match.groups()
            args = [arg.strip() for arg in args_str.split(",")] if args_str else []

            try:
                if hasattr(self, action_name):
                    method = getattr(self, action_name)  # 检查动作是否是 CustomActionServer 类中的方法 / Check if action is a method in CustomActionServer class
                    method(*args)  # 动态调用方法并传递参数 / Dynamically call method and pass parameters
                    feedback_msg.status = f'action service execute  {action}  successed'
                    self.get_logger().info(feedback_msg.status)
                    goal_handle.publish_feedback(feedback_msg)
                else:
                    self.get_logger().warning(f'action_service:inviald: {action_name}，skip execution')
            except Exception as e:
                self.get_logger().error(f'action_service:execute action: {action} failed,error-log: {str(e)}')
                goal_handle.abort()
                result = Rot.Result()
                result.success = False
                return result

        self.action_status_pub(f'机器人反馈：执行{actions}完成')  # Robot feedback: Execution of actions completed
        self.combination_mode=False  # 重置组合模式标志位 / Reset combo mode flag
    stop()  # 执行完全部动作停止机器人 / Stop the robot after executing all actions
    goal_handle.succeed()  # 返回成功信息给客户端 / Return success info to client
    result = Rot.Result()
    result.success = True
    return result

6. Interruption Function

The robot supports interruption at any stage, which can be divided into recording stage interruption, dialogue stage interruption, and action stage interruption. The principle of each stage interruption is introduced here.

6.1 Recording Stage Interruption

If the user misspeaks during recording or is unsatisfied with the recorded content and needs to re-record, they can directly wake up the robot again during the recording process to interrupt the previous recording and start speaking anew.

• The logic is implemented in the main_loop method of the ASRNode class in the asr.py file:

• Each time the robot is woken up, if there is already a running wake-up recording thread, it is interrupted via the thread event stop_event, and waits for it to finish.

• After clearing the stop event flag, start a new recording thread.

xxxxxxxxxx
def main_loop(self):
    while rclpy.ok():
        while (
            self.audio_request_queue.qsize() > 1
        ):  # 只处理最近的一次唤醒请求，防止重复唤醒 / Process only the most recent wake-up request to prevent duplicates
            self.audio_request_queue.get()

        if not self.audio_request_queue.empty():
            self.audio_request_queue.get()
            self.wakeup_pub.publish(
                Bool(data=True)
            )  # 发布唤醒信号 / Publish wake-up signal
            self.get_logger().info("I'm here")
            playsound(
                self.audio_dict[self.first_response]
            )  # 应答用户 / Respond to the user

            if (
                self.current_thread and self.current_thread.is_alive()
            ):  # 打断上次的唤醒处理线程 / Interrupt the previous wake-up handling thread
                self.stop_event.set()
                self.current_thread.join()  # 等待当前线程结束 / Wait for the current thread to finish
                self.stop_event.clear()  # 清除事件 / Clear the event
            self.current_thread = threading.Thread(target=self.kws_handler)
            self.current_thread.daemon = True
            self.current_thread.start()
        rclpy.spin_once(self, timeout_sec=0.1)
        time.sleep(0.1)

6.2 Dialogue Stage Interruption

If the robot is speaking and the user is unsatisfied with the reply or does not want the robot to continue, they can use the wake word to directly interrupt the robot's speech and start recording immediately. At this point, the user can give new instructions to the robot (still within the current task cycle), or say "End current task" to directly end the current task and start a new task cycle.

• The logic is implemented in the wakeup_callback and play_audio methods of the CustomActionServer class in the action_service.py file:

• wakeup_callback is the callback function for wake-up handling. In the asr.py program, each time the robot is woken up, a wake-up signal is published through topic communication, and wakeup_callback subscribes to and processes the signal.

• Each time the robot is woken up, it detects whether pygame.mixer is currently playing audio. If so, it notifies the playback thread to stop playing via the thread event self.stop_event.

• If a previous action is still running after wake-up, it sets the self.interrupt_flag flag for subsequent action interruption.

xxxxxxxxxx
def wakeup_callback(self, msg):
    """
    唤醒回调函数 / Wake-up callback function
    处理机器人被唤醒的事件，停止当前播放的音频和正在执行的动作 / Handle robot wake-up events, stop currently playing audio and executing actions
    """
    if msg.data:
        if pygame.mixer.music.get_busy():  # 检查音频是否正在播放 / Check if audio is playing
            self.stop_event.set()  # 设置停止事件 / Set stop event
        if self.action_runing:  # 检查是否有动作正在执行 / Check if any action is running
            self.interrupt_flag = True  # 设置中断标志 / Set interrupt flag
            self.stop()  # 停止机器人运动 / Stop robot movement
            self.pubSix_Arm(self.init_joints)  # 机器人恢复初始姿态 / Robot returns to initial posture

When play_audio plays audio, it checks whether self.stop_event is set. Once detected, it immediately stops playing the current audio.

xxxxxxxxxx
def play_audio(self, file_path: str, feedback: Bool = False) -> None:
    """
    同步方式播放音频函数 / The function for playing audio in synchronous mode
    支持中断的音频播放函数，当stop_event被设置时立即停止播放 / Audio playback function with interruption support, stops immediately when stop_event is set
    """
    pygame.mixer.music.load(file_path)  # 加载音频文件 / Load audio file
    pygame.mixer.music.play()  # 播放音频 / Play audio
    while pygame.mixer.music.get_busy():  # 等待音频播放完成 / Wait for audio playback to complete
        if self.stop_event.is_set():  # 检查停止事件是否被设置 / Check if stop event is set
            pygame.mixer.music.stop()  # 停止播放 / Stop playback
            self.stop_event.clear()  # 清除事件 / Clear event
            return
        pygame.time.Clock().tick(10)
    if feedback:
        self.action_status_pub("机器人反馈：回复用户完成")  # Robot feedback: User reply completed

6.3 Action Stage Interruption

If the robot is woken up while executing an action, it will stop the current action and return to the initial posture.

For example, actions like robotic arm grasping and sorting machine codes require starting external programs in a child process. Here, the color block搬运 function change_pose is used as an example:

When the color block搬运 is not completed, it will keep waiting in the while not self.change_pose_future.done(): loop. During this process, if the self.interrupt_flag interruption flag is detected, it first calls the check_close_change_pose function to recursively end the child process tree, then controls the robotic arm to return to the grasping posture and ends the task.

xxxxxxxxxx
def change_pose(self, x1, y1, x2, y2,x3,y3,x4,y4,src,tar,side):
    """
    颜色块搬运函数 / Color block handling function
    启动KCF目标检测程序，接收目标位置信息，等待搬运完成或中断 / Start KCF target detection program, receive target position info, wait for completion or interruption
    """
    src_color = src.strip("'\"")  # 去掉单引号和双引号 / Remove single and double quotes
    self.cur_down_name = src_color
    self.cur_down_pose[self.cur_down_name] = []
    cmd1 = "ros2 run largemodel_arm Get_Target_Pose_KCF"  # KCF目标检测命令 / KCF target detection command
    subprocess.Popen(  # 启动新终端运行KCF程序 / Launch new terminal to run KCF program
        [
            "gnome-terminal",
            "--title=grasp_desktop",
            "--",
            "bash",
            "-c",
            f"{cmd1}; exec bash",
        ]
    )
    time.sleep(3.0)  # 等待终端启动 / Wait for terminal to start
    # 将字符串参数转换为整数 / Convert string parameters to integers
    x1 = int(x1)
    y1 = int(y1)
    x2 = int(x2)
    y2 = int(y2)
    x3 = int(x3)
    y3 = int(y3)
    x4 = int(x4)
    y4 = int(y4)
    side = int(side)
    time.sleep(5.0)  # 等待目标检测准备就绪 / Wait for target detection to be ready
    self.object_position_pub.publish(Int16MultiArray(data=[x1, y1, x2, y2,x3,y3,x4,y4,side]))  # 发布目标位置信息 / Publish target position info

    while not self.change_pose_future.done():  # 等待搬运任务完成 / Wait for handling task to complete
        if self.interrupt_flag:  # 检测到中断标志 / Interrupt flag detected
            self.check_close_change_pose()  # 关闭子进程树 / Close child process tree
            Arm.Arm_serial_servo_write6(90,150,12,20,90,30,1000)  # 机械臂恢复抓取姿态 / Robotic arm returns to grasping posture
            return
        time.sleep(0.1)