int64 a
int64 b
---
int64 sum

# 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.add_callback
        )
        self.get_logger().info('服务已启动，等待请求...')

    def add_callback(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 sys
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('等待服务上线...')

    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()
    # 从命令行参数获取两个整数
    a = int(sys.argv[1]) if len(sys.argv) > 1 else 2
    b = int(sys.argv[2]) if len(sys.argv) > 2 else 3
    response = node.send_request(a, b)
    node.get_logger().info(f'结果: {a} + {b} = {response.sum}')
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

# 终端1：启动服务端
ros2 run demo_nodes_py add_two_ints_server

# 终端2：启动客户端
python3 add_two_ints_client.py 41 58

# sync_call.py
import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts


class SyncClient(Node):
    def __init__(self):
        super().__init__('sync_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('等待服务上线...')

    def call(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)
        if future.result() is not None:
            self.get_logger().info(f'同步结果: {future.result().sum}')
        else:
            self.get_logger().error('请求失败')


def main():
    rclpy.init()
    node = SyncClient()
    node.call(10, 20)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

# async_call.py
import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts


class AsyncClient(Node):
    def __init__(self):
        super().__init__('async_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('等待服务上线...')

    def call(self, a, b):
        req = AddTwoInts.Request()
        req.a = a
        req.b = b
        future = self.cli.call_async(req)
        # 注册回调，响应到达时自动触发
        future.add_done_callback(self.response_callback)

    def response_callback(self, future):
        result = future.result()
        if result is not None:
            self.get_logger().info(f'异步结果: {result.sum}')
        else:
            self.get_logger().error('请求失败')


def main():
    rclpy.init()
    node = AsyncClient()
    node.call(10, 20)
    # 需要spin让回调有机会执行
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

# 目标：计算斐波那契数列的前N项
int32 order
---
# 结果：最终的数列
int32[] sequence
---
# 反馈：当前计算进度
int32[] partial_sequence

# fibonacci_action_server.py
import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node
from example_interfaces.action import Fibonacci


class FibonacciActionServer(Node):
    def __init__(self):
        super().__init__('fibonacci_action_server')
        # 使用ReentrantCallbackGroup支持并发执行
        self._action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
            execute_callback=self.execute_callback,
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback,
            callback_group=ReentrantCallbackGroup(),
        )
        self.get_logger().info('动作服务端已启动')

    def goal_callback(self, goal_request):
        """决定是否接受目标"""
        self.get_logger().info(f'收到目标请求: order={goal_request.order}')
        # 拒绝非法输入
        if goal_request.order < 0:
            self.get_logger().warn('拒绝：order不能为负数')
            return GoalResponse.REJECT
        return GoalResponse.ACCEPT

    def cancel_callback(self, goal_handle):
        """决定是否接受取消"""
        self.get_logger().info('收到取消请求')
        return CancelResponse.ACCEPT

    async def execute_callback(self, goal_handle):
        """执行目标，发送反馈和结果"""
        self.get_logger().info('开始执行...')
        order = goal_handle.request.order
        feedback_msg = Fibonacci.Feedback()

        # 初始化斐波那契数列
        seq = [0, 1]

        for i in range(1, order):
            # 检查是否被取消——必须周期性检查
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('目标已取消')
                result = Fibonacci.Result()
                result.sequence = seq
                return result

            # 计算下一项
            next_val = seq[-1] + seq[-2]
            seq.append(next_val)

            # 发布反馈
            feedback_msg.partial_sequence = seq
            goal_handle.publish_feedback(feedback_msg)
            self.get_logger().info(f'反馈: {seq}')

            # 模拟耗时操作
            import time
            time.sleep(0.5)

        # 完成
        goal_handle.succeed()
        result = Fibonacci.Result()
        result.sequence = seq
        self.get_logger().info(f'完成: {seq}')
        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.action import ActionClient
from rclpy.node import Node
from example_interfaces.action import Fibonacci


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

    def send_goal(self, order):
        """发送目标"""
        self._action_client.wait_for_server()
        goal_msg = Fibonacci.Goal()
        goal_msg.order = order
        self.get_logger().info(f'发送目标: order={order}')

        # 发送目标，注册反馈回调和目标响应回调
        send_goal_future = self._action_client.send_goal_async(
            goal_msg,
            feedback_callback=self.feedback_callback,
        )
        send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        """处理目标接受/拒绝"""
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().warn('目标被拒绝')
            return
        self._goal_handle = goal_handle
        self.get_logger().info('目标已接受，等待结果...')
        # 注册结果回调
        get_result_future = goal_handle.get_result_async()
        get_result_future.add_done_callback(self.get_result_callback)

    def feedback_callback(self, feedback_msg):
        """接收反馈"""
        self.get_logger().info(f'收到反馈: {feedback_msg.partial_sequence}')

    def get_result_callback(self, future):
        """获取最终结果"""
        result = future.result().result
        status = future.result().status
        # 状态码：4=SUCCEEDED, 5=CANCELED, 6=ABORTED
        status_names = {4: 'SUCCEEDED', 5: 'CANCELED', 6: 'ABORTED'}
        self.get_logger().info(
            f'结果: {result.sequence}, 状态: {status_names.get(status, status)}'
        )

    def cancel_goal(self):
        """取消目标"""
        if self._goal_handle is not None:
            self.get_logger().info('发送取消请求...')
            future = self._goal_handle.cancel_goal_async()
            future.add_done_callback(self.cancel_done_callback)

    def cancel_done_callback(self, future):
        """取消完成回调"""
        cancel_response = future.result()
        if len(cancel_response.goals_canceling) > 0:
            self.get_logger().info('取消成功')
        else:
            self.get_logger().warn('取消失败')


def main(args=None):
    rclpy.init()
    node = FibonacciActionClient()
    node.send_goal(order=8)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

# 终端1：启动动作服务端
python3 fibonacci_action_server.py

# 终端2：启动动作客户端
python3 fibonacci_action_client.py

# 列出所有活跃服务
ros2 service list

# 查看服务类型
ros2 service type /add_two_ints

# 查找使用某类型的服务
ros2 service find example_interfaces/srv/AddTwoInts

# 调用服务
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 10, b: 20}"

# 列出所有活跃动作
ros2 action list

# 查看动作详细信息（包括类型、目标/结果/反馈字段）
ros2 action info /fibonacci

# 发送目标并接收反馈
ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 5}" --feedback