ROS2架构demo(python)

下面我将提供一个 ROS 2 Python 程序的架构 Demo。这个 Demo 将包含以下元素，以展示一个小型但功能相对完整的 ROS 2 系统架构：

1. 自定义接口 (Interfaces)： 定义自定义的消息、服务和动作类型。
2. 参数节点 (Parameter Node)： 一个节点，其行为可以通过 ROS 参数进行配置。
3. 发布者节点 (Publisher Node)： 发布自定义消息。
4. 订阅者节点 (Subscriber Node)： 订阅自定义消息并处理。
5. 服务服务器节点 (Service Server Node)： 提供一个服务。
6. 服务客户端节点 (Service Client Node)： 调用一个服务。
7. 动作服务器节点 (Action Server Node)： 提供一个长时间运行的动作。
8. 动作客户端节点 (Action Client Node)： 请求一个动作并处理反馈和结果。
9. 生命周期节点 (Lifecycle Node)： 一个具有受管理生命周期的节点。
10. 启动文件 (Launch File)： (概念性描述，因为实际运行需要文件系统操作) 用于启动和配置这些节点。

我们将把这些节点组织在一个假设的 Python 包中。

---

1. 项目结构（概念）

假设我们有一个 ROS 2 工作空间 ros2_demo_ws，其中包含两个包：

• my_interfaces: 用于存放自定义消息、服务和动作定义。
• python_ архитектура_demo: 包含我们所有的 Python 节点。
• ros2_demo_ws/
  ├── src/
  │   ├── my_interfaces/
  │   │   ├── msg/
  │   │   │   └── MessageData.msg
  │   │   ├── srv/
  │   │   │   └── ComputeSum.srv
  │   │   ├── action/
  │   │   │   └── ProcessTask.action
  │   │   ├── CMakeLists.txt
  │   │   └── package.xml
  │   └── python_architecture_demo/
  │       ├── python_architecture_demo/
  │       │   ├── __init__.py
  │       │   ├── publisher_node.py
  │       │   ├── subscriber_node.py
  │       │   ├── service_server_node.py
  │       │   ├── service_client_node.py
  │       │   ├── action_server_node.py
  │       │   ├── action_client_node.py
  │       │   ├── parameter_node.py
  │       │   └── lifecycle_manager_node.py
  │       ├── resource/
  │       │   └── python_architecture_demo  (empty file for ament_python)
  │       ├── launch/
  │       │   └── demo_launch.py
  │       ├── setup.py
  │       ├── setup.cfg
  │       └── package.xml
  └── install/ (after build)
  └── build/
  └── log/

---

2. 自定义接口 (my_interfaces 包)

a. my_interfaces/msg/MessageData.msg:

# MessageData.msg
string message
int32 counter

b. my_interfaces/srv/ComputeSum.srv:

# ComputeSum.srv
int64 a
int64 b
---
int64 sum

c. my_interfaces/action/ProcessTask.action:

# ProcessTask.action
int32 initial_value       # 目标：任务的初始值
---
bool success              # 结果：任务是否成功
string final_message      # 结果：任务结束时的消息
---
int32 percent_complete    # 反馈：任务完成百分比
string current_status     # 反馈：当前状态消息

d. my_interfaces/package.xml (关键部分):

<?xml version="1.0"?>
<package format="3">
  <name>my_interfaces</name>
  <version>0.0.0</version>
  <description>Custom interfaces for ROS 2 demo</description>
  <maintainer email="user@example.com">User</maintainer>
  <license>Apache License 2.0</license>
  <buildtool_depend>ament_cmake</buildtool_depend>
  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>
  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

e. my_interfaces/CMakeLists.txt (关键部分):

cmake_minimum_required(VERSION 3.8)
project(my_interfaces)

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/MessageData.msg"
  "srv/ComputeSum.srv"
  "action/ProcessTask.action"
)

ament_package()

---

3. Python 节点 (python_architecture_demo 包)

python_architecture_demo/package.xml (关键部分)

<?xml version="1.0"?>
<package format="3">
  <name>python_architecture_demo</name>
  <version>0.0.0</version>
  <description>Python architecture demo for ROS 2</description>
  <maintainer email="user@example.com">User</maintainer>
  <license>Apache License 2.0</license>
  <depend>rclpy</depend>
  <depend>my_interfaces</depend> <!-- 依赖自定义接口包 -->
  <depend>std_msgs</depend>
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

python_architecture_demo/setup.py

from setuptools import find_packages, setup

package_name = 'python_architecture_demo'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        # 包含 launch 文件
        ('share/' + package_name + '/launch', ['launch/demo_launch.py']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user@example.com',
    description='Python architecture demo for ROS 2',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'parameter_node = python_architecture_demo.parameter_node:main',
            'publisher_node = python_architecture_demo.publisher_node:main',
            'subscriber_node = python_architecture_demo.subscriber_node:main',
            'service_server_node = python_architecture_demo.service_server_node:main',
            'service_client_node = python_architecture_demo.service_client_node:main',
            'action_server_node = python_architecture_demo.action_server_node:main',
            'action_client_node = python_architecture_demo.action_client_node:main',
            'lifecycle_node = python_architecture_demo.lifecycle_manager_node:main',
        ],
    },
)

python_architecture_demo/python_architecture_demo/parameter_node.py

import rclpy
from rclpy.node import Node
from rcl_interfaces.msg import ParameterDescriptor, ParameterType

class ParameterDemoNode(Node):
    """
    演示ROS 2参数使用的节点。
    它声明一个参数，并根据该参数的值周期性地打印消息。
    """
    def __init__(self):
        super().__init__('parameter_demo_node')

        # 声明一个参数 'my_message'，类型为字符串，默认值为 "Hello from Parameter Node!"
        # ParameterDescriptor 提供了参数的额外元数据
        my_message_descriptor = ParameterDescriptor(
            name='my_message', # 确保这里与 declare_parameter 的第一个参数匹配
            type=ParameterType.PARAMETER_STRING,
            description='The message to be printed by the node.'
        )
        self.declare_parameter('my_message', 'Hello from Parameter Node!', my_message_descriptor)

        # 声明一个参数 'print_interval_sec'，类型为浮点数，默认值为 1.0 秒
        print_interval_descriptor = ParameterDescriptor(
            name='print_interval_sec',
            type=ParameterType.PARAMETER_DOUBLE,
            description='Interval in seconds for printing the message.',
            # 可以添加更多约束，例如浮点数范围
            floating_point_range=[rcl_interfaces.msg.FloatingPointRange(from_value=0.1, to_value=10.0, step=0.1)]
        )
        self.declare_parameter('print_interval_sec', 1.0, print_interval_descriptor)

        # 获取参数的初始值
        self.message_to_print = self.get_parameter('my_message').get_parameter_value().string_value
        self.print_interval = self.get_parameter('print_interval_sec').get_parameter_value().double_value

        # 创建一个定时器，根据 'print_interval_sec' 参数定义的间隔调用 timer_callback
        self.timer = self.create_timer(self.print_interval, self.timer_callback)

        self.get_logger().info(f"ParameterNode initialized. Message: '{self.message_to_print}', Interval: {self.print_interval}s")
        self.get_logger().info("你可以通过 ros2 param set /parameter_demo_node my_message 'New Message' 来改变消息")
        self.get_logger().info("或者 ros2 param set /parameter_demo_node print_interval_sec 2.0 来改变打印间隔")


    def timer_callback(self):
        """
        定时器回调函数。
        它会获取最新的参数值（以防运行时被更改），然后打印消息。
        """
        # 每次回调时都获取参数，以便响应运行时的参数更改
        current_message = self.get_parameter('my_message').get_parameter_value().string_value
        current_interval = self.get_parameter('print_interval_sec').get_parameter_value().double_value

        # 如果打印间隔被更改，我们需要重新创建定时器
        if current_interval != self.print_interval:
            self.get_logger().info(f"Print interval changed from {self.print_interval}s to {current_interval}s. Recreating timer.")
            self.print_interval = current_interval
            if self.timer is not None:
                self.timer.cancel() # 取消旧的定时器
            self.timer = self.create_timer(self.print_interval, self.timer_callback) # 创建新的定时器

        self.get_logger().info(f"Parameter says: '{current_message}' (Interval: {self.print_interval}s)")


def main(args=None):
    rclpy.init(args=args)
    node = ParameterDemoNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('ParameterNode stopped cleanly')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

python_architecture_demo/python_architecture_demo/publisher_node.py

import rclpy
from rclpy.node import Node
from my_interfaces.msg import MessageData # 导入自定义消息

class DemoPublisherNode(Node):
    """
    演示发布自定义消息的节点。
    它会周期性地发布一个包含计数器和自定义字符串的消息。
    """
    def __init__(self):
        super().__init__('demo_publisher_node')
        # 创建一个发布者，发布到 'custom_topic' 主题，消息类型为 MessageData，队列大小为 10
        self.publisher_ = self.create_publisher(MessageData, 'custom_topic', 10)
        
        # 从参数服务器获取发布频率，默认1.0秒
        self.declare_parameter('publish_frequency_hz', 1.0)
        frequency = self.get_parameter('publish_frequency_hz').get_parameter_value().double_value
        timer_period = 1.0 / frequency  # seconds

        # 创建一个定时器，每隔 timer_period 秒调用 timer_callback
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.counter_ = 0  # 消息计数器
        self.get_logger().info(f"PublisherNode initialized, publishing to 'custom_topic' at {frequency} Hz.")

    def timer_callback(self):
        """
        定时器回调函数，用于创建并发布消息。
        """
        msg = MessageData()
        msg.message = f"Hello from publisher! Count: {self.counter_}"
        msg.counter = self.counter_
        
        self.publisher_.publish(msg) # 发布消息
        self.get_logger().info(f'Publishing: "{msg.message}" with counter: {msg.counter}')
        self.counter_ += 1

def main(args=None):
    rclpy.init(args=args)
    node = DemoPublisherNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('PublisherNode stopped cleanly')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

python_architecture_demo/python_architecture_demo/subscriber_node.py

import rclpy
from rclpy.node import Node
from my_interfaces.msg import MessageData # 导入自定义消息

class DemoSubscriberNode(Node):
    """
    演示订阅自定义消息的节点。
    它会订阅 'custom_topic' 主题，并在接收到消息时打印消息内容。
    """
    def __init__(self):
        super().__init__('demo_subscriber_node')
        # 创建一个订阅者，订阅 'custom_topic' 主题，消息类型为 MessageData，
        # 当接收到消息时调用 listener_callback，队列大小为 10
        self.subscription = self.create_subscription(
            MessageData,
            'custom_topic',
            self.listener_callback,
            10)
        self.get_logger().info("SubscriberNode initialized, listening to 'custom_topic'.")

    def listener_callback(self, msg):
        """
        消息回调函数。当接收到消息时，此函数被调用。
        参数:
            msg (MessageData): 接收到的消息对象。
        """
        self.get_logger().info(f'Subscriber received: "{msg.message}" (Counter: {msg.counter})')

def main(args=None):
    rclpy.init(args=args)
    node = DemoSubscriberNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('SubscriberNode stopped cleanly')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

python_architecture_demo/python_architecture_demo/service_server_node.py

import rclpy
from rclpy.node import Node
from my_interfaces.srv import ComputeSum # 导入自定义服务

class DemoServiceServerNode(Node):
    """
    演示提供服务的节点。
    它提供一个名为 'compute_sum_service' 的服务，用于计算两个整数的和。
    """
    def __init__(self):
        super().__init__('demo_service_server_node')
        # 创建一个服务服务器，服务类型为 ComputeSum，服务名称为 'compute_sum_service'
        # 当接收到服务请求时，调用 handle_compute_sum 回调函数
        self.srv = self.create_service(ComputeSum, 'compute_sum_service', self.handle_compute_sum)
        self.get_logger().info("ServiceServerNode initialized, 'compute_sum_service' is ready.")

    def handle_compute_sum(self, request, response):
        """
        服务请求处理函数。
        参数:
            request (ComputeSum.Request): 服务请求对象，包含 'a' 和 'b'。
            response (ComputeSum.Response): 服务响应对象，需要填充 'sum'。
        返回:
            ComputeSum.Response: 填充后的响应对象。
        """
        response.sum = request.a + request.b
        self.get_logger().info(f'Incoming service request: a={request.a}, b={request.b}. Responding with sum={response.sum}')
        return response

def main(args=None):
    rclpy.init(args=args)
    node = DemoServiceServerNode()
    try:
        rclpy.spin(node) # 保持节点运行以接收服务请求
    except KeyboardInterrupt:
        node.get_logger().info('ServiceServerNode stopped cleanly')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

python_architecture_demo/python_architecture_demo/service_client_node.py

import rclpy
from rclpy.node import Node
from my_interfaces.srv import ComputeSum # 导入自定义服务
import sys # 用于从命令行获取参数

class DemoServiceClientNode(Node):
    """
    演示调用服务的节点。
    它会创建一个服务客户端，用于调用 'compute_sum_service' 服务。
    """
    def __init__(self):
        super().__init__('demo_service_client_node')
        # 创建一个服务客户端，服务类型为 ComputeSum，服务名称为 'compute_sum_service'
        self.client = self.create_client(ComputeSum, 'compute_sum_service')
        
        # 等待服务可用，设置超时时间为1秒
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Service "compute_sum_service" not available, waiting again...')
        
        self.get_logger().info("ServiceClientNode initialized and connected to 'compute_sum_service'.")

    def send_request(self, a, b):
        """
        发送服务请求并等待响应。
        参数:
            a (int): 第一个加数。
            b (int): 第二个加数。
        """
        request = ComputeSum.Request()
        request.a = a
        request.b = b
        
        # 异步发送请求
        # call_async 返回一个 future 对象，可用于检查请求是否完成
        self.future = self.client.call_async(request)
        self.get_logger().info(f"Sending request: a={a}, b={b}")

        # 等待服务响应（这里为了演示简单，直接在发送后等待）
        # 在实际应用中，你可能希望在主循环中检查 future.done()
        rclpy.spin_until_future_complete(self, self.future)
        
        if self.future.result() is not None:
            response = self.future.result()
            self.get_logger().info(f'Service response: Sum = {response.sum}')
            return response.sum
        else:
            self.get_logger().error('Service call failed: Exception during call')
            if self.future.exception() is not None:
                self.get_logger().error(f'Exception: {self.future.exception()}')
            return None

def main(args=None):
    rclpy.init(args=args)
    node = DemoServiceClientNode()

    # 从命令行参数获取要相加的数字，如果没有提供，则使用默认值
    if len(sys.argv) >= 3:
        try:
            a = int(sys.argv[1])
            b = int(sys.argv[2])
        except ValueError:
            node.get_logger().error("Invalid arguments. Please provide two integers.")
            node.destroy_node()
            rclpy.shutdown()
            return
    else:
        node.get_logger().info("No arguments provided, using default values a=5, b=10")
        a = 5
        b = 10

    result = node.send_request(a, b)
    if result is not None:
        node.get_logger().info(f"The sum of {a} and {b} is {result}")
    else:
        node.get_logger().info("Failed to get sum from service.")
    
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

python_architecture_demo/python_architecture_demo/action_server_node.py

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.node import Node
from my_interfaces.action import ProcessTask # 导入自定义动作
import time

class DemoActionServerNode(Node):
    """
    演示提供动作服务器的节点。
    它提供一个名为 'process_task_action' 的动作，模拟一个耗时任务。
    """
    def __init__(self):
        super().__init__('demo_action_server_node')
        self._action_server = ActionServer(
            self,
            ProcessTask,
            'process_task_action',
            execute_callback=self.execute_callback,
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback
        )
        self.get_logger().info("ActionServerNode initialized, 'process_task_action' is ready.")

    def goal_callback(self, goal_request):
        """
        当收到新的目标请求时调用。
        可以根据目标内容决定是否接受。
        """
        self.get_logger().info(f'Received goal request with initial_value: {goal_request.initial_value}')
        # 示例：只接受正的初始值
        if goal_request.initial_value <= 0:
            self.get_logger().warn('Rejecting goal: initial_value must be positive.')
            return GoalResponse.REJECT
        return GoalResponse.ACCEPT

    def cancel_callback(self, goal_handle):
        """
        当客户端请求取消一个目标时调用。
        """
        self.get_logger().info(f'Received cancel request for goal ID: {goal_handle.goal_id.uuid.tobytes()}')
        return CancelResponse.ACCEPT  # 接受取消请求

    async def execute_callback(self, goal_handle):
        """
        执行动作目标的核心逻辑。
        当目标被接受后，此协程被调用。
        """
        self.get_logger().info(f'Executing goal with initial_value: {goal_handle.request.initial_value}')
        
        feedback_msg = ProcessTask.Feedback()
        initial_value = goal_handle.request.initial_value
        
        # 模拟一个分10步完成的任务
        for i in range(1, 11):
            # 检查是否有取消请求
            if goal_handle.is_cancel_requested:
                goal_handle.canceled() # 将目标状态标记为 CANCELED
                self.get_logger().info('Goal canceled by client.')
                result = ProcessTask.Result()
                result.success = False
                result.final_message = 'Task was canceled.'
                return result

            feedback_msg.percent_complete = i * 10
            feedback_msg.current_status = f'Processing step {i}/10 with value {initial_value * i}'
            
            self.get_logger().info(f'Publishing feedback: {feedback_msg.current_status} ({feedback_msg.percent_complete}%)')
            goal_handle.publish_feedback(feedback_msg)
            
            time.sleep(1) # 模拟耗时操作

        goal_handle.succeed() # 将目标状态标记为 SUCCEEDED
        
        result = ProcessTask.Result()
        result.success = True
        result.final_message = f'Task completed successfully with final value: {initial_value * 10}'
        self.get_logger().info(f'Returning result: {result.final_message}')
        return result

def main(args=None):
    rclpy.init(args=args)
    node = DemoActionServerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('ActionServerNode stopped cleanly')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

python_architecture_demo/python_architecture_demo/action_client_node.py

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from my_interfaces.action import ProcessTask # 导入自定义动作
import sys
import threading

class DemoActionClientNode(Node):
    """
    演示调用动作的节点。
    它会创建一个动作客户端，用于请求 'process_task_action' 动作。
    """
    def __init__(self):
        super().__init__('demo_action_client_node')
        self._action_client = ActionClient(self, ProcessTask, 'process_task_action')
        self.get_logger().info("ActionClientNode initialized.")
        self._goal_handle = None # 用于存储当前目标的句柄

    def send_goal(self, initial_value):
        """
        发送动作目标。
        """
        self.get_logger().info("Waiting for 'process_task_action' server...")
        self._action_client.wait_for_server() # 等待动作服务器可用

        goal_msg = ProcessTask.Goal()
        goal_msg.initial_value = initial_value

        self.get_logger().info(f'Sending goal request with initial_value: {initial_value}...')
        
        # 发送目标，并指定反馈回调函数
        # send_goal_async 返回一个 future，该 future 的结果是 GoalHandle 对象
        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg,
            feedback_callback=self.feedback_callback)

        # 为 _send_goal_future 添加完成时的回调
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        """
        当服务器响应目标请求（接受或拒绝）时调用。
        """
        self._goal_handle = future.result() # 获取 GoalHandle
        if not self._goal_handle.accepted:
            self.get_logger().info('Goal rejected by server :(')
            return

        self.get_logger().info('Goal accepted by server :)')
        
        # 目标被接受后，获取结果的 future
        self._get_result_future = self._goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def feedback_callback(self, feedback_msg):
        """
        当服务器发送反馈时调用。
        """
        feedback = feedback_msg.feedback
        self.get_logger().info(f'Received feedback: Status="{feedback.current_status}", Complete={feedback.percent_complete}%')

    def get_result_callback(self, future):
        """
        当动作完成（成功、中止或取消）并返回结果时调用。
        """
        result = future.result().result # 获取最终的 ProcessTask.Result 对象
        status = future.result().status # 获取动作状态 (GoalStatus)

        if status == rclpy.action.GoalStatus.STATUS_SUCCEEDED:
             self.get_logger().info(f'Goal succeeded! Result: success={result.success}, message="{result.final_message}"')
        elif status == rclpy.action.GoalStatus.STATUS_ABORTED:
            self.get_logger().error('Goal aborted by server!')
        elif status == rclpy.action.GoalStatus.STATUS_CANCELED:
            self.get_logger().warn('Goal canceled!')
        else:
            self.get_logger().error(f'Goal failed with unknown status: {status}')
        
        # 优雅地关闭 rclpy，以便主线程可以退出
        # 注意：在回调中直接 shutdown 可能不是最佳实践，但为了演示简单性
        # 更好的方法是使用事件或条件变量来通知主线程
        # rclpy.shutdown() # 在实际应用中，让主线程处理关闭

    def cancel_current_goal(self):
        """
        尝试取消当前正在执行的目标。
        """
        if self._goal_handle:
            self.get_logger().info("Attempting to cancel the current goal...")
            # cancel_goal_async 返回一个 future，其结果是 CancelGoal.Response
            cancel_future = self._goal_handle.cancel_goal_async()
            # 这里可以为 cancel_future 添加回调来处理取消结果
            # rclpy.spin_until_future_complete(self, cancel_future) # 等待取消完成
            # if cancel_future.result():
            #     self.get_logger().info(f"Cancel request sent. Goals canceled: {len(cancel_future.result().goals_canceling)}")
            # else:
            #     self.get_logger().error("Failed to send cancel request.")
        else:
            self.get_logger().warn("No active goal to cancel.")


def main(args=None):
    rclpy.init(args=args)
    action_client_node = DemoActionClientNode()

    if len(sys.argv) >= 2:
        try:
            initial_value = int(sys.argv[1])
        except ValueError:
            action_client_node.get_logger().error("Invalid argument. Please provide an integer initial_value.")
            action_client_node.destroy_node()
            rclpy.shutdown()
            return
    else:
        action_client_node.get_logger().info("No initial_value provided, using default: 10")
        initial_value = 10

    action_client_node.send_goal(initial_value)

    # 创建一个线程来监听用户输入以取消目标
    # 这是为了演示取消功能，实际应用中可能有其他触发方式
    def listen_for_cancel():
        input("Press Enter to attempt to cancel the goal...\n")
        action_client_node.cancel_current_goal()

    # cancel_thread = threading.Thread(target=listen_for_cancel)
    # cancel_thread.start()

    try:
        # 保持节点运行以处理回调
        # 注意：如果所有工作都在回调中完成，并且希望在动作完成后自动退出，
        # 你需要一种机制（例如，在 get_result_callback 中设置一个事件）来优雅地停止 spin。
        # 对于这个演示，我们让它一直 spin 直到手动停止或回调中调用 rclpy.shutdown()。
        # 如果在 get_result_callback 中调用了 rclpy.shutdown(), spin 会在那里退出。
        rclpy.spin(action_client_node)
    except KeyboardInterrupt:
        action_client_node.get_logger().info('ActionClientNode stopped by user.')
        action_client_node.cancel_current_goal() # 尝试取消
        # 给一点时间处理取消
        # time.sleep(1)
    finally:
        action_client_node.get_logger().info('Shutting down ActionClientNode.')
        action_client_node.destroy_node()
        if rclpy.ok(): # 只有在 rclpy 仍然 ok 的时候才 shutdown (防止重复 shutdown)
            rclpy.shutdown()
    # if cancel_thread.is_alive():
    #     cancel_thread.join()


if __name__ == '__main__':
    main()

python_architecture_demo/python_architecture_demo/lifecycle_manager_node.py

import rclpy
from rclpy.lifecycle import Node as LifecycleNode # 注意导入的是 rclpy.lifecycle.Node
from rclpy.lifecycle import State, TransitionCallbackReturn
from std_msgs.msg import String # 用于演示在激活状态下发布消息
import time

class DemoLifecycleNode(LifecycleNode):
    """
    演示具有受管理生命周期的节点。
    此节点会经历配置、激活、停用、清理等状态。
    只有在 'active' 状态下，它才会周期性地发布消息。
    """
    def __init__(self, **kwargs):
        super().__init__('demo_lifecycle_node', **kwargs)
        self.publisher_ = None # 发布者将在配置阶段创建
        self.timer_ = None     # 定时器将在激活阶段创建
        self.message_count_ = 0
        self.get_logger().info(f"{self.get_name()} is in 'unconfigured' state (initial).")

    # --- 生命周期转换回调 ---

    def on_configure(self, state: State) -> TransitionCallbackReturn:
        """
        当节点从 'unconfigured' 转换到 'inactive' 状态时调用。
        通常用于初始化资源，如创建发布者、订阅者、服务等。
        """
        self.get_logger().info(f"'{self.get_name()}' received request to 'configure' from state '{state.label}'.")
        try:
            # 创建发布者，但此时还不用它
            self.publisher_ = self.create_publisher(String, 'lifecycle_topic', 10)
            # 模拟一些配置过程
            time.sleep(0.5) 
            self.get_logger().info(f"Configuration successful. Publisher for 'lifecycle_topic' created.")
            return TransitionCallbackReturn.SUCCESS
        except Exception as e:
            self.get_logger().error(f"Configuration failed: {e}")
            return TransitionCallbackReturn.FAILURE

    def on_activate(self, state: State) -> TransitionCallbackReturn:
        """
        当节点从 'inactive' 转换到 'active' 状态时调用。
        通常用于启动节点的活动，如启动定时器、使能数据处理等。
        """
        self.get_logger().info(f"'{self.get_name()}' received request to 'activate' from state '{state.label}'.")
        try:
            # 启动定时器，只有在激活状态下才发布消息
            if self.publisher_ is None:
                self.get_logger().error("Publisher not created. Cannot activate without configuring first.")
                return TransitionCallbackReturn.FAILURE
            
            self.timer_ = self.create_timer(1.0, self.timer_callback) # 每秒调用一次
            # 通过调用父类的 on_activate 来正确设置状态
            super().on_activate(state) # 重要: 确保状态正确转换
            self.get_logger().info(f"Activation successful. Timer started. Now publishing to 'lifecycle_topic'.")
            return TransitionCallbackReturn.SUCCESS
        except Exception as e:
            self.get_logger().error(f"Activation failed: {e}")
            return TransitionCallbackReturn.FAILURE

    def on_deactivate(self, state: State) -> TransitionCallbackReturn:
        """
        当节点从 'active' 转换到 'inactive' 状态时调用。
        通常用于停止节点的活动，如停止定时器。资源（如发布者）仍然保留。
        """
        self.get_logger().info(f"'{self.get_name()}' received request to 'deactivate' from state '{state.label}'.")
        try:
            if self.timer_ is not None:
                self.timer_.cancel()
                self.timer_ = None
            # 通过调用父类的 on_deactivate 来正确设置状态
            super().on_deactivate(state) # 重要
            self.get_logger().info(f"Deactivation successful. Timer stopped.")
            return TransitionCallbackReturn.SUCCESS
        except Exception as e:
            self.get_logger().error(f"Deactivation failed: {e}")
            return TransitionCallbackReturn.FAILURE

    def on_cleanup(self, state: State) -> TransitionCallbackReturn:
        """
        当节点从 'inactive' 转换到 'unconfigured' 状态时调用。
        通常用于释放 'on_configure' 中分配的资源。
        """
        self.get_logger().info(f"'{self.get_name()}' received request to 'cleanup' from state '{state.label}'.")
        try:
            if self.publisher_ is not None:
                self.destroy_publisher(self.publisher_)
                self.publisher_ = None
            if self.timer_ is not None: # 确保如果从 active 直接 cleanup，定时器也被处理
                self.timer_.cancel()
                self.timer_ = None
            self.get_logger().info(f"Cleanup successful. Resources released.")
            return TransitionCallbackReturn.SUCCESS
        except Exception as e:
            self.get_logger().error(f"Cleanup failed: {e}")
            return TransitionCallbackReturn.FAILURE

    def on_shutdown(self, state: State) -> TransitionCallbackReturn:
        """
        当节点从任何状态转换到 'finalized' 状态时调用（通常在节点关闭前）。
        用于执行最后的清理工作。
        """
        self.get_logger().info(f"'{self.get_name()}' received request to 'shutdown' from state '{state.label}'.")
        try:
            # 确保所有资源都被清理
            if self.publisher_ is not None:
                self.destroy_publisher(self.publisher_)
                self.publisher_ = None
            if self.timer_ is not None:
                self.timer_.cancel()
                self.timer_ = None
            self.get_logger().info(f"Shutdown successful. Node is finalizing.")
            return TransitionCallbackReturn.SUCCESS
        except Exception as e:
            self.get_logger().error(f"Shutdown failed: {e}")
            return TransitionCallbackReturn.FAILURE
            
    def on_error(self, state: State) -> TransitionCallbackReturn:
        """
        当转换回调返回 FAILURE 或发生未捕获异常时调用。
        节点将进入 'errorprocessing' 状态。
        """
        self.get_logger().error(f"'{self.get_name()}' encountered an error and is now in 'errorprocessing' state from '{state.label}'.")
        # 通常这里可以尝试恢复或执行特定错误处理逻辑
        # 为简单起见，我们直接返回 SUCCESS，允许系统尝试 cleanup 或 shutdown
        return TransitionCallbackReturn.SUCCESS


    # --- 节点的主要逻辑 (在 active 状态下执行) ---
    def timer_callback(self):
        """
        只有当节点处于 'active' 状态时，此定时器才会被创建和调用。
        """
        # 再次检查状态，以防万一（虽然理论上定时器只在 active 时运行）
        if self.get_current_lifecycle_state() == State(label='active'):
            msg = String()
            msg.data = f"Lifecycle node says: Active! Count: {self.message_count_}"
            if self.publisher_:
                self.publisher_.publish(msg)
                self.get_logger().info(f'Publishing: "{msg.data}"')
                self.message_count_ += 1
        else:
            self.get_logger().warn(f"Timer callback invoked but node is not active (current state: {self.get_current_lifecycle_state().label}). This should not happen if timer is managed correctly.")


def main(args=None):
    rclpy.init(args=args)
    
    # 注意：生命周期节点需要 executor 来处理转换请求
    # 使用 MultiThreadedExecutor 可以确保转换服务和节点本身的spin并行处理
    executor = rclpy.executors.MultiThreadedExecutor() 
    node = DemoLifecycleNode()
    executor.add_node(node)
    
    node.get_logger().info("LifecycleNode created. Use 'ros2 lifecycle' commands to manage its state.")
    node.get_logger().info("Example: ros2 lifecycle set /demo_lifecycle_node configure")
    node.get_logger().info("         ros2 lifecycle set /demo_lifecycle_node activate")
    
    try:
        executor.spin() # 使用 executor 的 spin
    except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
        node.get_logger().info('LifecycleNode stopped or executor externally shutdown.')
    finally:
        # 在关闭前，尝试将节点转换到 unconfigured 和 finalized 状态
        # 这对于确保资源被正确释放很重要
        if node.get_current_lifecycle_state().label == 'active':
            node.get_logger().info('Attempting to deactivate and cleanup node before shutdown...')
            node.trigger_transition(rclpy.lifecycle.Transition(label='deactivate'))
            # executor.spin_once(timeout_sec=1.0) # 给转换一点时间
            node.trigger_transition(rclpy.lifecycle.Transition(label='cleanup'))
            # executor.spin_once(timeout_sec=1.0)
        elif node.get_current_lifecycle_state().label == 'inactive':
            node.get_logger().info('Attempting to cleanup node before shutdown...')
            node.trigger_transition(rclpy.lifecycle.Transition(label='cleanup'))
            # executor.spin_once(timeout_sec=1.0)

        node.get_logger().info('Destroying LifecycleNode.')
        # 显式销毁节点并关闭 executor
        # 注意：如果 executor 仍在 spin，destroy_node 可能不会立即完成
        # executor.shutdown() # 先停止 executor
        node.destroy_node()
        if rclpy.ok():
             rclpy.try_shutdown() # 使用 try_shutdown 更安全

if __name__ == '__main__':
    main()

4. 启动文件 (python_architecture_demo/launch/demo_launch.py)

这个 launch 文件将启动上面定义的大部分节点。

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    """
    生成启动描述，用于启动演示中的多个节点。
    """
    return LaunchDescription([
        Node(
            package='python_architecture_demo',
            executable='parameter_node',
            name='my_parameter_node', # 可以覆盖节点在代码中定义的名称
            output='screen',
            emulate_tty=True, # 确保日志颜色等正常显示
            parameters=[ # 在启动时设置参数
                {'my_message': 'Hello from Launch File!'},
                {'print_interval_sec': 2.5}
            ]
        ),
        Node(
            package='python_architecture_demo',
            executable='publisher_node',
            name='my_publisher_node',
            output='screen',
            emulate_tty=True,
            parameters=[
                {'publish_frequency_hz': 0.5} # 0.5 Hz = 2秒一次
            ]
        ),
        Node(
            package='python_architecture_demo',
            executable='subscriber_node',
            name='my_subscriber_node',
            output='screen',
            emulate_tty=True,
        ),
        Node(
            package='python_architecture_demo',
            executable='service_server_node',
            name='my_service_server_node',
            output='screen',
            emulate_tty=True,
        ),
        # 服务客户端节点通常是按需运行或由其他逻辑触发，这里不自动启动
        # Node(
        #     package='python_architecture_demo',
        #     executable='service_client_node',
        #     name='my_service_client_node',
        #     output='screen',
        #     emulate_tty=True,
        #     # arguments=['10', '25'] # 可以传递命令行参数给客户端
        # ),
        Node(
            package='python_architecture_demo',
            executable='action_server_node',
            name='my_action_server_node',
            output='screen',
            emulate_tty=True,
        ),
        # 动作客户端节点也是按需运行
        # Node(
        #     package='python_architecture_demo',
        #     executable='action_client_node',
        #     name='my_action_client_node',
        #     output='screen',
        #     emulate_tty=True,
        #     # arguments=['5'] # 初始值
        # ),
        Node(
            package='python_architecture_demo',
            executable='lifecycle_node',
            name='my_lifecycle_node', # 使用不同的名称以便区分
            output='screen',
            emulate_tty=True,
        ),
    ])

---

5. 构建和运行

1. 创建工作空间和包：
   按照上面的目录结构创建 ros2_demo_ws, my_interfaces 和 python_architecture_demo 包。
   将对应的 .msg, .srv, .action, package.xml, CMakeLists.txt, .py, setup.py, setup.cfg 文件放入相应位置。
2. 构建包：
   进入工作空间根目录 (ros2_demo_ws)，然后运行：

colcon build --symlink-install

--symlink-install 使得修改 Python 代码后无需重新构建即可生效（除非修改了 setup.py 或 package.xml 或添加/删除文件）。

1. Source 工作空间：

source install/setup.bash
# 或者 source install/local_setup.bash

1. 运行节点：

• • 单独运行节点：
    打开多个终端，每个终端 source 工作空间后，运行：

# 终端 1: 参数节点
ros2 run python_architecture_demo parameter_node
# 终端 2: 发布者
ros2 run python_architecture_demo publisher_node
# 终端 3: 订阅者
ros2 run python_architecture_demo subscriber_node
# 终端 4: 服务服务器
ros2 run python_architecture_demo service_server_node
# 终端 5: 服务客户端 (它会发送请求然后退出)
ros2 run python_architecture_demo service_client_node 7 8 # 请求 7 + 8
# 终端 6: 动作服务器
ros2 run python_architecture_demo action_server_node
# 终端 7: 动作客户端 (它会发送目标然后等待结果)
ros2 run python_architecture_demo action_client_node 3 # 初始值为3
# 终端 8: 生命周期节点
ros2 run python_architecture_demo lifecycle_node

• • 使用 Launch 文件运行大部分节点：
    在一个终端中运行：

ros2 launch python_architecture_demo demo_launch.py

这会启动 parameter_node, publisher_node, subscriber_node, service_server_node, action_server_node, 和 lifecycle_node。
你仍然需要手动运行客户端节点（服务和动作）来与它们交互，或者将它们集成到更复杂的逻辑中。

1. 与节点交互：

• • 查看主题和消息：

ros2 topic list
ros2 topic echo /custom_topic
ros2 topic echo /lifecycle_topic

• • 调用服务： (如果服务服务器正在运行)

ros2 service list
ros2 service type /compute_sum_service
ros2 service call /compute_sum_service my_interfaces/srv/ComputeSum "{a: 10, b: 20}"

• • 发送动作目标： (如果动作服务器正在运行)

ros2 action list
ros2 action info /process_task_action
ros2 action send_goal /process_task_action my_interfaces/action/ProcessTask "{initial_value: 5}" --feedback

• • 管理参数：

ros2 param list /my_parameter_node # 注意 launch 文件中改了名
ros2 param get /my_parameter_node my_message
ros2 param set /my_parameter_node my_message "A new message from CLI!"

• • 管理生命周期节点：

ros2 lifecycle list # 查看所有可管理节点
ros2 lifecycle get /my_lifecycle_node # 获取当前状态
ros2 lifecycle set /my_lifecycle_node configure
ros2 lifecycle get /my_lifecycle_node
ros2 lifecycle set /my_lifecycle_node activate
# 当它 active 时，你会看到 /lifecycle_topic 上有消息
ros2 topic echo /lifecycle_topic
ros2 lifecycle set /my_lifecycle_node deactivate
ros2 lifecycle set /my_lifecycle_node cleanup
ros2 lifecycle set /my_lifecycle_node shutdown # 使其进入 finalized

---

这个 Demo 提供了一个 ROS 2 Python 应用程序的基本架构骨架。在实际项目中，这些节点的功能会更加复杂，并且会根据具体应用场景进行组织和交互。例如，一个机器人可能有一个感知节点（发布传感器数据）、一个规划节点（订阅传感器数据，提供动作服务进行路径规划）、一个控制节点（订阅规划结果，发布电机指令）和一个状态管理节点（生命周期节点，协调其他节点的启动和关闭）。

---