ROS中while循环内无法响应Joy回调的机器人模式切换问题

核心问题 #

1. 线程阻塞：ROS的rospy.spin()运行在单线程中，当callback调用autocamera()后，函数内的while True会持续占用线程，导致后续的Joy消息回调无法执行，全局变量button_pressed根本不会更新，自然检测不到退出按键。
2. 退出条件逻辑错误：cv2.waitKey(1)的返回值是按键的ASCII码（无按键时为-1），与button_pressed[5]做按位与完全不符合逻辑，无法正确触发退出。
3. 额外隐患：当未检测到轮廓时，c变量未定义，执行cv2.drawContours会抛出异常；颜色阈值参数顺序颠倒，导致轮廓检测可能失效。

解决方案：多线程分离自动模式逻辑 #

将自动模式的循环放到独立线程中运行，避免阻塞ROS的回调线程，同时用全局变量控制自动模式的启停状态。

修改后的完整代码 #

#! /usr/bin/env python
import rospy
from sensor_msgs.msg import Joy
import RPi.GPIO as GPIO
import cv2
import numpy as np
import threading
import time

cap = cv2.VideoCapture(0)
cap.set(3, 160)
cap.set(4, 120)
button_pressed = [0] * 12  # 初始化足够长度的按键数组
GPIO.setmode(GPIO.BCM)

# GPIO初始化
GPIO.setup(22, GPIO.OUT)
GPIO.setup(23, GPIO.OUT)
GPIO.setup(24, GPIO.OUT)
GPIO.setup(25, GPIO.OUT)

# 全局状态变量：控制自动模式启停
auto_running = False
auto_thread = None

def callback(data):
    global button_pressed, auto_running, auto_thread
    button_pressed = data.buttons
    rospy.loginfo(rospy.get_caller_id() + " Motor Links: %s ; Motor Rechts: %s ; Stepper: %s", 
                  round(data.axes[2],2), round(data.axes[5],2), round(data.axes[1],2))
    
    # 手动模式：按键5按下时，停止自动模式并切换到手动
    if data.buttons[5] == 1:
        auto_running = False
        coole_manuellsteuerung(data)
    # 自动模式：按键2按下时，启动自动模式（仅当未运行时）
    elif data.buttons[2] == 1 and not auto_running:
        auto_running = True
        auto_thread = threading.Thread(target=autocamera)
        auto_thread.start()
    # 默认手动模式（未触发自动时保持手动）
    elif not auto_running:
        coole_manuellsteuerung(data)
        
def coole_manuellsteuerung(data):
    if data.axes[5] != 0:
        # 简化的驱动逻辑：摇杆非中立时驱动
        GPIO.output(22, GPIO.LOW)
        GPIO.output(23, GPIO.HIGH)
        GPIO.output(24, GPIO.HIGH)
        GPIO.output(25, GPIO.LOW)
    else:
        # 摇杆中立时停止电机
        GPIO.output(22, GPIO.LOW)
        GPIO.output(23, GPIO.LOW)
        GPIO.output(24, GPIO.LOW)
        GPIO.output(25, GPIO.LOW)

def autocamera():
    global auto_running
    while auto_running:
        ret, frame = cap.read()
        if not ret:
            rospy.logwarn("Failed to read camera frame")
            time.sleep(0.01)
            continue
        
        # 修正颜色阈值顺序：检测黑色线条（BGR范围）
        low_b = np.uint8([0, 0, 0])
        high_b = np.uint8([5, 5, 5])
        mask = cv2.inRange(frame, low_b, high_b)
        contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        if len(contours) > 0:
            c = max(contours, key=cv2.contourArea)
            M = cv2.moments(c)
            if M["m00"] != 0:
                cx = int(M['m10']/M['m00'])
                cy = int(M['m01']/M['m00'])
                print(f"CX : {cx}  CY : {cy}")
                if cx >= 120:
                    print("Turn Left")
                    # 此处添加左转电机控制逻辑
                elif 40 < cx < 120:
                    print("On Track!")
                    # 此处添加直行电机控制逻辑
                elif cx <= 40:
                    print("Turn Right")
                    # 此处添加右转电机控制逻辑
                cv2.circle(frame, (cx, cy), 5, (255,255,255), -1)
            cv2.drawContours(frame, c, -1, (0,255,0), 1)
        else:
            print("I don't see the line")
            # 未检测到线条时停止电机
            GPIO.output(22, GPIO.LOW)
            GPIO.output(23, GPIO.LOW)
            GPIO.output(24, GPIO.LOW)
            GPIO.output(25, GPIO.LOW)
        
        cv2.imshow("Mask", mask)
        cv2.imshow("Frame", frame)
        
        # 处理CV窗口的按键退出（可选，比如按ESC退出）
        if cv2.waitKey(1) == 27:
            auto_running = False
    
    # 退出循环后清理窗口
    cv2.destroyAllWindows()
    # 停止电机
    GPIO.output(22, GPIO.LOW)
    GPIO.output(23, GPIO.LOW)
    GPIO.output(24, GPIO.LOW)
    GPIO.output(25, GPIO.LOW)

def listener():
    rospy.init_node('Coole_Motorsteuerung', anonymous=True)
    rospy.Subscriber("/joy", Joy, callback)
    rospy.spin()

if __name__ == '__main__':
    try:
        listener()
    except KeyboardInterrupt:
        auto_running = False
        if auto_thread is not None:
            auto_thread.join()
        GPIO.cleanup()
        cv2.destroyAllWindows()

关键修改说明 #

1. 多线程分离：将autocamera()放到独立线程中运行，ROS的回调线程可以正常处理Joy消息，更新button_pressed和控制auto_running状态。
2. 状态变量控制循环：用auto_running全局变量替代while True，当回调检测到退出按键时，将该变量设为False，自动模式循环会正常退出。
3. 修正颜色阈值：调换low_b和high_b的顺序，确保正确检测黑色线条。
4. 异常处理：添加摄像头读取失败的判断，以及未检测到轮廓时的drawContours跳过逻辑，避免程序崩溃。
5. 电机停止逻辑：在手动模式摇杆中立、自动模式未检测线条、自动模式退出时，都添加电机停止的逻辑，避免电机一直运行。

内容的提问来源于stack exchange，提问作者Cooler Mann