Nao机器人线程控制优化：检测到球后如何立即终止行走线程

问题概述 #

现有Nao机器人Python代码可实现行走与球体检测，但检测到球后无法立即终止当前行走动作，需完成整个动作序列才会停止。需要优化线程逻辑，实现类状态机的任务管理，让机器人在检测到球的瞬间停止行走，进入后续流程。

原代码 #

import time
import threading
import qi
import balldetection
import common
import cv2
import vision_definitions
import math 

lock = threading.Lock()

ball_detected = False
ip = "10.42.0.98"
port = 9559
session = qi.Session()
session.connect("tcp://" + ip + ":" + str(port))
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
video_service = session.service("ALVideoDevice")
speak_service = session.service("ALTextToSpeech")

def detect_ball_and_act():
    global ball_detected
    print("In detect_ball_and_act")
    videoClient = video_service.subscribeCamera("nao_opencv", 0, vision_definitions.kQVGA, vision_definitions.kBGRColorSpace, 5)
while not ball_detected:
    print("Not ball detected")
    try:
        print("In try")
        frame = common.readImage(video_service, videoClient)
        if frame is not None:
            print("in none")
            hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            img, (ret, x, y, radius, distance) = balldetection.detect_ball(hsv)
            if ret:
                print("Ball in sight at ({}, {}) with radius {} and distance {}".format(x, y, radius, distance))
                print("Performing action...")
                speak_service.say("Ball is in sight!")
                with lock:
                    ball_detected = True
            else:
                time.sleep(1.0)
    except Exception as e:
        print("An error occurred:", e)

def keep_walking_and_looking():
    while not ball_detected:
        print("Walking and looking around...")
        walk_and_look()

 def walk_and_look():
    print("In walk_and_look")
    motion_service.moveToward(0.5, 0, 0)
    time.sleep(3.0)
    motion_service.moveToward(0, 0, 0)
    time.sleep(1.0)
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"],     [0.5, 1.0], 0.2)
    time.sleep(1.0)
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"], [-0.5, 1.0], 0.2)
    time.sleep(1.0)
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"], [0.0, 0.0], 0.2)

walking_thread = threading.Thread(target=keep_walking_and_looking)
ball_detection_thread = threading.Thread(target=detect_ball_and_act)

motion_service.setStiffnesses("Body", 1.0)
posture_service.goToPosture("StandInit", 1.0)

walking_thread.start()
ball_detection_thread.start()

ball_detection_thread.join()
walking_thread.join()

优化方案：状态机驱动的线程管理 #

核心思路 #

1. 用枚举定义清晰状态：替代单一布尔变量，支持状态扩展（如后续添加靠近、抓取等状态）。
2. 细粒度状态检查：拆分长阻塞睡眠为多次短睡眠+状态检查，确保能即时响应状态变化。
3. 线程安全状态更新：用锁保护状态变量，避免多线程竞争。
4. 即时终止动作：检测到球后立即调用ALMotion的停止接口，终止所有正在执行的动作。

修改后的代码 #

import time
import threading
import qi
import balldetection
import common
import cv2
import vision_definitions
from enum import Enum

# 定义机器人状态枚举
class RobotState(Enum):
    SEARCHING = 1    # 搜索球体中
    FOUND_BALL = 2   # 已发现球体

lock = threading.Lock()
current_state = RobotState.SEARCHING

ip = "10.42.0.98"
port = 9559
session = qi.Session()
session.connect("tcp://" + ip + ":" + str(port))
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
video_service = session.service("ALVideoDevice")
speak_service = session.service("ALTextToSpeech")

def detect_ball_and_act():
    global current_state
    print("Starting ball detection...")
    videoClient = video_service.subscribeCamera("nao_opencv", 0, vision_definitions.kQVGA, vision_definitions.kBGRColorSpace, 5)
    
    while True:
        # 检查是否已找到球，避免无效检测
        with lock:
            if current_state == RobotState.FOUND_BALL:
                break
        
        try:
            frame = common.readImage(video_service, videoClient)
            if frame is not None:
                hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
                img, (ret, x, y, radius, distance) = balldetection.detect_ball(hsv)
                if ret:
                    print(f"Ball detected at ({x}, {y}) | Radius: {radius} | Distance: {distance}")
                    speak_service.say("Ball is in sight!")
                    
                    # 更新状态并立即停止所有动作
                    with lock:
                        current_state = RobotState.FOUND_BALL
                    motion_service.stopMove()
                    motion_service.stopAll()
                    break
            time.sleep(0.5)  # 缩短检测间隔，提升响应速度
        except Exception as e:
            print("Detection error:", e)
            time.sleep(1.0)
    
    video_service.unsubscribe(videoClient)

def keep_walking_and_looking():
    global current_state
    while True:
        with lock:
            if current_state == RobotState.FOUND_BALL:
                break
        walk_and_look()

def walk_and_look():
    global current_state
    
    # 向前行走，每0.5秒检查一次状态
    motion_service.moveToward(0.5, 0, 0)
    for _ in range(6):
        time.sleep(0.5)
        with lock:
            if current_state == RobotState.FOUND_BALL:
                motion_service.stopMove()
                return
    
    # 停止移动
    motion_service.moveToward(0, 0, 0)
    time.sleep(0.5)
    with lock:
        if current_state == RobotState.FOUND_BALL:
            return
    
    # 头部右转
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"], [0.5, 1.0], 0.2)
    time.sleep(0.5)
    with lock:
        if current_state == RobotState.FOUND_BALL:
            return
    
    # 头部左转
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"], [-0.5, 1.0], 0.2)
    time.sleep(0.5)
    with lock:
        if current_state == RobotState.FOUND_BALL:
            return
    
    # 头部回正
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"], [0.0, 0.0], 0.2)
    time.sleep(0.5)

# 初始化机器人姿态
motion_service.setStiffnesses("Body", 1.0)
posture_service.goToPosture("StandInit", 1.0)

# 启动线程
walking_thread = threading.Thread(target=keep_walking_and_looking)
ball_detection_thread = threading.Thread(target=detect_ball_and_act)

walking_thread.start()
ball_detection_thread.start()

# 等待线程结束
ball_detection_thread.join()
walking_thread.join()

# 此处可添加后续靠近球体的逻辑，例如切换到APPROACHING状态

关键改进说明 #

• 状态扩展性：通过RobotState枚举，后续可轻松添加APPROACHING（靠近球）、GRABBING（抓取）等状态，构建完整的任务流程。
• 即时响应：将原3秒的长睡眠拆分为6次0.5秒睡眠+状态检查，确保机器人能在0.5秒内响应球的检测结果。
• 动作终止可靠性：检测到球后，同时调用stopMove()和stopAll()，确保所有行走和头部动作立即停止。
• 线程安全：所有状态的读取和更新都通过锁保护，避免多线程环境下的状态竞争问题。

内容的提问来源于stack exchange，提问作者Stephen Martinez