ROS2单节点/文件中实现回调与同步主代码及节点终止停车

问题分析 #

原代码存在几个核心问题：

• 用额外线程运行主逻辑，容易引发ROS2上下文冲突——ROS2的节点操作（发布/订阅）需在同一个executor管理的线程中执行，跨线程操作可能导致资源异常。
• 信号处理逻辑有缺陷：调用rclpy.shutdown()后节点资源已被释放，此时stop()里的发布指令无法生效；同时线程内的主循环未正确响应节点关闭状态。
• 代码缺少必要依赖导入（Image、cv_bridge、cv2），直接运行会报错。

修正后的代码 #

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from rclpy.exceptions import ROSInterruptException


class RobotNode(Node):
    def __init__(self):
        super().__init__('my_node')
        # 初始化订阅器
        self.subscription = self.create_subscription(
            Image, 
            '/camera/image_raw', 
            self.callback, 
            10
        )
        # 初始化发布器
        self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)
        # 用Timer替代线程，实现同步主逻辑（10Hz执行）
        self.timer = self.create_timer(0.1, self.main_loop)
        self.bridge = CvBridge()

    def stop_robot(self):
        # 发送零速度指令
        zero_vel = Twist()
        self.publisher.publish(zero_vel)
        # 短暂等待确保消息发布完成
        self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.1))

    def callback(self, data):
        # 相机回调逻辑
        try:
            image = self.bridge.imgmsg_to_cv2(data, 'bgr8')
            cv2.namedWindow('Camera') 
            cv2.imshow('Camera', image)
            cv2.waitKey(1) 
        except Exception as e:
            self.get_logger().error(f"Camera callback error: {str(e)}")

    def main_loop(self):
        # 同步主逻辑，由Timer定时触发
        if rclpy.ok():
            desired_velocity = Twist()
            desired_velocity.linear.x = 0.03
            self.publisher.publish(desired_velocity)


def main(args=None):
    rclpy.init(args=args)
    robot_node = RobotNode()
    try:
        rclpy.spin(robot_node)
    except ROSInterruptException:
        # 捕获Ctrl+C终止信号，执行停止逻辑
        robot_node.stop_robot()
    finally:
        # 销毁节点资源
        robot_node.destroy_node()
        rclpy.shutdown()
        cv2.destroyAllWindows()


if __name__ == '__main__':
    main()

关键修改说明 #

• 用Timer替代线程：ROS2的create_timer会将主逻辑任务加入节点的executor队列，和回调函数共享同一线程，避免跨线程资源冲突，同时保证定时执行的准确性。
• 可靠的停止逻辑：通过捕获ROSInterruptException（ROS2对Ctrl+C的标准处理），在节点退出前调用stop_robot()发送零速度，并短暂等待确保消息发布完成，再销毁节点和关闭ROS上下文。
• 修复依赖导入：补充了Image、CvBridge、cv2的导入，保证相机回调逻辑正常运行。

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