ROS2 Humble第二天实战：从Topic到Action，构建巡检任务状态机（附完整代码）

July 22, 2026

今日完整学习记录：ROS 2 从零到“巡检任务雏形”（Ubuntu 22.04 + Humble）

本文记录了在ROS 2 Humble环境下完成基础通信机制实战，并实现巡检任务最小原型的过程，涵盖Topic、QoS、Service、Action及状态机，所有示例代码均可直接运行。

一、开发环境与版本

操作系统：Ubuntu 22.04（虚拟机）
ROS 2 发行版：Humble（LTS）

二、创建Python包并构建

创建工作空间与包：

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python py_pubsub --dependencies rclpy std_msgs

构建与安装：

cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

三、Topic通信：发布与订阅

talker.py – 发布者节点：

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class Talker(Node):
    def __init__(self):
        super().__init__('talker')
        self.pub = self.create_publisher(String, 'chatter', 10)
        self.timer = self.create_timer(1.0, self.on_timer)
        self.count = 0

    def on_timer(self):
        msg = String()
        msg.data = f'hello {self.count}'
        self.pub.publish(msg)
        self.get_logger().info(msg.data)
        self.count += 1

def main():
    rclpy.init()
    node = Talker()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

listener.py – 订阅者节点：

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class Listener(Node):
    def __init__(self):
        super().__init__('listener')
        self.sub = self.create_subscription(String, 'chatter', self.on_msg, 10)

    def on_msg(self, msg: String):
        self.get_logger().info(f'I heard: {msg.data}')

def main():
    rclpy.init()
    node = Listener()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

四、QoS策略：实现可靠传输

将服务质量配置修改为可靠模式：

from rclpy.qos import QoSProfile, ReliabilityPolicy
qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE)

五、Service服务：请求与响应

add_two_ints_server.py – 服务端：

import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts

class AddTwoIntsServer(Node):
    def __init__(self):
        super().__init__('add_two_ints_server')
        self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.on_request)

    def on_request(self, request, response):
        response.sum = request.a + request.b
        self.get_logger().info(f'{request.a} + {request.b} = {response.sum}')
        return response

def main():
    rclpy.init()
    node = AddTwoIntsServer()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

add_two_ints_client.py – 客户端：

import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts

class AddTwoIntsClient(Node):
    def __init__(self):
        super().__init__('add_two_ints_client')
        self.cli = self.create_client(AddTwoInts, 'add_two_ints')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('waiting for service...')

    def send_request(self, a, b):
        req = AddTwoInts.Request()
        req.a = a
        req.b = b
        future = self.cli.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        return future.result()

def main():
    rclpy.init()
    node = AddTwoIntsClient()
    result = node.send_request(2, 3)
    node.get_logger().info(f'result: {result.sum}')
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

六、Action动作：长任务与实时反馈

本示例中Humble版本的Feedback字段名称为sequence。

fibonacci_action_server.py – 服务端：

import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from example_interfaces.action import Fibonacci

class FibonacciActionServer(Node):
    def __init__(self):
        super().__init__('fibonacci_action_server')
        self._server = ActionServer(self, Fibonacci, 'fibonacci', self.execute_callback)

    def execute_callback(self, goal_handle):
        self.get_logger().info('Received goal')
        feedback_msg = Fibonacci.Feedback()
        sequence = [0, 1]

        for i in range(2, goal_handle.request.order):
            sequence.append(sequence[i-1] + sequence[i-2])
            feedback_msg.sequence = sequence
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(0.5)

        goal_handle.succeed()
        result = Fibonacci.Result()
        result.sequence = sequence
        return result

def main():
    rclpy.init()
    node = FibonacciActionServer()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

fibonacci_action_client.py – 客户端：

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from example_interfaces.action import Fibonacci

class FibonacciActionClient(Node):
    def __init__(self):
        super().__init__('fibonacci_action_client')
        self._client = ActionClient(self, Fibonacci, 'fibonacci')

    def send_goal(self, order):
        self._client.wait_for_server()
        goal_msg = Fibonacci.Goal()
        goal_msg.order = order
        self._client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)

    def feedback_callback(self, feedback_msg):
        self.get_logger().info(f'feedback: {feedback_msg.feedback.sequence}')

def main():
    rclpy.init()
    node = FibonacciActionClient()
    node.send_goal(10)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

七、巡检任务原型：状态机+服务触发+状态发布

目标：通过服务启动巡检，状态驱动发布，实现最小可用原型。

inspection_manager.py – 管理节点：

import time
import threading
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from std_srvs.srv import Trigger

class InspectionManager(Node):
    def __init__(self):
        super().__init__('inspection_manager')
        self.pub = self.create_publisher(String, 'inspection_status', 10)
        self.srv = self.create_service(Trigger, 'start_inspection', self.on_start)
        self.state = 'IDLE'
        self._lock = threading.Lock()
        self.timer = self.create_timer(1.0, self.publish_status)

    def publish_status(self):
        msg = String()
        with self._lock:
            msg.data = self.state
        self.pub.publish(msg)

    def on_start(self, request, response):
        with self._lock:
            if self.state == 'RUNNING':
                response.success = False
                response.message = 'Already running'
                return response
            self.state = 'RUNNING'
        threading.Thread(target=self.run_inspection, daemon=True).start()
        response.success = True
        response.message = 'Inspection started'
        return response

    def run_inspection(self):
        time.sleep(5)
        with self._lock:
            self.state = 'DONE'

def main():
    rclpy.init()
    node = InspectionManager()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

inspection_client.py – 客户端，用于发送启动指令：

import rclpy
from rclpy.node import Node
from std_srvs.srv import Trigger

class InspectionClient(Node):
    def __init__(self):
        super().__init__('inspection_client')
        self.cli = self.create_client(Trigger, 'start_inspection')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Waiting for start_inspection service...')

    def send_request(self):
        req = Trigger.Request()
        future = self.cli.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        return future.result()

def main():
    rclpy.init()
    node = InspectionClient()
    result = node.send_request()
    if result.success:
        node.get_logger().info(f'Success: {result.message}')
    else:
        node.get_logger().error(f'Failed: {result.message}')
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

inspection_status_echo.py – 状态监听节点：

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class StatusEcho(Node):
    def __init__(self):
        super().__init__('inspection_status_echo')
        self.sub = self.create_subscription(String, 'inspection_status', self.on_msg, 10)

    def on_msg(self, msg: String):
        self.get_logger().info(f'Status: {msg.data}')

def main():
    rclpy.init()
    node = StatusEcho()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

八、总结与后续计划

已完成的通信模式与原型：

完整覆盖 Topic、Service、Action 三种通信机制
使用 QoS 策略实现可靠传输
基于状态机构建巡检任务原型，支持服务触发与状态发布

后续扩展方向：

增加 stop_inspection 服务，实现随时停止巡检
引入 ERROR 状态分支，提升容错能力
结合 Nav2 的 NavigateToPose 动作，实现多点自主巡检