6.1 Python接口使用示例

本章节将带您实现索引所示若干功能

构建 & 运行说明

• 进入到 AimDK 顶层目录(机器人上为 ~/aimdk)，执行以下命令

  source /opt/ros/humble/setup.bash
  colcon build
  source install/local_setup.bash
  ros2 run py_examples '对应功能名称如: get_mc_action'
  

📝 代码说明

完整的代码实现包含完整的错误处理、信号处理、超时处理等机制，确保程序的健壮性。请您在 py_examples 目录下查看/修改

小心

ROS的服务(service)机制在跨板通信时存在一些待优化问题, 二次开发时请参考例程添加异常处理、快速重试等保护机制

机器人处于稳定站立 / 走跑模式时, 请勿批量快速启动 ROS 节点(建议不超过 2 个/秒), 短时间内大量节点参与 DDS 发现会造成通信拥塞、影响运控实时性, 存在机器人失稳摔倒风险

6.1.1 进入开发者模式

通过调用'GetSystemState'服务获取机器人当前的系统状态，并且通过'MigrateSystemState'进入对应的开发者模式。

开发者模式定义

#!/usr/bin/env python3
"""
System State Migrator - Synchronous Python version
"""

import sys
import time
import rclpy
from rclpy.node import Node
import aimdk_msgs.srv as aimdk_srv
import aimdk_msgs.msg as aimdk_msg


class SyncSystemStateMigrator(Node):
    """Synchronous version of system state migrator"""

    def __init__(self, target_state: str):
        super().__init__('sync_system_state_migrator')
        self.target_state = target_state

        # Create service clients
        self.migrate_client = self.create_client(
            aimdk_srv.MigrateSystemState,
            '/aimdk_5Fmsgs/srv/MigrateSystemState'
        )

        self.get_state_client = self.create_client(
            aimdk_srv.GetSystemState,
            '/aimdk_5Fmsgs/srv/GetSystemState'
        )

        self.get_logger().info(f'Target state: {self.target_state}')

    def wait_for_services(self, timeout_sec: float = 30.0) -> bool:
        """Wait for services to become available"""
        self.get_logger().info('Waiting for services...')

        start_time = time.time()
        while time.time() - start_time < timeout_sec and rclpy.ok():
            if (self.migrate_client.wait_for_service(timeout_sec=1.0) and
                    self.get_state_client.wait_for_service(timeout_sec=1.0)):
                self.get_logger().info('Services are available')
                return True

            self.get_logger().info('Services not available, waiting...')
            time.sleep(0.5)

        self.get_logger().error(
            f'Timeout waiting for services after {timeout_sec} seconds')
        return False

    def migrate_state_sync(self, max_retries: int = 5, timeout_sec: float = 30.0) -> bool:
        """Send migration request synchronously with retry"""
        request = aimdk_srv.MigrateSystemState.Request()
        now = self.get_clock().now()
        request.header.header.stamp.sec = now.nanoseconds // 1_000_000_000
        request.header.header.stamp.nanosec = now.nanoseconds % 1_000_000_000
        request.state = self.target_state

        for attempt in range(1, max_retries + 1):
            self.get_logger().info(
                f'Sending migration request to {self.target_state} '
                f'(attempt {attempt}/{max_retries})...'
            )

            future = None
            try:
                future = self.migrate_client.call_async(request)

                start_time = time.time()
                while rclpy.ok() and time.time() - start_time < timeout_sec:
                    rclpy.spin_once(self, timeout_sec=0.1)
                    if future.done():
                        response = future.result()
                        # SUCCESS (1)
                        if response.header.status.value == aimdk_msg.CommonState.SUCCESS:
                            self.get_logger().info('Migration request accepted')
                            return True
                        else:
                            self.get_logger().error(
                                f'Migration failed: {response.header.message}'
                            )
                            return False

                self.get_logger().warning(
                    f'Timeout waiting for migration response '
                    f'(attempt {attempt}/{max_retries})'
                )

            except Exception as e:
                self.get_logger().error(
                    f'Migration service call failed (attempt {attempt}/{max_retries}): {e}'
                )
            finally:
                if future is not None and not future.done():
                    future.cancel()

            if attempt < max_retries:
                self.get_logger().info('Retrying migration request in 2.0 seconds...')
                time.sleep(2.0)

        self.get_logger().error(
            f'Migration request failed after {max_retries} attempts'
        )
        return False

    def get_current_state_sync(self, max_retries: int = 3, timeout_sec: float = 10.0):
        """Get current system state synchronously with retry"""
        request = aimdk_srv.GetSystemState.Request()
        now = self.get_clock().now()
        request.header.header.stamp.sec = now.nanoseconds // 1_000_000_000
        request.header.header.stamp.nanosec = now.nanoseconds % 1_000_000_000

        for attempt in range(1, max_retries + 1):
            future = None
            try:
                future = self.get_state_client.call_async(request)

                start_time = time.time()
                while rclpy.ok() and time.time() - start_time < timeout_sec:
                    rclpy.spin_once(self, timeout_sec=0.1)
                    if future.done():
                        return future.result()

                self.get_logger().warning(
                    f'Timeout getting system state (attempt {attempt}/{max_retries})'
                )

            except Exception as e:
                self.get_logger().error(
                    f'GetSystemState service call failed (attempt {attempt}/{max_retries}): {e}'
                )
            finally:
                if future is not None and not future.done():
                    future.cancel()

            if attempt < max_retries:
                time.sleep(1.0)

        self.get_logger().error(
            f'Failed to get system state after {max_retries} attempts'
        )
        return None

    def monitor_migration_sync(self, max_checks: int = 300) -> bool:
        """Monitor migration progress synchronously"""
        self.get_logger().info('Monitoring migration progress...')

        check_count = 0
        start_time = time.time()

        while check_count < max_checks and rclpy.ok():
            # Wait between checks
            time.sleep(1.0)

            # Check current state
            response = self.get_current_state_sync()
            if response is None:
                self.get_logger().warning('Failed to get system state, will retry...')
                continue

            check_count += 1

            # Check if migration is complete
            # Ready auto-transitions to Business, so also accept Business
            # when the target is Ready
            current_state_lower = response.cur_state.lower()
            target_state_lower = self.target_state.lower()

            state_match = (
                current_state_lower == target_state_lower or
                (target_state_lower == 'ready' and
                 current_state_lower == 'business')
            )
            ready_auto_transitioned = (
                target_state_lower == 'ready' and
                current_state_lower == 'business'
            )
            status_match = (
                ready_auto_transitioned or
                response.curr_status.value == aimdk_msg.SystemStatus.IN_READY
            )

            self.get_logger().info(
                f'Check {check_count}: State="{response.cur_state}" '
                f'(match={state_match}), Status={response.curr_status.value} (match={status_match})'
            )

            if state_match and status_match:
                self.get_logger().info(
                    f'Migration to {self.target_state} completed successfully!')
                return True

            # Check timeout
            if time.time() - start_time > 300.0:  # 5 minutes total timeout
                self.get_logger().error('Migration timeout after 5 minutes')
                return False

        self.get_logger().error(f'Migration failed after {max_checks} checks')
        return False

    def run_migration(self) -> bool:
        """Run complete migration process"""
        # Step 1: Wait for services
        if not self.wait_for_services():
            return False

        # Step 2: Send migration request
        if not self.migrate_state_sync():
            return False

        # Step 3: Monitor migration progress
        return self.monitor_migration_sync()


def main():
    """
    Main function
    """
    # Check command line arguments
    if len(sys.argv) != 2:
        print(f'Usage: {sys.argv[0]} <target_state>')
        print('\nAvailable target states: Ready, Develop_Nav, Develop_Audio_Linux, Develop_Audio_ROS, Develop_MC')
        return 1

    # Get target state from command line argument
    target_state = sys.argv[1]

    # Validate target state
    if not target_state:
        print('Error: Target state cannot be empty', file=sys.stderr)
        return 1

    # Initialize ROS2
    rclpy.init()

    result = 1
    node = None

    try:
        # Create migrator node
        node = SyncSystemStateMigrator(target_state)

        # Run migration
        if node.run_migration():
            print(f'\nMigration to {target_state} completed successfully!')
            result = 0
        else:
            print(f'\nMigration to {target_state} failed')
            result = 1

    except KeyboardInterrupt:
        print('\nProgram interrupted by user')
        result = 1
    except Exception as e:
        print(f'Error: {e}', file=sys.stderr)
        result = 1
    finally:
        # Cleanup
        if node:
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()

    return result


if __name__ == '__main__':
    sys.exit(main())

使用说明

ros2 run py_examples migrate_system_state Ready

输出示例

Ready 是开机后的过渡态，系统会自动迁移到 Business。因此以 Ready 为目标时，监控循环可能在 Business 状态下完成（视为迁移成功）。

[INFO] [1770125663.176574907] [sync_system_state_migrator]: Target state: Ready
[INFO] [1770125663.176758843] [sync_system_state_migrator]: Waiting for services...
[INFO] [1770125663.678726368] [sync_system_state_migrator]: Services are available
[INFO] [1770125663.679642487] [sync_system_state_migrator]: Sending migration request to Ready (attempt 1/5)...
[INFO] [1770125663.692394889] [sync_system_state_migrator]: Migration request accepted
[INFO] [1770125663.693117341] [sync_system_state_migrator]: Monitoring migration progress...
[INFO] [1770125664.728735142] [sync_system_state_migrator]: Check 1: State="Business" (match=True), Status=2 (match=True)
[INFO] [1770125664.728735142] [sync_system_state_migrator]: Migration to Ready completed successfully!

6.1.2 获取机器人模式

通过调用GetMcAction服务获取机器人当前的运行模式，包括名称和状态信息。

运动模式定义

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import GetMcAction
from aimdk_msgs.msg import CommonRequest


class GetMcActionClient(Node):
    def __init__(self):
        super().__init__('get_mc_action_client')
        self.client = self.create_client(
            GetMcAction, '/aimdk_5Fmsgs/srv/GetMcAction')
        self.get_logger().info('✅ GetMcAction client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self):
        request = GetMcAction.Request()
        request.request = CommonRequest()

        self.get_logger().info('📨 Sending request to get robot mode')
        for i in range(8):
            request.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(request)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return

        self.get_logger().info('✅ Robot mode get successfully.')
        self.get_logger().info(f'Mode name: {response.info.action_desc}')
        self.get_logger().info(f'Mode status: {response.info.status.value}')


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        node = GetMcActionClient()
        node.send_request()
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明

ros2 run py_examples get_mc_action

输出示例

[INFO] [1779173975.378396584] [get_mc_action_client]: ✅ GetMcAction client node created.
[INFO] [1779173975.961720731] [get_mc_action_client]: 🟢 Service available, ready to send request.
[INFO] [1779173975.961868725] [get_mc_action_client]: 📨 Sending request to get robot mode
[INFO] [1779173975.965180630] [get_mc_action_client]: ✅ Robot mode get successfully.
[INFO] [1779173975.965282450] [get_mc_action_client]: Mode name: PASSIVE_DEFAULT
[INFO] [1779173975.965307345] [get_mc_action_client]: Mode status: 100

接口参考

• 服务：/aimdk_5Fmsgs/srv/GetMcAction

• 消息：aimdk_msgs/srv/GetMcAction

6.1.3 设置机器人模式

该示例中用到了SetMcAction服务，运行节点程序后终端输入模式对应的字段值，机器人会尝试进入相应的运动模式。
切入稳定站立(STAND_DEFAULT)模式前，确保机器人已立起且双脚着地。
模式切换需遵循模式转换图, 跨模式转换不能成功
走跑模式(LOCOMOTION_DEFAULT)和稳定站立模式一体化自动切换，按模式转换图流转到两者中较近的即可

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetMcAction
from aimdk_msgs.msg import RequestHeader, CommonState, McActionCommand


class SetMcActionClient(Node):
    def __init__(self):
        super().__init__('set_mc_action_client')
        self.client = self.create_client(
            SetMcAction, '/aimdk_5Fmsgs/srv/SetMcAction'
        )
        self.get_logger().info('✅ SetMcAction client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, action_name: str):
        req = SetMcAction.Request()
        req.header = RequestHeader()
        req.source = 'node.set_mc_action'
        #
        cmd = McActionCommand()
        cmd.action_desc = action_name
        req.command = cmd

        self.get_logger().info(
            f'📨 Sending request to set robot mode: {action_name}')
        for i in range(8):
            req.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return

        # SUCCESS (1)
        if response.response.status.value == CommonState.SUCCESS:
            self.get_logger().info('✅ Robot mode set successfully.')
        else:
            self.get_logger().error(
                f'❌ Failed (code={response.response.header.code}): '
                f'{response.response.message}'
            )


def main(args=None):
    action_info = {
        'PASSIVE_DEFAULT': ('PD', 'joints with zero torque'),
        'DAMPING_DEFAULT': ('DD', 'joints in damping mode'),
        'JOINT_DEFAULT': ('JD', 'Position Control Stand (joints locked)'),
        'STAND_DEFAULT': ('SD', 'Stable Stand (auto-balance)'),
        'LOCOMOTION_DEFAULT': ('LD', 'locomotion mode (walk or run)'),
        'HEAD_ONLY': ('HO', 'head-only control'),
        'UPPERBODY_REMOTE_SPLIT': ('URS', 'upper body remote split (head + arms + hands)'),
    }

    choices = {}
    for k, v in action_info.items():
        choices[v[0]] = k

    rclpy.init(args=args)
    node = None
    try:
        # Prefer command-line argument, otherwise prompt for input
        if len(sys.argv) > 1:
            motion = sys.argv[1]
        else:
            print('{:<4} - {:<20} : {}'.format('abbr',
                  'robot mode', 'description'))
            for k, v in action_info.items():
                print(f'{v[0]:<4} - {k:<20} : {v[1]}')
            motion = input('Enter abbr of robot mode:')

        action_name = choices.get(motion)
        if not action_name:
            raise ValueError(f'Invalid abbr of robot mode: {motion}')

        node = SetMcActionClient()
        node.send_request(action_name)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明

# 使用命令行参数设置模式（推荐）
ros2 run py_examples set_mc_action JD  # 零力矩模式>>站姿预备（位控站立）
ros2 run py_examples set_mc_action SD  # 机器人已立起且脚着地后方可执行，站姿预备>>稳定站立
# 稳定站立>>走跑模式 自动切换，无需手动切换

# 或者不带参数运行，程序会提示用户输入双字母缩写代码
ros2 run py_examples set_mc_action

输出示例

...
[INFO] [1764066567.502968540] [set_mc_action_client]: ✅ Robot mode set successfully.

注意事项

• 切入STAND_DEFAULT模式前，确保机器人已立起且双脚着地

• 模式切换可能需要几秒钟时间完成

接口参考

• 服务：/aimdk_5Fmsgs/srv/SetMcAction

• 消息：aimdk_msgs/srv/SetMcAction

6.1.4 设置机器人动作

该示例中用到了preset_motion_client，进入稳定站立模式后，启动节点程序，输入相应的字段值可实现左手（右手）的握手（举手、挥手、飞吻）动作。

可供调用的参数见预设动作表

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetMcPresetMotion, SetMcInputSource
from aimdk_msgs.msg import (McPresetMotion, McControlArea, McInputAction,
                            RequestHeader, CommonState)

INPUT_SOURCE_NAME = "node.preset_motion"
INPUT_SOURCE_PRIORITY = 40
INPUT_SOURCE_TIMEOUT = 1000


class SetMcPresetMotionClient(Node):
    def __init__(self):
        super().__init__('preset_motion_client')
        self.client = self.create_client(
            SetMcPresetMotion, '/aimdk_5Fmsgs/srv/SetMcPresetMotion')
        self.input_source_client = self.create_client(
            SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource')
        self.get_logger().info('✅ SetMcPresetMotion client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def _call_input_source(self, action_value: int) -> bool:
        action_name = {
            McInputAction.INPUTACTION_ADD: "ADD",
            McInputAction.INPUTACTION_ENABLE: "ENABLE",
        }.get(action_value, str(action_value))

        req = SetMcInputSource.Request()
        req.action.value = action_value
        req.input_source.name = INPUT_SOURCE_NAME
        req.input_source.priority = INPUT_SOURCE_PRIORITY
        req.input_source.timeout = INPUT_SOURCE_TIMEOUT

        future = None
        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.input_source_client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
            if future.done():
                break
            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(
                f'trying input source action {action_name} ({action_value})... [{i}]')

        if future is None or not future.done():
            return False
        response = future.result()
        return response is not None and response.response.header.code == 0

    def register_input_source(self) -> bool:
        if not self.input_source_client.wait_for_service(timeout_sec=8.0):
            self.get_logger().error('❌ SetMcInputSource service unavailable.')
            return False

        # Try ADD first; if already registered, fall back to ENABLE
        if self._call_input_source(McInputAction.INPUTACTION_ADD):  # ADD (1001)
            self.get_logger().info(
                f'✅ Input source "{INPUT_SOURCE_NAME}" registered (ADD).')
            return True
        self.get_logger().info(
            'ADD failed (likely already registered), trying ENABLE...')
        if self._call_input_source(McInputAction.INPUTACTION_ENABLE):  # ENABLE (2001)
            self.get_logger().info(
                f'✅ Input source "{INPUT_SOURCE_NAME}" enabled.')
            return True
        self.get_logger().error('❌ Failed to register input source via ADD or ENABLE.')
        return False

    def send_request(self, area_id: int, motion_id: int) -> bool:
        request = SetMcPresetMotion.Request()
        request.header = RequestHeader()

        request.input_source.name = INPUT_SOURCE_NAME
        request.input_source.priority = INPUT_SOURCE_PRIORITY
        request.input_source.timeout = INPUT_SOURCE_TIMEOUT

        motion = McPresetMotion()
        area = McControlArea()

        motion.value = motion_id
        area.value = area_id

        request.motion = motion
        request.area = area
        request.interrupt = False

        self.get_logger().info(
            f'📨 Sending request to set preset motion: motion={motion_id}, area={area_id}')

        for i in range(8):
            request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(request)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        if response.response.header.code == 0:
            self.get_logger().info(
                f'✅ Preset motion set successfully: {response.response.task_id}')
            return True
        # RUNNING (400)
        elif response.response.state.value == CommonState.RUNNING:
            self.get_logger().info(
                f'⏳ Preset motion executing: {response.response.task_id}')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed to set preset motion: {response.response.task_id}'
            )
            return False


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        area = int(input("Enter arm area ID (1-left, 2-right): "))
        motion = int(input(
            "Enter preset motion ID (1001-raise, 1002-wave, 1003-handshake, 1004-airkiss): "))

        node = SetMcPresetMotionClient()
        if not node.register_input_source():
            node.get_logger().error("❌ Input source registration failed, exiting")
        else:
            node.send_request(area, motion)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.5 夹爪控制

该示例中用到了hand_control，通过往话题/aima/hal/joint/hand/command发布消息来控制夹爪的运动

注意

注意⚠️ :运行本示例前需要在机器人运控计算单元（PC1）使用aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全

import rclpy
from rclpy.node import Node
from aimdk_msgs.msg import HandCommandArray, HandCommand, HandType, MessageHeader


class HandControl(Node):
    def __init__(self):
        super().__init__('hand_control')

        # Preset parameter list: [(left hand, right hand), ...]
        self.position_pairs = [
            (1.0, 1.0),   # fully open
            (0.0, 0.0),   # fully closed
            (0.5, 0.5),   # half open
            (0.2, 0.8),   # left slightly closed, right more open
            (0.7, 0.3)    # left more open, right slightly closed
        ]
        self.current_index = 0
        self.last_switch_time = self.get_clock().now().nanoseconds / 1e9  # seconds

        # Create publisher
        self.publisher_ = self.create_publisher(
            HandCommandArray,
            '/aima/hal/joint/hand/command',
            10
        )

        # 50 Hz timer
        self.timer_ = self.create_timer(
            0.02,  # 20 ms = 50 Hz
            self.publish_hand_commands
        )

        self.get_logger().info("Hand control node started!")

    def publish_hand_commands(self):
        # Check time to decide whether to switch to the next preset
        now_sec = self.get_clock().now().nanoseconds / 1e9
        if now_sec - self.last_switch_time >= 2.0:
            self.current_index = (self.current_index +
                                  1) % len(self.position_pairs)
            self.last_switch_time = now_sec
            self.get_logger().info(
                f"Switched to preset: {self.current_index}, left={self.position_pairs[self.current_index][0]:.2f}, right={self.position_pairs[self.current_index][1]:.2f}"
            )

        # Use current preset
        left_position, right_position = self.position_pairs[self.current_index]

        msg = HandCommandArray()
        msg.header = MessageHeader()

        # Configure left hand
        left_hand = HandCommand()
        left_hand.name = "left_hand"
        left_hand.position = float(left_position)
        left_hand.velocity = 1.0
        left_hand.acceleration = 1.0
        left_hand.deceleration = 1.0
        left_hand.effort = 1.0

        # Configure right hand
        right_hand = HandCommand()
        right_hand.name = "right_hand"
        right_hand.position = float(right_position)
        right_hand.velocity = 1.0
        right_hand.acceleration = 1.0
        right_hand.deceleration = 1.0
        right_hand.effort = 1.0

        msg.left_hand_type = HandType(value=HandType.CLAW)   # grippers (2)
        msg.right_hand_type = HandType(value=HandType.CLAW)  # grippers (2)
        msg.left_hands = [left_hand]
        msg.right_hands = [right_hand]

        # Publish message
        self.publisher_.publish(msg)
        # We only log when switching presets to avoid too much log output


def main(args=None):
    rclpy.init(args=args)
    hand_control_node = HandControl()

    try:
        rclpy.spin(hand_control_node)
    except KeyboardInterrupt:
        pass
    finally:
        hand_control_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.6 灵巧手控制

该示例中用到了omnihand_control，通过往话题/aima/hal/joint/hand/command发布消息来控制灵巧手的运动

注意

注意⚠️ :运行本示例前需要在机器人运控计算单元（PC1）使用aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全

import rclpy
from rclpy.node import Node
from aimdk_msgs.msg import HandCommandArray, HandCommand, HandType


class HandControl(Node):
    def __init__(self):
        super().__init__('hand_control')

        # Create publisher
        self.publisher_ = self.create_publisher(
            HandCommandArray,
            '/aima/hal/joint/hand/command',
            10
        )

        self.timer_ = self.create_timer(
            1.0,
            self.publish_hand_commands
        )

        # Initialize variables
        self.target_finger = 0
        self.step = 1
        self.increasing = True
        self.get_logger().info("Hand control node started!")

    def build_hand_cmd(self, name: str) -> HandCommand:
        cmd = HandCommand()
        cmd.name = name
        cmd.position = 0.0
        cmd.velocity = 0.1
        cmd.acceleration = 0.0
        cmd.deceleration = 0.0
        cmd.effort = 0.0
        return cmd

    def publish_hand_commands(self):
        msg = HandCommandArray()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'hand_command'
        # dexterous hands (1)
        msg.left_hand_type.value = HandType.NIMBLE_HANDS
        # dexterous hands (1)
        msg.right_hand_type.value = HandType.NIMBLE_HANDS

        left_names = ['L_thumb_roll_joint', 'L_thumb_abad_joint', 'L_thumb_mcp_joint',
                      'L_index_abad_joint', 'L_index_pip_joint', 'L_middle_pip_joint',
                      'L_ring_abad_joint', 'L_ring_pip_joint', 'L_pinky_abad_joint', 'L_pinky_pip_joint']
        right_names = [n.replace('L_', 'R_', 1) for n in left_names]

        # left hand
        msg.left_hands = [self.build_hand_cmd(
            left_names[i]) for i in range(10)]

        # right hand
        msg.right_hands = [self.build_hand_cmd(
            right_names[i]) for i in range(10)]

        # Absolute joint targets in rad = half-grip
        # Joint order: [thumb_roll, thumb_abad, thumb_mcp, index_abad, index_pip,
        #               middle_pip, ring_abad, ring_pip, pinky_abad, pinky_pip]
        left_hand_target = [-0.6, 0.8, -0.4,
                            0.1, 0.7, 0.7, -0.1, 0.7, -0.1, 0.7]
        right_hand_target = [0.6, -0.8, 0.4,
                             -0.1, 0.7, 0.7, 0.1, 0.7, 0.1, 0.7]

        if self.target_finger < 10:
            msg.right_hands[self.target_finger].position = right_hand_target[self.target_finger]
        else:
            target_finger_ = self.target_finger - 10
            msg.left_hands[target_finger_].position = left_hand_target[target_finger_]

        self.publisher_.publish(msg)
        self.get_logger().info(
            f'Published hand command with target_finger: {self.target_finger}')
        self.update_target_finger()

    def update_target_finger(self):
        if self.increasing:
            self.target_finger += self.step
            if self.target_finger >= 19:
                self.target_finger = 19
                self.increasing = False
        else:
            self.target_finger -= self.step
            if self.target_finger <= 0:
                self.target_finger = 0
                self.increasing = True


def main(args=None):
    rclpy.init(args=args)
    hand_control_node = HandControl()

    try:
        rclpy.spin(hand_control_node)
    except KeyboardInterrupt:
        pass
    finally:
        hand_control_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.7 灵巧手触觉数据（T2.1支持）

该示例中用到了 omnihand_touch，通过订阅话题/aima/hal/joint/hand/state来接收灵巧手指尖压力传感器的数据

注意

注意⚠️ :运行本示例前需要在机器人运控计算单元（PC1）使用aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from aimdk_msgs.msg import HandStateArray, HandCommandArray, HandType


class HandStateSubscriber(Node):
    def __init__(self):
        super().__init__('hand_state_subscriber')

        # Create publisher
        self.publisher_ = self.create_publisher(
            HandCommandArray,
            '/aima/hal/joint/hand/command',
            10
        )

        # Create QoS profile with BEST_EFFORT reliability
        qos_profile = QoSProfile(depth=10)
        qos_profile.reliability = ReliabilityPolicy.BEST_EFFORT

        # Create subscriber
        self.subscription = self.create_subscription(
            HandStateArray,
            '/aima/hal/joint/hand/state',
            self.topic_callback,
            qos_profile
        )

        # Create a timer to publish once per second
        self.timer = self.create_timer(1.0, self.publish_command)

        self.get_logger().info(
            "Subscriber started, listening to /aima/hal/joint/hand/state topic..."
        )

    def topic_callback(self, msg):
        """Callback function for handling incoming messages"""
        # Print message header information
        self.get_logger().info(
            f"Message received - Sequence: {msg.header.sequence}, "
            f"Timestamp: {msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"
        )

        # Print left hand touch sensor data
        self.print_touch_sensor_data("Left Hand", msg.left_touch_sensors)

        # Print right hand touch sensor data
        self.print_touch_sensor_data("Right Hand", msg.right_touch_sensors)

        print()

    def print_touch_sensor_data(self, hand_name, sensor_data):
        """
        Print touch sensor data for a specific hand

        Args:
            hand_name: Name of the hand (Left Hand/Right Hand)
            sensor_data: Touch sensor data structure
        """
        print(f"=== {hand_name} Touch Sensor Data ===")

        # Print palm touch data
        print("Palm Touch Data (36 elements): ", end="")
        self.print_array(sensor_data.palm_touch_data)

        # Print back of hand touch data
        print("Back of Hand Touch Data (36 elements): ", end="")
        self.print_array(sensor_data.back_of_hand_touch_data)

        # Print finger touch data
        print("Thumb Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.thumb_touch_data)

        print("Index Finger Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.index_finger_touch_data)

        print("Middle Finger Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.middle_finger_touch_data)

        print("Ring Finger Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.ring_finger_touch_data)

        print("Little Finger Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.little_finger_touch_data)

    def print_array(self, arr):
        """
        Print array of uint8_t elements

        Args:
            arr: Array to print (list or tuple)
        """
        print("[", end="")
        for i, val in enumerate(arr):
            print(f"{val:3d}", end="")
            if i < len(arr) - 1:
                print(" ", end="")
        print("]")

    def publish_command(self):
        """Publish hand command message"""
        message = HandCommandArray()

        # Set header
        message.header.stamp = self.get_clock().now().to_msg()
        message.header.frame_id = "hand_command"

        # Set the hand type
        # dexterous hands (1)
        message.left_hand_type.value = HandType.NIMBLE_HANDS
        # dexterous hands (1)
        message.right_hand_type.value = HandType.NIMBLE_HANDS

        # Create hand command arrays
        from aimdk_msgs.msg import HandCommand

        # Joint names for readability, mirroring the /aima/hal/joint/hand/state feedback (URDF names).
        # The driver routes by array index/order (see endeffector.md), not by these name strings.
        left_names = ["L_thumb_roll_joint", "L_thumb_abad_joint", "L_thumb_mcp_joint",
                      "L_index_abad_joint", "L_index_pip_joint", "L_middle_pip_joint",
                      "L_ring_abad_joint", "L_ring_pip_joint", "L_pinky_abad_joint", "L_pinky_pip_joint"]
        right_names = [n.replace("L_", "R_", 1) for n in left_names]

        def build_cmd(name):
            cmd = HandCommand()
            cmd.name = name
            cmd.position = 0.0
            cmd.velocity = 0.1
            cmd.acceleration = 0.0
            cmd.deceleration = 0.0
            cmd.effort = 0.0
            return cmd

        message.left_hands = [build_cmd(n) for n in left_names]
        message.right_hands = [build_cmd(n) for n in right_names]

        # Publish the message
        self.publisher_.publish(message)

        self.get_logger().info("Published hand command")


def main(args=None):
    rclpy.init(args=args)
    node = HandStateSubscriber()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

压力传感器位于五个手指的指尖、手心、手背。需要给灵巧手发送至少一个运动指令，才能获得对应的压力数据上报。

6.1.8 注册二开输入源

对于v0.7之后的版本，在实现对MC的控制前，需要先注册输入源。该示例中通过/aimdk_5Fmsgs/srv/SetMcInputSource服务来注册二开的输入源，让MC感知到，只有注册过输入源后才能实现机器人的速度控制

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetMcInputSource
from aimdk_msgs.msg import RequestHeader, McInputAction

ACTION_NAMES = {
    McInputAction.INPUTACTION_ADD: "ADD",
    McInputAction.INPUTACTION_MODIFY: "MODIFY",
    McInputAction.INPUTACTION_DELETE: "DELETE",
    McInputAction.INPUTACTION_ENABLE: "ENABLE",
    McInputAction.INPUTACTION_DISABLE: "DISABLE",
}


class McInputClient(Node):
    def __init__(self):
        super().__init__('set_mc_input_source_client')
        self.client = self.create_client(
            SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource'
        )

        self.get_logger().info('✅ SetMcInputSource client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self):
        req = SetMcInputSource.Request()

        # header
        req.request.header = RequestHeader()

        # action (e.g. ADD = register new input source)
        req.action = McInputAction()
        req.action.value = McInputAction.INPUTACTION_ADD  # ADD (1001)

        # input source info
        req.input_source.name = 'node.set_mc_input_source'
        req.input_source.priority = 40
        req.input_source.timeout = 1000  # ms

        # Send request and wait for response
        act_name = ACTION_NAMES.get(req.action.value, str(req.action.value))
        self.get_logger().info(
            f'📨 Sending input source request: {act_name} ({req.action.value}), '
            f'name={req.input_source.name}, priority={req.input_source.priority}'
        )
        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(
                self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        if not future.done():
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        response = future.result()
        ret_code = response.response.header.code
        task_id = response.response.task_id

        if ret_code == 0:
            self.get_logger().info(
                f'✅ Input source set successfully. task_id={task_id}'
            )
            return True
        else:
            self.get_logger().error(
                f'❌ Input source set failed. ret_code={ret_code}, task_id={task_id} (duplicated ADD? or MODIFY/ENABLE/DISABLE for unknown source?)'
            )
            return False


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

    node = None
    try:
        node = McInputClient()
        ok = node.send_request()
        if not ok:
            node.get_logger().error('Input source request failed.')
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.9 获取当前输入源

该示例中用到了GetCurrentInputSource服务，用于获取当前已注册的输入源信息，包括输入源名称、优先级和超时时间等信息。

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from aimdk_msgs.srv import GetCurrentInputSource
from aimdk_msgs.msg import CommonRequest


class GetCurrentInputSourceClient(Node):
    def __init__(self):
        super().__init__('get_current_input_source_client')
        self.client = self.create_client(
            GetCurrentInputSource,
            '/aimdk_5Fmsgs/srv/GetCurrentInputSource'
        )

        self.get_logger().info('✅ GetCurrentInputSource client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self):
        # Create request
        req = GetCurrentInputSource.Request()
        req.request = CommonRequest()

        # Send request and wait for response
        self.get_logger().info('📨 Sending request to get current input source')
        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        if not future.done():
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        response = future.result()
        ret_code = response.response.header.code
        if ret_code == 0:
            self.get_logger().info(
                '✅ Current input source get successfully:')
            self.get_logger().info(
                f'Name: {response.input_source.name}')
            self.get_logger().info(
                f'Priority: {response.input_source.priority}')
            self.get_logger().info(
                f'Timeout: {response.input_source.timeout}')
            return True
        else:
            self.get_logger().error(
                f'❌ Current input source get failed, return code: {ret_code}')
            return False


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

    node = None
    try:
        node = GetCurrentInputSourceClient()
        success = node.send_request()
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明

# 获取当前输入源信息
ros2 run py_examples get_current_input_source

输出示例

[INFO] [1779174540.534817548] [get_current_input_source_client]: ✅ GetCurrentInputSource client node created.
[INFO] [1779174541.371095227] [get_current_input_source_client]: 🟢 Service available, ready to send request.
[INFO] [1779174541.371216694] [get_current_input_source_client]: 📨 Sending request to get current input source
[INFO] [1779174541.382602338] [get_current_input_source_client]: ✅ Current input source get successfully:
[INFO] [1779174541.382692894] [get_current_input_source_client]: Name: node
[INFO] [1779174541.382713533] [get_current_input_source_client]: Priority: 40
[INFO] [1779174541.382726205] [get_current_input_source_client]: Timeout: 1000

注意事项

• 确保GetCurrentInputSource服务正常运行

• 需要在注册输入源之后才能获取到有效信息

• 状态码为0表示查询成功

6.1.10 机器人走跑控制

该示例中用到了mc_locomotion_velocity，以下示例通过发布/aima/mc/locomotion/velocity话题控制机器人的行走,对于v0.7之后的版本，实现速度控制前需要注册输入源（该示例已实现注册输入源），具体注册步骤可参考代码。

进入稳定站立模式后执行本程序

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
import time
import signal
import sys

from aimdk_msgs.msg import McLocomotionVelocity, McInputAction, MessageHeader
from aimdk_msgs.srv import SetMcInputSource


class DirectVelocityControl(Node):
    def __init__(self):
        super().__init__('direct_velocity_control')

        self.publisher = self.create_publisher(
            McLocomotionVelocity, '/aima/mc/locomotion/velocity', 10)
        self.client = self.create_client(
            SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource')

        self.forward_velocity = 0.0
        self.lateral_velocity = 0.0
        self.angular_velocity = 0.0

        # Maximum speed limits
        self.max_forward_speed = 1.0  # m/s
        self.min_forward_speed = 0.2  # m/s

        self.max_lateral_speed = 1.0  # m/s
        self.min_lateral_speed = 0.2  # m/s

        self.max_angular_speed = 1.0  # rad/s
        self.min_angular_speed = 0.1  # rad/s

        self.timer = None

        self.get_logger().info("Direct velocity control node started!")

    def start_publish(self):
        if not self.timer:
            self.timer = self.create_timer(0.02, self.publish_velocity)

    def _call_input_source(self, action_value):
        action_name = {
            McInputAction.INPUTACTION_ADD: "ADD",
            McInputAction.INPUTACTION_ENABLE: "ENABLE",
        }.get(action_value, str(action_value))

        req = SetMcInputSource.Request()
        req.action.value = action_value
        req.input_source.name = "node.locomotion_velocity"
        req.input_source.priority = 40
        req.input_source.timeout = 1000

        future = None
        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
            if future.done():
                break
            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(
                f"trying input source action {action_name} ({action_value})... [{i}]")

        if future is None or not future.done():
            return False
        try:
            response = future.result()
        except Exception as e:
            self.get_logger().error(f"Service call exception: {str(e)}")
            return False
        return response is not None and response.response.header.code == 0

    def register_input_source(self):
        self.get_logger().info("Registering input source...")

        timeout_sec = 8.0
        start = self.get_clock().now().nanoseconds / 1e9

        while not self.client.wait_for_service(timeout_sec=2.0):
            now = self.get_clock().now().nanoseconds / 1e9
            if now - start > timeout_sec:
                self.get_logger().error("Waiting for service timed out")
                return False
            self.get_logger().info("Waiting for input source service...")

        # Try ADD first; if already registered, fall back to ENABLE
        if self._call_input_source(McInputAction.INPUTACTION_ADD):  # ADD (1001)
            self.get_logger().info("✅ Input source registered (ADD).")
            return True
        self.get_logger().info(
            "ADD failed (likely already registered), trying ENABLE...")
        if self._call_input_source(McInputAction.INPUTACTION_ENABLE):  # ENABLE (2001)
            self.get_logger().info("✅ Input source enabled.")
            return True
        self.get_logger().error(
            "❌ Failed to register input source via ADD or ENABLE.")
        return False

    def publish_velocity(self):
        msg = McLocomotionVelocity()
        msg.header = MessageHeader()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.source = "node.locomotion_velocity"
        msg.forward_velocity = self.forward_velocity
        msg.lateral_velocity = self.lateral_velocity
        msg.angular_velocity = self.angular_velocity

        self.publisher.publish(msg)

        self.get_logger().info(
            f"Publishing velocity: forward {self.forward_velocity:.2f} m/s, "
            f"lateral {self.lateral_velocity:.2f} m/s, "
            f"angular {self.angular_velocity:.2f} rad/s"
        )

    def set_forward(self, forward):
        # check value range, mc has thresholds to start movement
        if abs(forward) < 0.005:
            self.forward_velocity = 0.0
            return True
        elif abs(forward) > self.max_forward_speed or abs(forward) < self.min_forward_speed:
            raise ValueError("out of range")
        else:
            self.forward_velocity = forward
            return True

    def set_lateral(self, lateral):
        # check value range, mc has thresholds to start movement
        if abs(lateral) < 0.005:
            self.lateral_velocity = 0.0
            return True
        elif abs(lateral) > self.max_lateral_speed or abs(lateral) < self.min_lateral_speed:
            raise ValueError("out of range")
        else:
            self.lateral_velocity = lateral
            return True

    def set_angular(self, angular):
        # check value range, mc has thresholds to start movement
        if abs(angular) < 0.005:
            self.angular_velocity = 0.0
            return True
        elif abs(angular) > self.max_angular_speed or abs(angular) < self.min_angular_speed:
            raise ValueError("out of range")
        else:
            self.angular_velocity = angular
            return True

    def clear_velocity(self):
        self.forward_velocity = 0.0
        self.lateral_velocity = 0.0
        self.angular_velocity = 0.0


# Global node instance for signal handling
global_node = None


def signal_handler(sig, frame):
    global global_node
    if global_node is not None:
        global_node.clear_velocity()
        global_node.get_logger().info(
            f"Received signal {sig}, clearing velocity and shutting down")
    if rclpy.ok():
        rclpy.shutdown()
    sys.exit(0)


def main():
    global global_node
    rclpy.init()

    node = DirectVelocityControl()
    global_node = node

    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)

    if not node.register_input_source():
        node.get_logger().error("❌ Input source registration failed, exiting")
        if rclpy.ok():
            rclpy.shutdown()
        return

    # get and check control values
    # notice that mc has thresholds to start movement
    try:
        # get input forward
        forward = float(
            input("Please enter forward velocity 0 or ±(0.2 ~ 1.0) m/s: "))
        node.set_forward(forward)
        # get input lateral
        lateral = float(
            input("Please enter lateral velocity 0 or ±(0.2 ~ 1.0) m/s: "))
        node.set_lateral(lateral)
        # get input angular
        angular = float(
            input("Please enter angular velocity 0 or ±(0.1 ~ 1.0) rad/s: "))
        node.set_angular(angular)
    except Exception as e:
        node.get_logger().error(f"Invalid input: {e}")
        if rclpy.ok():
            rclpy.shutdown()
        return

    node.get_logger().info("Setting velocity, moving for 5 seconds")
    node.start_publish()

    start = node.get_clock().now()
    while (node.get_clock().now() - start).nanoseconds / 1e9 < 5.0:
        rclpy.spin_once(node, timeout_sec=0.1)
        time.sleep(0.001)

    node.clear_velocity()
    node.get_logger().info("5-second motion finished, robot stopped")

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.11 关节电机控制

本示例展示了如何使用ROS2和Ruckig库来控制机器人的关节运动。

注意

注意⚠️ :运行本示例前需要在机器人运控计算单元（PC1）使用aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全

！该示例代码直接对底层电机（即 HAL 层）进行控制，在运行程序前请检查代码中对关节的安全限位是否与所用机器人相符, 并确保安全！

机器人关节控制示例

这个示例展示了如何使用ROS2和Ruckig库来控制机器人的关节运动。示例实现了以下功能：

1. 机器人关节模型定义

2. 基于Ruckig的轨迹插补

3. 多关节协同控制

4. 实时位置、速度和加速度控制

示例功能说明

1. 创建了四个控制器节点，分别控制：

   • 腿部x2（12个关节）

   • 腰部x1（3个关节）

   • 手臂x2（14个关节）

   • 头部x1（2个关节）

2. 演示功能：

   • 每10秒让指定关节在正负0.5弧度之间摆动

   • 使用Ruckig库实现平滑的运动轨迹

   • 实时发布关节控制命令

自定义使用

2. 添加新的控制逻辑：

   • 修改set_target_position函数

   • 添加新的控制回调函数

3. 调整控制频率：

   • 修改timer的周期（当前为3s）和ruckig的dt（当前为0.002s）

#!/usr/bin/env python3
"""
Robot joint control example
This script implements a ROS2-based robot joint control system, using the Ruckig trajectory
planner to achieve smooth joint motion control.

Main features:
1. Supports controlling multiple joint areas (head, arm, waist, leg)
2. Uses Ruckig for trajectory planning to ensure smooth motion
3. Supports real-time control of joint position, velocity, and acceleration
4. Provides joint limit and PID (stiffness/damping) parameter configuration
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from aimdk_msgs.msg import JointCommandArray, JointStateArray, JointCommand
import ruckig
from enum import Enum
from dataclasses import dataclass
from typing import List, Dict
from threading import Lock

# QoS config: define ROS2 Quality of Service parameters
# Subscriber/Publisher QoS: best-effort reliability, keep last 10 messages
# (matches the robot-side joint command/state subscribers)
qos = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
    durability=DurabilityPolicy.VOLATILE
)


class JointArea(Enum):
    HEAD = 'HEAD'    # Head joints
    ARM = 'ARM'      # Arm joints
    WAIST = 'WAIST'  # Waist joints
    LEG = 'LEG'      # Leg joints


@dataclass
class JointInfo:
    # Joint information data class
    name: str           # Joint name
    lower_limit: float  # Joint lower angle limit
    upper_limit: float  # Joint upper angle limit
    kp: float           # Position control proportional gain
    kd: float           # Velocity control derivative gain


# Robot model configuration: define all joint parameters
robot_model: Dict[JointArea, List[JointInfo]] = {
    # Leg joint configuration
    JointArea.LEG: [
        # Left leg joints
        JointInfo("left_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0),
        JointInfo("left_hip_roll_joint", -0.235, 2.906, 40.0, 4.0),
        JointInfo("left_hip_yaw_joint", -1.684, 3.430, 30.0, 3.0),
        JointInfo("left_knee_joint", 0.0000, 2.4073, 80.0, 8.0),
        JointInfo("left_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0),
        JointInfo("left_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0),
        # Right leg joints
        JointInfo("right_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0),
        JointInfo("right_hip_roll_joint", -2.906, 0.235, 40.0, 4.0),
        JointInfo("right_hip_yaw_joint", -3.430, 1.684, 30.0, 3.0),
        JointInfo("right_knee_joint", 0.0000, 2.4073, 80.0, 8.0),
        JointInfo("right_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0),
        JointInfo("right_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0),
    ],
    # Waist joint configuration
    JointArea.WAIST: [
        JointInfo("waist_yaw_joint", -3.43, 2.382, 20.0, 4.0),
        JointInfo("waist_pitch_joint", -0.314, 0.314, 20.0, 4.0),
        JointInfo("waist_roll_joint", -0.488, 0.488, 20.0, 4.0),
    ],
    # Arm joint configuration
    JointArea.ARM: [
        # Left arm
        JointInfo("left_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0),
        JointInfo("left_shoulder_roll_joint", -0.061, 2.993, 20.0, 2.0),
        JointInfo("left_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0),
        JointInfo("left_elbow_joint", -2.3556, 0.0, 20.0, 2.0),
        JointInfo("left_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0),
        JointInfo("left_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0),
        JointInfo("left_wrist_roll_joint", -1.571, 0.724, 20.0, 2.0),
        # Right arm
        JointInfo("right_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0),
        JointInfo("right_shoulder_roll_joint", -2.993, 0.061, 20.0, 2.0),
        JointInfo("right_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0),
        JointInfo("right_elbow_joint", -2.3556, 0.0000, 20.0, 2.0),
        JointInfo("right_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0),
        JointInfo("right_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0),
        JointInfo("right_wrist_roll_joint", -0.724, 1.571, 20.0, 2.0),
    ],
    # Head joint configuration
    JointArea.HEAD: [
        JointInfo("head_yaw_joint", -0.366, 0.366, 20.0, 2.0),
        JointInfo("head_pitch_joint", -0.3838, 0.3838, 20.0, 2.0),
    ],
}


class JointControllerNode(Node):
    """
    Joint controller node
    Responsible for receiving joint states, using Ruckig for trajectory planning,
    and publishing joint commands.
    """

    def __init__(self, node_name: str, sub_topic: str, pub_topic: str, area: JointArea, dofs: int):
        """
        Initialize joint controller
        Args:
            node_name: node name
            sub_topic: topic name to subscribe (joint states)
            pub_topic: topic name to publish (joint commands)
            area: joint area (head/arm/waist/leg)
            dofs: number of DOFs
        """
        super().__init__(node_name)
        self.lock = Lock()
        self.joint_info = robot_model[area]
        self.dofs = dofs
        self.ruckig = ruckig.Ruckig(dofs, 0.002)  # 2 ms control period
        self.input = ruckig.InputParameter(dofs)
        self.output = ruckig.OutputParameter(dofs)
        self.ruckig_initialized = False

        # Initialize trajectory parameters
        self.input.current_position = [0.0] * dofs
        self.input.current_velocity = [0.0] * dofs
        self.input.current_acceleration = [0.0] * dofs

        # Motion limits
        self.input.max_velocity = [1.0] * dofs
        self.input.max_acceleration = [1.0] * dofs
        self.input.max_jerk = [25.0] * dofs

        # ROS2 subscriber and publisher
        self.sub = self.create_subscription(
            JointStateArray,
            sub_topic,
            self.joint_state_callback,
            qos
        )
        self.pub = self.create_publisher(
            JointCommandArray,
            pub_topic,
            qos
        )

    def joint_state_callback(self, msg: JointStateArray):
        """
        Joint state callback
        Receives and processes joint state messages
        """
        self.ruckig_initialized = True

    def control_callback(self, joint_idx):
        """
        Control callback
        Uses Ruckig for trajectory planning and publishes control commands
        Args:
            joint_idx: target joint index
        """
        # Run Ruckig until the target is reached
        while self.ruckig.update(self.input, self.output) in [ruckig.Result.Working, ruckig.Result.Finished]:
            # Update current state
            self.input.current_position = self.output.new_position
            self.input.current_velocity = self.output.new_velocity
            self.input.current_acceleration = self.output.new_acceleration

            # Check if target is reached
            tolerance = 1e-6
            current_p = self.output.new_position[joint_idx]
            if abs(current_p - self.input.target_position[joint_idx]) < tolerance:
                break

            # Create and publish command
            cmd = JointCommandArray()
            for i, joint in enumerate(self.joint_info):
                j = JointCommand()
                j.name = joint.name
                j.position = self.output.new_position[i]
                j.velocity = self.output.new_velocity[i]
                j.effort = 0.0
                j.stiffness = joint.kp
                j.damping = joint.kd
                cmd.joints.append(j)

            self.pub.publish(cmd)

    def set_target_position(self, joint_name, position):
        """
        Set target joint position
        Args:
            joint_name: joint name
            position: target position
        """
        p_s = [0.0] * self.dofs
        joint_idx = 0
        for i, joint in enumerate(self.joint_info):
            if joint.name == joint_name:
                p_s[i] = position
                joint_idx = i
        self.input.target_position = p_s
        self.input.target_velocity = [0.0] * self.dofs
        self.input.target_acceleration = [0.0] * self.dofs
        self.control_callback(joint_idx)


def main(args=None):
    """
    Main function
    Initialize ROS2 node and start joint controller
    """
    rclpy.init(args=args)

    # Create leg controller node
    leg_node = JointControllerNode(
        "leg_node",
        "/aima/hal/joint/leg/state",
        "/aima/hal/joint/leg/command",
        JointArea.LEG,
        12
    )

    # waist_node = JointControllerNode(
    #     "waist_node",
    #     "/aima/hal/joint/waist/state",
    #     "/aima/hal/joint/waist/command",
    #     JointArea.WAIST,
    #     3
    # )

    # arm_node = JointControllerNode(
    #     "arm_node",
    #     "/aima/hal/joint/arm/state",
    #     "/aima/hal/joint/arm/command",
    #     JointArea.ARM,
    #     14
    # )

    # head_node = JointControllerNode(
    #     "head_node",
    #     "/aima/hal/joint/head/state",
    #     "/aima/hal/joint/head/command",
    #     JointArea.HEAD,
    #     2
    # )

    position = 0.8

    # Only control the left leg joint. If you want to control a specific joint, assign it directly.
    def timer_callback():
        """
        Timer callback
        Periodically change target position to achieve oscillating motion
        """
        nonlocal position
        position = -position
        position = 1.3 + position
        leg_node.set_target_position("left_knee_joint", position)

    #     arm_node.set_target_position("left_shoulder_pitch_joint", position)
    #     waist_node.set_target_position("waist_yaw_joint", position)
    #     head_node.set_target_position("head_pitch_joint", position)

    leg_node.create_timer(3.0, timer_callback)

    # Multi-threaded executor
    executor = rclpy.executors.MultiThreadedExecutor()
    executor.add_node(leg_node)

    # executor.add_node(waist_node)
    # executor.add_node(arm_node)
    # executor.add_node(head_node)

    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        leg_node.destroy_node()
        # waist_node.destroy_node()
        # arm_node.destroy_node()
        # head_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.12 键盘控制机器人

本示例实现了通过PC的键盘输入控制机器人的前进后退转弯的功能。

通过WASD控制机器人行走方向，增加/减少速度（±0.2 m/s）,Q/E增加/减少角速度（±0.1 rad/s）,ESC退出程序并释放终端资源，Space立即将速度归零，执行急停。

小心

注意:运行本示例前需要先使用手柄，将机器人切入稳定站立模式。（位控站立/走跑模式时按R2+X, 其他模式详见模式流转图进行操作），然后在机器人的终端界面使用：aima em stop-app rc关闭遥控器，防止通道占用。

实现键盘控制前需要注册输入源（该示例已实现注册输入源）

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node
from aimdk_msgs.msg import McLocomotionVelocity, McInputAction, MessageHeader
from aimdk_msgs.srv import SetMcInputSource
import curses


class KeyboardVelocityController(Node):
    def __init__(self, stdscr):
        super().__init__('keyboard_velocity_controller')
        self.stdscr = stdscr
        self.forward_velocity = 0.0
        self.lateral_velocity = 0.0
        self.angular_velocity = 0.0
        self.step = 0.2
        self.angular_step = 0.1

        self.publisher = self.create_publisher(
            McLocomotionVelocity, '/aima/mc/locomotion/velocity', 10)
        self.client = self.create_client(
            SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource')

        if not self.register_input_source():
            self.get_logger().error("❌ Input source registration failed, exiting")
            raise RuntimeError("Failed to register input source")

        # Configure curses
        curses.cbreak()
        curses.noecho()
        self.stdscr.keypad(True)
        self.stdscr.nodelay(True)

        self.get_logger().info(
            "Control started: W/S forward/backward, A/D strafe, Q/E turn, Space stop, Esc exit")

        # Timer: check keyboard every 50 ms
        self.timer = self.create_timer(0.05, self.check_key_and_publish)

    def _call_input_source(self, action_value):
        action_name = {
            McInputAction.INPUTACTION_ADD: "ADD",
            McInputAction.INPUTACTION_ENABLE: "ENABLE",
        }.get(action_value, str(action_value))

        req = SetMcInputSource.Request()
        req.action.value = action_value
        req.input_source.name = "node.keyboard"
        req.input_source.priority = 40
        req.input_source.timeout = 1000

        future = None
        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
            if future.done():
                break
            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(
                f"trying input source action {action_name} ({action_value})... [{i}]")

        if future is None or not future.done():
            return False
        try:
            resp = future.result()
        except Exception as e:
            self.get_logger().error(f"Service exception: {str(e)}")
            return False
        return resp is not None and resp.response.header.code == 0

    def register_input_source(self):
        self.get_logger().info("Registering input source...")

        timeout_sec = 8.0
        start = self.get_clock().now().nanoseconds / 1e9

        while not self.client.wait_for_service(timeout_sec=2.0):
            now = self.get_clock().now().nanoseconds / 1e9
            if now - start > timeout_sec:
                self.get_logger().error("Waiting for service timed out")
                return False
            self.get_logger().info("Waiting for input source service...")

        # Try ADD first; if already registered, fall back to ENABLE
        if self._call_input_source(McInputAction.INPUTACTION_ADD):  # ADD (1001)
            self.get_logger().info("✅ Input source registered (ADD).")
            return True
        self.get_logger().info(
            "ADD failed (likely already registered), trying ENABLE...")
        if self._call_input_source(McInputAction.INPUTACTION_ENABLE):  # ENABLE (2001)
            self.get_logger().info("✅ Input source enabled.")
            return True
        self.get_logger().error(
            "❌ Failed to register input source via ADD or ENABLE.")
        return False

    def check_key_and_publish(self):
        try:
            ch = self.stdscr.getch()
        except Exception:
            ch = -1

        if ch != -1:
            if ch == ord(' '):
                self.forward_velocity = 0.0
                self.lateral_velocity = 0.0
                self.angular_velocity = 0.0
            elif ch == ord('w'):
                self.forward_velocity = min(
                    self.forward_velocity + self.step, 1.0)
            elif ch == ord('s'):
                self.forward_velocity = max(
                    self.forward_velocity - self.step, -1.0)
            elif ch == ord('a'):
                self.lateral_velocity = min(
                    self.lateral_velocity + self.step, 1.0)
            elif ch == ord('d'):
                self.lateral_velocity = max(
                    self.lateral_velocity - self.step, -1.0)
            elif ch == ord('q'):
                self.angular_velocity = min(
                    self.angular_velocity + self.angular_step, 1.0)
            elif ch == ord('e'):
                self.angular_velocity = max(
                    self.angular_velocity - self.angular_step, -1.0)
            elif ch == 27:  # ESC
                self.get_logger().info("Exiting control")
                self.timer.cancel()
                return

        msg = McLocomotionVelocity()
        msg.header = MessageHeader()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.source = "node.keyboard"
        msg.forward_velocity = self.forward_velocity
        msg.lateral_velocity = self.lateral_velocity
        msg.angular_velocity = self.angular_velocity

        self.publisher.publish(msg)

        # Update UI
        self.stdscr.clear()
        self.stdscr.addstr(
            0, 0, "W/S: Forward/Backward | A/D: Strafe | Q/E: Turn | Space: Stop | ESC: Exit")
        self.stdscr.addstr(2, 0,
                           f"Speed Status: Forward: {self.forward_velocity:.2f} m/s | "
                           f"Lateral: {self.lateral_velocity:.2f} m/s | "
                           f"Angular: {self.angular_velocity:.2f} rad/s")
        self.stdscr.refresh()


def curses_main(stdscr):
    rclpy.init()
    try:
        node = KeyboardVelocityController(stdscr)
        while not node.timer.is_canceled():
            rclpy.spin_once(node, timeout_sec=0.1)
    except (Exception, KeyboardInterrupt) as e:
        rclpy.logging.get_logger("main").fatal(
            f"Program exited with exception: {e}")
    finally:
        curses.endwin()
        if rclpy.ok():
            rclpy.shutdown()


def main():
    curses.wrapper(curses_main)


if __name__ == '__main__':
    main()

6.1.13 拍照

该示例中用到了take_photo，在运行节点程序前，修改需要拍照的相机话题，启动节点程序后，会创建/images/目录，将当前帧图像保存在这个目录下。

#!/usr/bin/env python3
import threading
import time
from pathlib import Path

import rclpy
import rclpy.task
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2


class SaveOneRawPy(Node):
    def __init__(self):
        super().__init__('save_one_image')

        # parameter: image topic
        self.declare_parameter(
            'image_topic', '/aima/hal/sensor/stereo_head_front_left/rgb_image'
        )
        self.topic = self.get_parameter(
            'image_topic').get_parameter_value().string_value

        # save directory
        self.save_dir = Path('images').resolve()
        self.save_dir.mkdir(parents=True, exist_ok=True)

        self._bridge = CvBridge()

        self._future = rclpy.task.Future()
        self._lock = threading.Lock()

        # subscriber (sensor QoS)
        self.sub = self.create_subscription(
            Image,
            self.topic,
            self.image_cb,
            qos_profile_sensor_data
        )
        self.get_logger().info(f'Subscribing to raw image: {self.topic}')
        self.get_logger().info(f'Images will be saved to: {self.save_dir}')

    def image_cb(self, msg: Image):
        with self._lock:
            if self._future.done():
                return

        try:
            enc = msg.encoding.lower()
            self.get_logger().info(f'Received image with encoding: {enc}')

            img = self._bridge.imgmsg_to_cv2(
                msg, desired_encoding='passthrough')

            if enc == 'rgb8':
                img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
            elif enc == 'mono8':
                img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)

            ts_ms = int(time.time() * 1000)
            out_path = self.save_dir / f'frame_{ts_ms}.png'

            ok = cv2.imwrite(str(out_path), img)
            if ok:
                self.get_logger().info(
                    f'Saved image: {out_path}  ({img.shape[1]}x{img.shape[0]})'
                )
                with self._lock:
                    if not self._future.done():
                        self._future.set_result(str(out_path))
            else:
                self.get_logger().error(f'cv2.imwrite failed: {out_path}')
        except Exception as e:
            self.get_logger().error(f'Failed to decode / save image: {e}')
            with self._lock:
                if not self._future.done():
                    self._future.set_exception(e)


def main():
    rclpy.init()
    node = SaveOneRawPy()

    try:
        rclpy.spin_until_future_complete(node, node._future)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.14 相机推流示例集

该示例集提供了多种相机数据订阅和处理功能，支持深度相机、双目相机和单目相机的数据流订阅。
这些相机数据订阅 example 并没有实际的业务用途， 仅提供相机数据基础信息的打印；若您比较熟悉 ros2 使用，会发现 ros2 topic echo + ros2 topic hz 也能够实现 example 提供的功能。 您可以选择快速查阅 SDK 接口手册中话题列表直接快速进入自己模块的开发，也可以使用相机 example 作为脚手架加入自己的业务逻辑。 我们发布的传感器数据均为未经预处理(去畸变等)的原始数据，如果您需要查询传感器的详细信息（如分辨率、焦距等），请关注相应的内参（camera_info）话题。

深度相机数据订阅

该示例中用到了echo_camera_rgbd，通过订阅/aima/hal/sensor/rgbd_head_front/话题来接收机器人的深度相机数据，支持深度图、RGB图、压缩RGB图和相机内参等多种数据类型。

功能特点：

• 支持多种数据类型订阅（深度图、RGB图、压缩图、相机内参）

• 实时FPS统计和数据显示

• 支持RGB图视频录制功能

• 可配置的topic类型选择

支持的数据类型：

• depth_image: 深度图像 (sensor_msgs/Image)

• rgb_image: RGB图像 (sensor_msgs/Image)

• rgb_image_compressed: 压缩RGB图像 (sensor_msgs/CompressedImage)

• camera_info: 相机内参 (sensor_msgs/CameraInfo)

#!/usr/bin/env python3
"""
Head depth camera multi-topic subscription example

Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
  - depth_image: Depth image (sensor_msgs/Image)
  - depth_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
  - rgb_image: RGB image (sensor_msgs/Image)
  - rgb_image_compressed: RGB compressed image (sensor_msgs/CompressedImage)
  - rgb_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)

Example:
  ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image
  ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_camera_info

Default topic_type is rgb_image
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import Image, CompressedImage, CameraInfo
from collections import deque
import cv2
import numpy as np


class CameraTopicEcho(Node):
    def __init__(self):
        super().__init__('camera_topic_echo')

        # Select the topic type to subscribe
        self.declare_parameter('topic_type', 'rgb_image')
        self.declare_parameter('dump_video_path', '')
        self.declare_parameter('dump_video_fps', 30.0)

        self.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').value
        self.dump_video_fps = self.get_parameter('dump_video_fps').value
        self.video_writer = None
        self.frame_count = 0

        # SensorDataQoS: BEST_EFFORT + VOLATILE
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=5
        )

        # Create different subscribers based on topic_type
        if self.topic_type == "depth_image":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/depth_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing Depth Image: {self.topic_name}")

        elif self.topic_type == "rgb_image":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing RGB Image: {self.topic_name}")
            if self.dump_video_path:
                self.get_logger().info(
                    f"📝 Will dump received images to video: {self.dump_video_path}")

        elif self.topic_type == "rgb_image_compressed":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_image/compressed"
            self.sub_compressed = self.create_subscription(
                CompressedImage, self.topic_name, self.cb_compressed, qos)
            self.get_logger().info(
                f"✅ Subscribing CompressedImage: {self.topic_name}")

        elif self.topic_type == "rgb_camera_info":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_camera_info"
            # RGB-D CameraInfo subscriptions are different from other cameras. The best_effort + volatile QoS is sufficient for 30Hz rgb_camera_info
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, qos)
            self.get_logger().info(
                f"✅ Subscribing RGB CameraInfo: {self.topic_name}")

        elif self.topic_type == "depth_camera_info":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/depth_camera_info"
            # RGB-D CameraInfo subscriptions are different from other cameras. The best_effort + volatile QoS is sufficient for 30Hz depth_camera_info
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, qos)
            self.get_logger().info(
                f"✅ Subscribing Depth CameraInfo: {self.topic_name}")

        else:
            self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
            raise ValueError("Unknown topic_type")

        # Internal state
        self.last_print = self.get_clock().now()
        self.print_allowed = False
        self.arrivals = deque()

    def update_arrivals(self):
        """Calculate received FPS"""
        now = self.get_clock().now()
        self.arrivals.append(now)
        while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
            self.arrivals.popleft()

    def get_fps(self):
        """Get FPS"""
        return len(self.arrivals)

    def should_print(self, master=True):
        """Control print frequency"""
        if not master:
            return self.print_allowed
        now = self.get_clock().now()
        if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
            self.last_print = now
            self.print_allowed = True
        else:
            self.print_allowed = False
        return self.print_allowed

    def cb_image(self, msg: Image):
        """Image callback (Depth/RGB image)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"📸 {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • encoding:        {msg.encoding}\n"
                f"  • size (WxH):      {msg.width} x {msg.height}\n"
                f"  • step (bytes/row):{msg.step}\n"
                f"  • is_bigendian:    {msg.is_bigendian}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

        # Only RGB image supports video dump
        if self.topic_type == "rgb_image" and self.dump_video_path:
            self.dump_image_to_video(msg)

    def cb_compressed(self, msg: CompressedImage):
        """CompressedImage callback"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"🗜️  CompressedImage received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • format:          {msg.format}\n"
                f"  • data size:       {len(msg.data)}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

    def cb_camerainfo(self, msg: CameraInfo):
        """CameraInfo callback (camera intrinsic parameters)"""
        # Camera info will only receive one frame, print it directly
        stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

        # Format D array
        d_str = ", ".join([f"{d:.6f}" for d in msg.d])

        # Format K matrix
        k_str = ", ".join([f"{k:.6f}" for k in msg.k])

        # Format P matrix
        p_str = ", ".join([f"{p:.6f}" for p in msg.p])

        self.get_logger().info(
            f"📷 {self.topic_type} received\n"
            f"  • frame_id:        {msg.header.frame_id}\n"
            f"  • stamp (sec):     {stamp_sec:.6f}\n"
            f"  • width x height:  {msg.width} x {msg.height}\n"
            f"  • distortion_model:{msg.distortion_model}\n"
            f"  • D: [{d_str}]\n"
            f"  • K: [{k_str}]\n"
            f"  • P: [{p_str}]\n"
            f"  • binning_x: {msg.binning_x}\n"
            f"  • binning_y: {msg.binning_y}\n"
            f"  • roi: {{ x_offset: {msg.roi.x_offset}, y_offset: {msg.roi.y_offset}, height: {msg.roi.height}, width: {msg.roi.width}, do_rectify: {msg.roi.do_rectify} }}"
        )

    def dump_image_to_video(self, msg: Image):
        """Dump RGB image to video file using mp4v encoding"""
        if msg.encoding != "rgb8":
            self.get_logger().warn(
                f"image encoding not expected: {msg.encoding}")
            return

        np_array = np.frombuffer(msg.data, dtype=np.uint8).reshape(
            msg.height, msg.width, 3)
        cv_image = cv2.cvtColor(np_array, cv2.COLOR_RGB2BGR)

        if self.video_writer is None:
            fourcc = cv2.VideoWriter_fourcc(*'mp4v')
            height, width = cv_image.shape[:2]
            self.video_writer = cv2.VideoWriter(
                self.dump_video_path, fourcc, self.dump_video_fps, (width, height))
            if not self.video_writer.isOpened():
                self.get_logger().error(
                    f"Failed to open video writer: {self.dump_video_path}")
                self.dump_video_path = ''
                return
            self.get_logger().info(
                f"📹 Video writer initialized: {width}x{height} @ {self.dump_video_fps}fps")

        self.video_writer.write(cv_image)
        self.frame_count += 1

        if self.frame_count % 30 == 0:
            self.get_logger().info(
                f"📹 Written {self.frame_count} frames to video")

    def destroy_node(self):
        if self.video_writer is not None:
            self.video_writer.release()
            self.get_logger().info(
                f"📹 Video saved: {self.dump_video_path} ({self.frame_count} frames)")
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    try:
        node = CameraTopicEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

1. 订阅RGB图像数据：

   ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image
   

2. 订阅相机内参：

   ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_camera_info
   ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=depth_camera_info
   

3. 录制RGB视频：

   # dump_video_path的值可改为其他路径, 注意提前创建该文件所在目录才能保存
   ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/output.mp4
   

双目相机数据订阅

该示例中用到了echo_camera_stereo，通过订阅/aima/hal/sensor/stereo_head_front_*/话题来接收机器人的双目相机数据，支持左右相机的RGB图、压缩图和相机内参数据。

功能特点：

• 支持左右相机独立数据订阅

• 实时FPS统计和数据显示

• 支持RGB图视频录制功能

• 可配置的相机选择（左/右）

支持的数据类型：

• left_rgb_image: 左相机RGB图像 (sensor_msgs/Image)

• left_rgb_image_compressed: 左相机压缩RGB图像 (sensor_msgs/CompressedImage)

• left_camera_info: 左相机内参 (sensor_msgs/CameraInfo)

• right_rgb_image: 右相机RGB图像 (sensor_msgs/Image)

• right_rgb_image_compressed: 右相机压缩RGB图像 (sensor_msgs/CompressedImage)

• right_camera_info: 右相机内参 (sensor_msgs/CameraInfo)

#!/usr/bin/env python3
"""
Head stereo camera multi-topic subscription example

Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
  - left_rgb_image: Left camera RGB image (sensor_msgs/Image)
  - left_rgb_image_compressed: Left camera RGB compressed image (sensor_msgs/CompressedImage)
  - left_camera_info: Left camera intrinsic parameters (sensor_msgs/CameraInfo)
  - right_rgb_image: Right camera RGB image (sensor_msgs/Image)
  - right_rgb_image_compressed: Right camera RGB compressed image (sensor_msgs/CompressedImage)
  - right_camera_info: Right camera intrinsic parameters (sensor_msgs/CameraInfo)

Example:
  ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image
  ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=right_rgb_image
  ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_camera_info

Default topic_type is left_rgb_image
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import Image, CompressedImage, CameraInfo
from collections import deque
import cv2
import numpy as np


class StereoCameraTopicEcho(Node):
    def __init__(self):
        super().__init__('stereo_camera_topic_echo')

        # Select the topic type to subscribe
        self.declare_parameter('topic_type', 'left_rgb_image')
        self.declare_parameter('dump_video_path', '')
        self.declare_parameter('dump_video_fps', 10.0)

        self.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').value
        self.dump_video_fps = self.get_parameter('dump_video_fps').value
        self.video_writer = None
        self.frame_count = 0

        # Set QoS parameters - use sensor data QoS
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=5,
            durability=QoSDurabilityPolicy.VOLATILE
        )

        # Create different subscribers based on topic_type
        if self.topic_type == "left_rgb_image":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_left/rgb_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing Left RGB Image: {self.topic_name}")
            if self.dump_video_path:
                self.get_logger().info(
                    f"📝 Will dump received images to video: {self.dump_video_path}")

        elif self.topic_type == "left_rgb_image_compressed":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_left/rgb_image/compressed"
            self.sub_compressed = self.create_subscription(
                CompressedImage, self.topic_name, self.cb_compressed, qos)
            self.get_logger().info(
                f"✅ Subscribing Left CompressedImage: {self.topic_name}")

        elif self.topic_type == "left_camera_info":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_left/camera_info"
            # CameraInfo subscription must use reliable + transient_local QoS to receive historical messages (even if only one frame is published)
            camera_qos = QoSProfile(
                reliability=QoSReliabilityPolicy.RELIABLE,
                history=QoSHistoryPolicy.KEEP_LAST,
                depth=1,
                durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
            )
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
            self.get_logger().info(
                f"✅ Subscribing Left CameraInfo (with transient_local): {self.topic_name}")

        elif self.topic_type == "right_rgb_image":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_right/rgb_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing Right RGB Image: {self.topic_name}")
            if self.dump_video_path:
                self.get_logger().info(
                    f"📝 Will dump received images to video: {self.dump_video_path}")

        elif self.topic_type == "right_rgb_image_compressed":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_right/rgb_image/compressed"
            self.sub_compressed = self.create_subscription(
                CompressedImage, self.topic_name, self.cb_compressed, qos)
            self.get_logger().info(
                f"✅ Subscribing Right CompressedImage: {self.topic_name}")

        elif self.topic_type == "right_camera_info":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_right/camera_info"
            # CameraInfo subscription must use reliable + transient_local QoS to receive historical messages (even if only one frame is published)
            camera_qos = QoSProfile(
                reliability=QoSReliabilityPolicy.RELIABLE,
                history=QoSHistoryPolicy.KEEP_LAST,
                depth=1,
                durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
            )
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
            self.get_logger().info(
                f"✅ Subscribing Right CameraInfo (with transient_local): {self.topic_name}")

        else:
            self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
            raise ValueError("Unknown topic_type")

        # Internal state
        self.last_print = self.get_clock().now()
        self.print_allowed = False
        self.arrivals = deque()

    def update_arrivals(self):
        """Calculate received FPS"""
        now = self.get_clock().now()
        self.arrivals.append(now)
        while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
            self.arrivals.popleft()

    def get_fps(self):
        """Get FPS"""
        return len(self.arrivals)

    def should_print(self, master=True):
        """Control print frequency"""
        if not master:
            return self.print_allowed
        now = self.get_clock().now()
        if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
            self.last_print = now
            self.print_allowed = True
        else:
            self.print_allowed = False
        return self.print_allowed

    def cb_image(self, msg: Image):
        """Image callback (Left/Right camera RGB image)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"📸 {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • encoding:        {msg.encoding}\n"
                f"  • size (WxH):      {msg.width} x {msg.height}\n"
                f"  • step (bytes/row):{msg.step}\n"
                f"  • is_bigendian:    {msg.is_bigendian}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

        # Only RGB images support video dump
        if (self.topic_type in ["left_rgb_image", "right_rgb_image"]) and self.dump_video_path:
            self.dump_image_to_video(msg)

    def cb_compressed(self, msg: CompressedImage):
        """CompressedImage callback (Left/Right camera RGB compressed image)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"🗜️  {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • format:          {msg.format}\n"
                f"  • data size:       {len(msg.data)}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

    def cb_camerainfo(self, msg: CameraInfo):
        """CameraInfo callback (Left/Right camera intrinsic parameters)"""
        # Camera info will only receive one frame, print it directly
        stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

        # Format D array
        d_str = ", ".join([f"{d:.6f}" for d in msg.d])

        # Format K matrix
        k_str = ", ".join([f"{k:.6f}" for k in msg.k])

        # Format P matrix
        p_str = ", ".join([f"{p:.6f}" for p in msg.p])

        self.get_logger().info(
            f"📷 {self.topic_type} received\n"
            f"  • frame_id:        {msg.header.frame_id}\n"
            f"  • stamp (sec):     {stamp_sec:.6f}\n"
            f"  • width x height:  {msg.width} x {msg.height}\n"
            f"  • distortion_model:{msg.distortion_model}\n"
            f"  • D: [{d_str}]\n"
            f"  • K: [{k_str}]\n"
            f"  • P: [{p_str}]\n"
            f"  • binning_x: {msg.binning_x}\n"
            f"  • binning_y: {msg.binning_y}\n"
            f"  • roi: {{ x_offset: {msg.roi.x_offset}, y_offset: {msg.roi.y_offset}, height: {msg.roi.height}, width: {msg.roi.width}, do_rectify: {msg.roi.do_rectify} }}"
        )

    def dump_image_to_video(self, msg: Image):
        """Dump RGB image to video file using mp4v encoding"""
        if msg.encoding != "rgb8":
            self.get_logger().warn(
                f"image encoding not expected: {msg.encoding}")
            return

        np_array = np.frombuffer(msg.data, dtype=np.uint8).reshape(
            msg.height, msg.width, 3)
        cv_image = cv2.cvtColor(np_array, cv2.COLOR_RGB2BGR)

        if self.video_writer is None:
            fourcc = cv2.VideoWriter_fourcc(*'mp4v')
            height, width = cv_image.shape[:2]
            self.video_writer = cv2.VideoWriter(
                self.dump_video_path, fourcc, self.dump_video_fps, (width, height))
            if not self.video_writer.isOpened():
                self.get_logger().error(
                    f"Failed to open video writer: {self.dump_video_path}")
                self.dump_video_path = ''
                return
            self.get_logger().info(
                f"📹 Video writer initialized: {width}x{height} @ {self.dump_video_fps}fps")

        self.video_writer.write(cv_image)
        self.frame_count += 1

        if self.frame_count % 30 == 0:
            self.get_logger().info(
                f"📹 Written {self.frame_count} frames to video")

    def destroy_node(self):
        if self.video_writer is not None:
            self.video_writer.release()
            self.get_logger().info(
                f"📹 Video saved: {self.dump_video_path} ({self.frame_count} frames)")
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    try:
        node = StereoCameraTopicEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

1. 订阅左相机RGB图像：

   ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image
   

2. 订阅右相机RGB图像：

   ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=right_rgb_image
   

3. 订阅左相机内参：

   ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_camera_info
   

4. 录制左相机视频：

   # dump_video_path的值可改为其他路径, 注意提前创建该文件所在目录才能保存
   ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image -p dump_video_path:=$PWD/left_camera.mp4
   

头部后置单目相机数据订阅

该示例中用到了echo_camera_head_rear，通过订阅/aima/hal/sensor/rgb_head_rear/话题来接收机器人的头部后置单目相机数据，支持RGB图(及其mask处理)、压缩图和相机内参数据。

功能特点：

• 支持头部后置相机数据订阅

• 实时FPS统计和数据显示

• 支持RGB图视频录制功能及把手遮挡区域mask处理

• 可配置的topic类型选择

• 基于软件标记识别相机安装方向，对图像旋转补偿

支持的数据类型：

• rgb_image: RGB图像 (sensor_msgs/Image)

• rgb_image_compressed: 压缩RGB图像 (sensor_msgs/CompressedImage)

• camera_info: 相机内参 (sensor_msgs/CameraInfo)

#!/usr/bin/env python3
"""
Head rear monocular camera multi-topic subscription example

Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
  - rgb_image: RGB image (sensor_msgs/Image)
  - rgb_image_compressed: RGB compressed image (sensor_msgs/CompressedImage)
  - camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)

Example:
  ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
  ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image_compressed
  ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=camera_info

Default topic_type is rgb_image

Note: Some rear cameras are rotated by 90 degrees. This example automatically
detects them and applies rotation compensation when needed.
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import Image, CompressedImage, CameraInfo
from collections import deque
import os
import cv2
import numpy as np
import re

REAR_ROTATION_PATTERN = re.compile(r't3[d.]5', re.IGNORECASE)


class HeadRearCameraTopicEcho(Node):
    def __init__(self):
        super().__init__('head_rear_camera_topic_echo')

        self.declare_parameter('topic_type', 'rgb_image')
        self.declare_parameter('dump_video_path', '')
        self.declare_parameter('dump_video_fps', 10.0)
        self.declare_parameter('with_mask', False)
        self.declare_parameter('auto_rotate', True)

        self.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').value
        self.dump_video_fps = self.get_parameter('dump_video_fps').value
        self.with_mask = self.get_parameter('with_mask').value
        self.auto_rotate = self.get_parameter('auto_rotate').value
        self.mask_image = None
        self.need_rotation = False
        self.hw_version = None
        self.video_writer = None
        self.frame_count = 0

        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=5,
            durability=QoSDurabilityPolicy.VOLATILE
        )

        if self.auto_rotate:
            self._detect_rotate_from_file()

        if self.with_mask and self.dump_video_path:
            mask_file = 'rgb_head_rear_mask_t35.png' if self.need_rotation else 'rgb_head_rear_mask.png'
            mask_path = os.path.join(
                os.path.dirname(__file__), 'data', mask_file)
            self.mask_image = cv2.imread(mask_path, cv2.IMREAD_GRAYSCALE)
            if self.mask_image is None:
                self.get_logger().error(
                    f"Failed to load mask file: {mask_path}")
                raise ValueError("Failed to load mask file")
            self.get_logger().info(f"🎭 Loaded mask: {mask_file}")

        if self.topic_type == "rgb_image":
            self.topic_name = "/aima/hal/sensor/rgb_head_rear/rgb_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing RGB Image: {self.topic_name}")
            if self.dump_video_path:
                mask_state = "with mask" if self.with_mask else "without mask"
                rotation_state = "with rotation compensation" if self.need_rotation else "without rotation"
                self.get_logger().info(
                    f"📝 Will dump received images {mask_state} {rotation_state} to video: {self.dump_video_path}")

        elif self.topic_type == "rgb_image_compressed":
            self.topic_name = "/aima/hal/sensor/rgb_head_rear/rgb_image/compressed"
            self.sub_compressed = self.create_subscription(
                CompressedImage, self.topic_name, self.cb_compressed, qos)
            self.get_logger().info(
                f"✅ Subscribing CompressedImage: {self.topic_name}")

        elif self.topic_type == "camera_info":
            self.topic_name = "/aima/hal/sensor/rgb_head_rear/camera_info"
            camera_qos = QoSProfile(
                reliability=QoSReliabilityPolicy.RELIABLE,
                history=QoSHistoryPolicy.KEEP_LAST,
                depth=1,
                durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
            )
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
            self.get_logger().info(
                f"✅ Subscribing CameraInfo (with transient_local): {self.topic_name}")

        else:
            self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
            raise ValueError("Unknown topic_type")

        self.last_print = self.get_clock().now()
        self.print_allowed = False
        self.arrivals = deque()

    def _detect_rotate_from_file(self):
        try:
            hw_file = '/agibot/data/info/hardware_version'
            if os.path.exists(hw_file):
                with open(hw_file, 'r') as f:
                    self.hw_version = f.read().strip()
                self.get_logger().info(
                    f"📋 Hardware version from file: '{self.hw_version}'")
                self._check_rotation(self.hw_version)
            else:
                self.get_logger().warn(
                    f"⚠️  Hardware version file not found: {hw_file}")
        except Exception as e:
            self.get_logger().warn(
                f"⚠️  Failed to read hardware version from file: {e}")

    def _check_rotation(self, version: str):
        if REAR_ROTATION_PATTERN.search(version):
            self.need_rotation = True
            self.get_logger().info(
                "🔄 Rotation detected: Rear camera rotation compensation enabled (90° counter-clockwise)"
            )
        else:
            self.get_logger().info(
                f"✅ No rotation detected ({version}): No rotation compensation needed"
            )

    def _rotate_image_90_ccw(self, image: np.ndarray) -> np.ndarray:
        return cv2.rotate(image, cv2.ROTATE_90_COUNTERCLOCKWISE)

    def update_arrivals(self):
        now = self.get_clock().now()
        self.arrivals.append(now)
        while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
            self.arrivals.popleft()

    def get_fps(self):
        return len(self.arrivals)

    def should_print(self, master=True):
        if not master:
            return self.print_allowed
        now = self.get_clock().now()
        if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
            self.last_print = now
            self.print_allowed = True
        else:
            self.print_allowed = False
        return self.print_allowed

    def cb_image(self, msg: Image):
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            rotation_note = " (rotated 90° CCW)" if self.need_rotation else ""
            self.get_logger().info(
                f"📸 {self.topic_type} received{rotation_note}\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • encoding:        {msg.encoding}\n"
                f"  • size (WxH):      {msg.width} x {msg.height}\n"
                f"  • step (bytes/row):{msg.step}\n"
                f"  • is_bigendian:    {msg.is_bigendian}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

        if self.topic_type == "rgb_image" and self.dump_video_path:
            self.dump_image_to_video(msg)

    def cb_compressed(self, msg: CompressedImage):
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"🗜️  {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • format:          {msg.format}\n"
                f"  • data size:       {len(msg.data)}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

    def cb_camerainfo(self, msg: CameraInfo):
        stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

        d_str = ", ".join([f"{d:.6f}" for d in msg.d])
        k_str = ", ".join([f"{k:.6f}" for k in msg.k])
        p_str = ", ".join([f"{p:.6f}" for p in msg.p])

        self.get_logger().info(
            f"📷 {self.topic_type} received\n"
            f"  • frame_id:        {msg.header.frame_id}\n"
            f"  • stamp (sec):     {stamp_sec:.6f}\n"
            f"  • width x height:  {msg.width} x {msg.height}\n"
            f"  • distortion_model:{msg.distortion_model}\n"
            f"  • D: [{d_str}]\n"
            f"  • K: [{k_str}]\n"
            f"  • P: [{p_str}]\n"
            f"  • binning_x: {msg.binning_x}\n"
            f"  • binning_y: {msg.binning_y}\n"
            f"  • roi: {{ x_offset: {msg.roi.x_offset}, y_offset: {msg.roi.y_offset}, height: {msg.roi.height}, width: {msg.roi.width}, do_rectify: {msg.roi.do_rectify} }}"
        )

    def dump_image_to_video(self, msg: Image):
        """Dump RGB image to video file using mp4v encoding"""
        try:
            if msg.encoding != "rgb8":
                self.get_logger().warn(
                    f"image encoding not expected: {msg.encoding}")
                return

            np_array = np.frombuffer(msg.data, dtype=np.uint8).reshape(
                msg.height, msg.width, 3)
            cv_image = cv2.cvtColor(np_array, cv2.COLOR_RGB2BGR)

            if self.need_rotation:
                cv_image = self._rotate_image_90_ccw(cv_image)

            if self.mask_image is not None:
                mask_resized = cv2.resize(
                    self.mask_image, (cv_image.shape[1], cv_image.shape[0]))
                cv_image[mask_resized == 0] = 0

            if self.video_writer is None:
                fourcc = cv2.VideoWriter_fourcc(*'mp4v')
                height, width = cv_image.shape[:2]
                self.video_writer = cv2.VideoWriter(
                    self.dump_video_path, fourcc, self.dump_video_fps, (width, height))
                if not self.video_writer.isOpened():
                    self.get_logger().error(
                        f"Failed to open video writer: {self.dump_video_path}")
                    self.dump_video_path = ''
                    return
                self.get_logger().info(
                    f"📹 Video writer initialized: {width}x{height} @ {self.dump_video_fps}fps")

            self.video_writer.write(cv_image)
            self.frame_count += 1

            if self.frame_count % 30 == 0:
                self.get_logger().info(
                    f"📹 Written {self.frame_count} frames to video")

        except Exception as e:
            self.get_logger().error(f"Failed to process image for video: {e}")

    def destroy_node(self):
        if self.video_writer is not None:
            self.video_writer.release()
            self.get_logger().info(
                f"� Video saved: {self.dump_video_path} ({self.frame_count} frames)")
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        node = HeadRearCameraTopicEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if node is not None:
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

1. 订阅RGB图像数据：

   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
   

2. 订阅压缩图像数据：

   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image_compressed
   

3. 订阅相机内参：

   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=camera_info
   

4. 录制视频：

   # dump_video_path的值可改为其他路径, 注意提前创建该文件所在目录才能保存
   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/rear_camera.mp4
   

5. 录制视频并对把手遮挡区域mask处理：

   # dump_video_path的值可改为其他路径, 注意提前创建该文件所在目录才能保存
   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p with_mask:=true -p dump_video_path:=$PWD/rear_camera.mp4
   

6. 禁用自动旋转补偿（获取原始图像）：

   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p auto_rotate:=false
   

应用场景：

• 人脸识别和追踪

• 目标检测和识别

• 视觉SLAM

• 图像处理和计算机视觉算法开发

• 机器人视觉导航

6.1.15 头部触摸传感器数据订阅

该示例中用到了 echo_head_touch_sensor，通过订阅/aima/hal/sensor/touch_head话题来接收机器人的头部触摸传感器的反馈数据。

功能特点：

• 订阅了头部传感器的反馈数据，当头部被触摸时候，输出会从 IDLE->TOUCH

#!/usr/bin/env python3
"""
Head touch state subscription example
"""

import rclpy
from rclpy.node import Node
from aimdk_msgs.msg import TouchState


class TouchStateSubscriber(Node):
    def __init__(self):
        super().__init__('touch_state_subscriber')

        # touch event types
        self.event_type_map = {
            TouchState.UNKNOWN: "UNKNOWN",
            TouchState.IDLE: "IDLE",
            TouchState.TOUCH: "TOUCH",
        }

        # create subscriber
        self.subscription = self.create_subscription(
            TouchState,
            '/aima/hal/sensor/touch_head',
            self.touch_callback,
            10
        )

        self.get_logger().info(
            'TouchState subscriber started, listening to /aima/hal/sensor/touch_head')

    def touch_callback(self, msg):
        event_str = self.event_type_map.get(
            msg.event_type, "INVALID")

        self.get_logger().info(f'Timestamp: {msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}, '
                               f'Event: {event_str} ({msg.event_type})')


def main(args=None):
    rclpy.init(args=args)
    node = TouchStateSubscriber()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

ros2 run py_examples echo_head_touch_sensor

输出示例：

...
[INFO] [1779174766.897555028] [touch_state_subscriber]: Timestamp: 1779174766.892997567, Event: IDLE (1)
[INFO] [1779174766.905498343] [touch_state_subscriber]: Timestamp: 1779174766.902994523, Event: TOUCH (2)

6.1.16 激光雷达数据订阅

该示例中用到了echo_lidar_data，通过订阅/aima/hal/sensor/lidar_chest_front/话题来接收机器人的激光雷达数据，支持点云数据和IMU数据两种数据类型。

功能特点：

• 支持激光雷达点云数据订阅

• 支持激光雷达IMU数据订阅

• 实时FPS统计和数据显示

• 可配置的topic类型选择

• 详细的数据字段信息输出

支持的数据类型：

• PointCloud2: 激光雷达点云数据 (sensor_msgs/PointCloud2)

• Imu: 激光雷达IMU数据 (sensor_msgs/Imu)

技术实现：

• 使用SensorDataQoS配置（BEST_EFFORT + VOLATILE）

• 支持点云字段信息解析和显示

• 支持IMU四元数、角速度和线性加速度数据

• 提供详细的调试日志输出

应用场景：

• 激光雷达数据采集和分析

• 点云数据处理和可视化

• 机器人导航和定位

• SLAM算法开发

• 环境感知和建图

#!/usr/bin/env python3
"""
Chest LiDAR data subscription example

Supports subscribing to the following topics:
  1. /aima/hal/sensor/lidar_chest_front/lidar_pointcloud
     - Data type: sensor_msgs/PointCloud2
     - frame_id: lidar_chest_front
     - child_frame_id: /
     - Content: LiDAR point cloud data
  2. /aima/hal/sensor/lidar_chest_front/imu
     - Data type: sensor_msgs/Imu
     - frame_id: lidar_imu_chest_front
     - Content: LiDAR IMU data

You can select the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
  - pointcloud: subscribe to LiDAR point cloud
  - imu: subscribe to LiDAR IMU
Default topic_type is pointcloud

Examples:
  ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=pointcloud
  ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=imu
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import PointCloud2, Imu
from collections import deque


class LidarChestEcho(Node):
    def __init__(self):
        super().__init__('lidar_chest_echo')

        # Select the topic type to subscribe
        self.declare_parameter('topic_type', 'pointcloud')
        self.topic_type = self.get_parameter('topic_type').value

        # SensorDataQoS: BEST_EFFORT + VOLATILE
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=5
        )

        # Create different subscribers based on topic_type
        if self.topic_type == "pointcloud":
            self.topic_name = "/aima/hal/sensor/lidar_chest_front/lidar_pointcloud"
            self.sub_pointcloud = self.create_subscription(
                PointCloud2, self.topic_name, self.cb_pointcloud, qos)
            self.get_logger().info(
                f"✅ Subscribing LIDAR PointCloud2: {self.topic_name}")

        elif self.topic_type == "imu":
            self.topic_name = "/aima/hal/sensor/lidar_chest_front/imu"
            self.sub_imu = self.create_subscription(
                Imu, self.topic_name, self.cb_imu, qos)
            self.get_logger().info(
                f"✅ Subscribing LIDAR IMU: {self.topic_name}")

        else:
            self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
            raise ValueError("Unknown topic_type")

        # Internal state
        self.last_print = self.get_clock().now()
        self.arrivals = deque()

    def update_arrivals(self):
        """Calculate received FPS"""
        now = self.get_clock().now()
        self.arrivals.append(now)
        while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
            self.arrivals.popleft()

    def get_fps(self):
        """Get FPS"""
        return len(self.arrivals)

    def should_print(self):
        """Control print frequency"""
        now = self.get_clock().now()
        if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
            self.last_print = now
            return True
        return False

    def cb_pointcloud(self, msg: PointCloud2):
        """PointCloud2 callback (LiDAR point cloud)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = f"{msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"

            # Format fields info
            fields_str = " ".join(
                [f"{f.name}({int(f.datatype)})" for f in msg.fields])

            self.get_logger().info(
                f"🟢 LIDAR PointCloud2 received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec}\n"
                f"  • width x height:  {msg.width} x {msg.height}\n"
                f"  • point_step:      {msg.point_step}\n"
                f"  • row_step:        {msg.row_step}\n"
                f"  • fields:          {fields_str}\n"
                f"  • is_bigendian:    {msg.is_bigendian}\n"
                f"  • is_dense:        {msg.is_dense}\n"
                f"  • data size:       {len(msg.data)}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

    def cb_imu(self, msg: Imu):
        """IMU callback (LiDAR IMU)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = f"{msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"

            self.get_logger().info(
                f"🟢 LIDAR IMU received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec}\n"
                f"  • orientation:     [{msg.orientation.x:.6f}, {msg.orientation.y:.6f}, {msg.orientation.z:.6f}, {msg.orientation.w:.6f}]\n"
                f"  • angular_velocity:[{msg.angular_velocity.x:.6f}, {msg.angular_velocity.y:.6f}, {msg.angular_velocity.z:.6f}]\n"
                f"  • linear_accel:    [{msg.linear_acceleration.x:.6f}, {msg.linear_acceleration.y:.6f}, {msg.linear_acceleration.z:.6f}]\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )


def main(args=None):
    rclpy.init(args=args)
    try:
        node = LidarChestEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

# 订阅激光雷达点云数据
ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=pointcloud

# 订阅激光雷达IMU数据
ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=imu

输出示例：

...
[INFO] [1779175640.420801950] [lidar_chest_echo]: 🟢 LIDAR PointCloud2 received
  • frame_id:        lidar_chest_front
  • stamp (sec):     1779175640.315327000
  • width x height:  24779 x 1
  • point_step:      32
  • row_step:        792928
  • fields:          x(7) y(7) z(7) intensity(7) ring(4) timestamp(8)
  • is_bigendian:    False
  • is_dense:        True
  • data size:       792928
  • recv FPS (1s):   11.0

6.1.17 IMU数据订阅

该示例中用到了echo_imu_data，通过订阅IMU话题接收机器人的姿态、角速度与线性加速度数据，并打印各字段的协方差对角线信息。默认订阅胸部IMU，可通过参数切换到胯部IMU、Lidar IMU或深度相机IMU。

功能特点：

• 订阅 sensor_msgs/Imu 数据，输出姿态四元数、角速度、线性加速度

• 打印姿态/角速度/加速度的协方差对角线（-1 表示未知）

• 统计接收间隔（recv_dt），便于观察实际到达频率

• 通过 imu_topic 参数可切换不同 IMU 源

支持的话题：

• /aima/hal/imu/chest/state：胸部IMU（默认）

• /aima/hal/imu/torso/state：胯部IMU

• /aima/hal/sensor/lidar_chest_front/imu：Lidar IMU

• /aima/hal/sensor/rgbd_head_front/imu：深度相机 IMU

技术实现：

• 使用 qos_profile_sensor_data（BEST_EFFORT + VOLATILE），与传感器数据默认 QoS 一致

#!/usr/bin/env python3
import time
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.time import Time
from sensor_msgs.msg import Imu


class ImuPrinter(Node):
    def __init__(self):
        super().__init__('imu_printer')
        # Allow overriding the topic name via a parameter
        self.declare_parameter('imu_topic', '/aima/hal/imu/chest/state')
        # Can be replaced with other IMU topic names.
        # Waist IMU: /aima/hal/imu/torso/state
        # LiDAR IMU: /aima/hal/sensor/lidar_chest_front/imu
        self.topic = self.get_parameter(
            'imu_topic').get_parameter_value().string_value

        self._last_recv = None
        # qos_profile_sensor_data = BestEffort / Volatile
        self.sub = self.create_subscription(
            Imu, self.topic, self.cb, qos_profile_sensor_data
        )
        self.get_logger().info(f'Subscribing IMU: {self.topic}')

    def cb(self, msg: Imu):
        # Compute receive interval (based on wall time, for observation only)
        now = time.perf_counter()
        dt_ms = 0.0 if self._last_recv is None else (
            now - self._last_recv) * 1000.0
        self._last_recv = now

        t_sec = Time.from_msg(msg.header.stamp).nanoseconds / 1e9
        q = msg.orientation
        w = msg.angular_velocity
        a = msg.linear_acceleration
        oc = msg.orientation_covariance
        wc = msg.angular_velocity_covariance
        ac = msg.linear_acceleration_covariance

        # Optional: print diagonal covariances (-1 means unknown)
        self.get_logger().info(
            (
                f"stamp={t_sec:.6f}s  frame={msg.header.frame_id}  recv_dt={dt_ms:.3f}ms\n"
                f"  orientation (x,y,z,w): [{q.x:.6f}, {q.y:.6f}, {q.z:.6f}, {q.w:.6f}]\n"
                f"  angular_velocity (rad/s): [{w.x:.6f}, {w.y:.6f}, {w.z:.6f}]\n"
                f"  linear_accel (m/s^2): [{a.x:.6f}, {a.y:.6f}, {a.z:.6f}]\n"
                f"  cov_diag(ori, ang, acc): "
                f"[{oc[0]:.6f},{oc[4]:.6f},{oc[8]:.6f} | "
                f"{wc[0]:.6f},{wc[4]:.6f},{wc[8]:.6f} | "
                f"{ac[0]:.6f},{ac[4]:.6f},{ac[8]:.6f}]"
            )
        )


def main():
    rclpy.init()
    node = ImuPrinter()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

# 订阅胸部IMU（默认）
ros2 run py_examples echo_imu_data

# 订阅胯部IMU
ros2 run py_examples echo_imu_data --ros-args -p imu_topic:=/aima/hal/imu/torso/state

# 订阅Lidar IMU
ros2 run py_examples echo_imu_data --ros-args -p imu_topic:=/aima/hal/sensor/lidar_chest_front/imu

输出示例：

[INFO] [1781764206.320174965] [imu_printer]: Subscribing IMU: /aima/hal/imu/chest/state
[INFO] [1781764206.318604000] [imu_printer]: stamp=1781764206.318604s  frame=base_link  recv_dt=2.039ms
  orientation (x,y,z,w): [0.087036, -0.264735, -0.019103, -0.960195]
  angular_velocity (rad/s): [-0.006531, 0.002874, 0.002391]
  linear_accel (m/s^2): [-5.013936, -1.548364, 8.280822]
  cov_diag(ori, ang, acc): [0.000000,0.000000,0.000000 | 0.000000,0.000000,0.000000 | 0.000000,0.000000,0.000000]

6.1.18 播放视频

该示例中用到了play_video，在运行节点程序前，需要先将视频上传到机器人的**交互计算单元（PC3）**上（用户可在其上创建一个用来存储视频的目录，如/var/tmp/videos/），然后将节点程序中的video_path改为需要播放视频的路径。

注意

⚠️ 请注意！交互计算单元（PC3）独立于二开程序所在的开发计算单元（PC2）, 音视频文件务必存入交互计算单元（IP: 10.0.1.42）。
⚠️ 音视频文件夹及该文件夹所有父目录应当为所有用户可访问读取(建议在/var/tmp/下创建子目录存放)

注意

使用本功能前，请在运控计算单元（PC1）上执行以下命令关闭 task_manager 模块，避免其与二开程序争抢显示屏控制权：

aima em stop-app task_manager

功能说明 通过调用PlayVideo服务，可以让机器人在屏幕上播放指定路径的视频文件。请确保视频文件已上传到交互计算单元，否则播放会失败。

💡 提示：视频文件的准备方式

您可以使用 C++ echo_camera_rgbd 示例 将机器人的深度相机 RGB 图像录制为 .mp4 视频文件，再将其作为 play_video 的输入：

第一步：在机器人开发计算单元（PC2）上录制视频（使用 C++ 示例）

ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/output.mp4
# 录制完成后按 Ctrl+C 停止，视频保存为 output.mp4

第二步：将视频传输到交互计算单元（PC3，IP: 10.0.1.42）

scp output.mp4 user@10.0.1.42:/var/tmp/videos/output.mp4

第三步：运行 play_video 播放

ros2 run py_examples play_video  # 修改代码中 video_path 为 /var/tmp/videos/output.mp4

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import PlayVideo


class PlayVideoClient(Node):
    def __init__(self):
        super().__init__('play_video_client')
        self.client = self.create_client(
            PlayVideo, '/aimdk_5Fmsgs/srv/PlayVideo')
        self.get_logger().info('✅ PlayVideo client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, video_path, mode, priority):
        req = PlayVideo.Request()

        req.video_path = video_path
        req.mode = mode
        req.priority = priority

        # async call
        self.get_logger().info(
            f'📨 Sending request to play video: mode={mode} video={video_path}')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        resp = future.result()
        if resp is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if resp.success:
            self.get_logger().info(
                f'✅ Request to play video recorded successfully: {resp.message}')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed to record play-video request: {resp.message}')
            return False


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

    try:
        # video path and priority can be customized
        default_video_path = "/agibot/software/face_ui/bin/app/aarch64-generic/assets/mp4/emoji_x2_sleepy.mp4"
        video_path = input(
            f"Enter video path (default: {default_video_path}): ").strip()
        if not video_path:
            video_path = default_video_path
        priority = 5
        # input play mode
        mode = int(input("Enter video play mode (1: play once, 2: loop): "))
        if mode not in (1, 2):
            raise ValueError(f'invalid mode {mode}')

        node = PlayVideoClient()
        node.send_request(video_path, mode, priority)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.19 音频文件播放

该示例中用到了play_audio，通过该节点可实现播放指定的音频文件，支持WAV（PCM编码）、RAW PCM格式的音频文件播放，不支持MP3等其他格式。

功能特点：

• 支持WAV（PCM编码）、RAW PCM格式播放，不支持MP3等其他格式

• 支持优先级控制，可设置播放优先级

• 支持自定义文件路径和播放参数

• 提供完整的错误处理和状态反馈

技术实现：

• 使用PlayAudioFile服务进行音频文件播放

• 支持优先级设置（priority: 1~10，priority_weight: 1~100）

• 提供详细的播放状态反馈

应用场景：

• 音频文件播放和媒体控制

• 语音提示和音效播放

• 多媒体应用开发

• 机器人交互音频反馈

注意

⚠️ 请注意！交互计算单元（PC3）独立于二开程序所在的开发计算单元（PC2）, 音视频文件务必存入交互计算单元（IP: 10.0.1.42）。
⚠️ 音视频文件夹及该文件夹所有父目录应当为所有用户可访问读取(建议在/var/tmp/下创建子目录存放)

#!/usr/bin/env python3

import os
import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import PlayAudioFile
from aimdk_msgs.msg import CommonState


class PlayAudioClient(Node):
    def __init__(self):
        super().__init__('play_audio_client')
        self.client = self.create_client(
            PlayAudioFile, '/aimdk_5Fmsgs/srv/PlayAudioFile')
        self.get_logger().info('✅ PlayAudio client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, audio_path):
        req = PlayAudioFile.Request()

        req.file.pkg_name = 'demo_client'
        req.file.file_name = os.path.basename(audio_path)
        req.file.file_path = os.path.dirname(audio_path) + '/'
        req.file.priority = 8
        req.file.priority_weight = 0

        self.get_logger().info(
            f'📨 Sending request to play audio: {audio_path}')
        for i in range(5):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=1.0)

            if future.done():
                break

            if not rclpy.ok():
                self.get_logger().warn('Interrupted while waiting')
                return False

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        resp = future.result()
        if resp is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if resp.reponse.status.value == CommonState.SUCCESS:  # SUCCESS (1)
            self.get_logger().info('✅ Request to play audio file recorded successfully.')
            return True
        else:
            # FAILURE(2): focus-apply failed; INVALID(5): invalid params;
            # other: see CommonState for details.
            reasons = {CommonState.FAILURE: 'focus apply failed',  # FAILURE (2)
                       CommonState.INVALID: 'invalid params'}      # INVALID (5)
            reason = reasons.get(resp.reponse.status.value, 'unknown')
            self.get_logger().error(
                f'❌ Failed to record play-audio request '
                f'(status={resp.reponse.status.value}, {reason}): '
                f'{resp.reponse.message}')
            return False


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

    default_audio = '/agibot/data/var/hal_audio/file/bingo.wav'
    try:
        if len(sys.argv) > 1:
            audio_path = sys.argv[1]
        else:
            audio_path = input(
                f'Enter audio file path to play (default: {default_audio}): ').strip()
            if not audio_path:
                audio_path = default_audio

        node = PlayAudioClient()
        node.send_request(audio_path)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

# 播放默认音频文件
ros2 run py_examples play_audio

# 播放指定音频文件
# 注意替换/path/to/your/audio_file.wav为交互板上实际文件路径
ros2 run py_examples play_audio /path/to/your/audio_file.wav

# 播放TTS缓存文件
ros2 run py_examples play_audio /agibot/data/var/hal_audio/file/bingo.wav

输出示例：

[INFO] [1779183967.998445447] [play_audio_client]: ✅ PlayAudio client node created.
[INFO] [1779183968.005492518] [play_audio_client]: 🟢 Service available, ready to send request.
[INFO] [1779183968.007782727] [play_audio_client]: 📨 Sending request to play audio: /agibot/data/var/hal_audio/file/bingo.wav
[INFO] [1779183968.029756237] [play_audio_client]: ✅ Request to play audio file recorded successfully.

注意事项：

• 确保音频文件路径正确且文件存在

• 支持的文件格式：WAV（PCM编码）、RAW PCM，不支持MP3等其他格式

• 优先级设置影响播放队列顺序

6.1.20 TTS (文字转语音)

该示例中用到了play_tts，通过该节点可实现语音播放输入的文字，用户可根据不同的场景输入相应的文本。

功能特点：

• 支持命令行参数和交互式输入

• 完整的服务可用性检查和错误处理

• 支持优先级控制和打断机制

• 提供详细的播放状态反馈

核心代码

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import PlayTts
from aimdk_msgs.msg import TtsPriorityLevel


class PlayTTSClient(Node):
    def __init__(self):
        super().__init__('play_tts_client')

        # fill in the actual service name
        self.client = self.create_client(PlayTts, '/aimdk_5Fmsgs/srv/PlayTts')
        self.get_logger().info('✅ PlayTts client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, text):
        req = PlayTts.Request()

        req.tts_req.text = text
        req.tts_req.domain = 'demo_client'   # required: caller domain
        req.tts_req.trace_id = 'demo'        # optional: request id
        req.tts_req.is_interrupted = True    # required: interrupt same-priority
        req.tts_req.priority_weight = 0
        # L6 (6)
        req.tts_req.priority_level.value = TtsPriorityLevel.INTERACTION_L6

        self.get_logger().info(f'📨 Sending request to play tts: text={text}')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        resp = future.result()
        if resp is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if resp.tts_resp.is_success:
            self.get_logger().info('✅ TTS sent successfully.')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed to send TTS: error_code={resp.tts_resp.error_code}, '
                f'error_message={resp.tts_resp.error_message}')
            return False


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

    try:
        # get text to speak
        if len(sys.argv) > 1:
            text = sys.argv[1]
        else:
            text = input('Enter text to speak: ')
            if not text:
                text = 'Hello, I am AgiBot X2.'

        node = PlayTTSClient()
        node.send_request(text)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明

# 使用命令行参数播报文本（推荐）
ros2 run py_examples play_tts "你好，我是灵犀X2机器人"

# 或者不带参数运行，程序会提示用户输入
ros2 run py_examples play_tts

输出示例

[INFO] [1779181480.764266243] [play_tts_client]: ✅ PlayTts client node created.
[INFO] [1779181480.765467698] [play_tts_client]: 🟢 Service available, ready to send request.
[INFO] [1779181480.766450608] [play_tts_client]: 📨 Sending request to play tts: text=Hello
[INFO] [1779181480.774077262] [play_tts_client]: ✅ TTS sent successfully.

注意事项

• 确保TTS服务正常运行

• 支持中文和英文文本播报

• 优先级设置影响播放队列顺序

• 打断功能可中断当前播放的语音

接口参考

• 服务：/aimdk_5Fmsgs/srv/PlayTts

• 消息：aimdk_msgs/srv/PlayTts

6.1.21 麦克风数据接收

该示例中用到了mic_receiver，通过订阅/agent/process_audio_output话题来接收机器人的降噪音频数据，支持内置麦克风和外置麦克风两种音频流，并根据VAD（语音活动检测）状态自动保存完整的语音片段为PCM文件。

功能特点：

• 基于VAD状态自动检测语音开始、处理中、结束

• 自动保存完整语音片段为PCM文件（单声道，采样率16kHz，格式S16LE）

• 按时间戳和音频流分类存储

• 支持音频时长计算和统计信息输出

应用场景：

• 语音识别和语音处理

• 音频数据采集和分析

• 实时语音监控

• 音频质量检测

• 多麦克风阵列数据处理

#!/usr/bin/env python3
"""
Microphone data receiving example

This example subscribes to the `/agent/process_audio_output` topic to receive the robot's
noise-suppressed audio data. It supports both the built-in microphone and the external
microphone audio streams, and automatically saves complete speech segments as PCM files
based on the VAD (Voice Activity Detection) state.

Features:
- Supports receiving multiple audio streams at the same time (built-in mic stream_id=1, external mic stream_id=2)
- Automatically detects speech start / in-progress / end based on VAD state
- Automatically saves complete speech segments as PCM files
- Stores files categorized by timestamp and audio stream
- Supports audio duration calculation and logging

VAD state description:
- 1: Speech start
- 2: Speech in progress
- 3: Speech end
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from aimdk_msgs.msg import ProcessedAudioOutput, AudioVadStateType
import os
from datetime import datetime
from collections import defaultdict
from typing import Dict, List


class AudioSubscriber(Node):
    def __init__(self):
        super().__init__('audio_subscriber')

        # Audio buffers, stored separately by stream_id
        # stream_id -> buffer
        self.audio_buffers: Dict[int, List[bytes]] = defaultdict(list)
        self.recording_state: Dict[int, bool] = defaultdict(bool)

        # Create audio output directory
        self.audio_output_dir = "audio_recordings"
        os.makedirs(self.audio_output_dir, exist_ok=True)

        # VAD state name mapping
        self.vad_state_names = {
            AudioVadStateType.AUDIO_VAD_STATE_BEGIN: "Speech start",
            AudioVadStateType.AUDIO_VAD_STATE_PROCESSING: "Speech in progress",
            AudioVadStateType.AUDIO_VAD_STATE_END: "Speech end",
        }

        # Audio stream name mapping
        self.stream_names = {
            1: "Built-in microphone",
            2: "External microphone"
        }

        # QoS settings
        # Note: deep queue to avoid missing data in a burst at start of VAD.
        qos = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=500,
            reliability=QoSReliabilityPolicy.BEST_EFFORT
        )

        # Create subscriber
        self.subscription = self.create_subscription(
            ProcessedAudioOutput,
            '/agent/process_audio_output',
            self.audio_callback,
            qos
        )

        self.get_logger().info("Start subscribing to noise-suppressed audio data...")

    def audio_callback(self, msg: ProcessedAudioOutput):
        """Audio data callback"""
        try:
            stream_id = msg.stream_id
            vad_state = msg.audio_vad_state.value
            audio_data = bytes(msg.audio_data)

            self.get_logger().info(
                f"Received audio data: stream_id={stream_id}, "
                f"vad_state={vad_state}({self.vad_state_names.get(vad_state, 'Unknown state')}), "
                f"audio_size={len(audio_data)} bytes"
            )

            self.handle_vad_state(stream_id, vad_state, audio_data)

        except Exception as e:
            self.get_logger().error(
                f"Error while processing audio message: {str(e)}")

    def handle_vad_state(self, stream_id: int, vad_state: int, audio_data: bytes):
        """Handle VAD state changes"""
        stream_name = self.stream_names.get(
            stream_id, f"Unknown stream {stream_id}")
        vad_name = self.vad_state_names.get(
            vad_state, f"Unknown state {vad_state}")

        self.get_logger().info(
            f"[{stream_name}] VAD state: {vad_name} audio: {len(audio_data)} bytes"
        )

        # Speech start
        if vad_state == AudioVadStateType.AUDIO_VAD_STATE_BEGIN:  # BEGIN (1)
            self.get_logger().info("🎤 Speech start detected")
            if not self.recording_state[stream_id]:
                self.audio_buffers[stream_id].clear()
                self.recording_state[stream_id] = True
            if audio_data:
                self.audio_buffers[stream_id].append(audio_data)

        # Speech in progress
        # PROCESSING (2)
        elif vad_state == AudioVadStateType.AUDIO_VAD_STATE_PROCESSING:
            self.get_logger().info("🔄 Speech in progress...")
            if self.recording_state[stream_id] and audio_data:
                self.audio_buffers[stream_id].append(audio_data)

        # Speech end
        elif vad_state == AudioVadStateType.AUDIO_VAD_STATE_END:  # END (3)
            self.get_logger().info("✅ Speech end")
            if self.recording_state[stream_id] and audio_data:
                self.audio_buffers[stream_id].append(audio_data)

            if self.recording_state[stream_id] and self.audio_buffers[stream_id]:
                self.save_audio_segment(stream_id)
            self.recording_state[stream_id] = False

        # Unknown speech state
        else:
            if self.recording_state[stream_id]:
                self.get_logger().info("⏹️ Reset recording state")
                self.recording_state[stream_id] = False

        # Print current buffer status
        buffer_size = sum(len(chunk)
                          for chunk in self.audio_buffers[stream_id])
        recording = self.recording_state[stream_id]
        self.get_logger().debug(
            f"[Stream {stream_id}] Buffer size: {buffer_size} bytes, recording: {recording}"
        )

    def save_audio_segment(self, stream_id: int):
        """Save audio segment"""
        if not self.audio_buffers[stream_id]:
            return

        # Merge all audio data
        audio_data = b''.join(self.audio_buffers[stream_id])

        # Get current timestamp
        now = datetime.now()
        timestamp = now.strftime("%Y%m%d_%H%M%S_%f")[:-3]  # to milliseconds

        # Create subdirectory by stream_id
        stream_dir = os.path.join(self.audio_output_dir, f"stream_{stream_id}")
        os.makedirs(stream_dir, exist_ok=True)

        # Generate filename
        stream_name = "internal_mic" if stream_id == 1 else "external_mic" if stream_id == 2 else f"stream_{stream_id}"
        filename = f"{stream_name}_{timestamp}.pcm"
        filepath = os.path.join(stream_dir, filename)

        try:
            # Save PCM file
            with open(filepath, 'wb') as f:
                f.write(audio_data)

            self.get_logger().info(
                f"Audio segment saved: {filepath} (size: {len(audio_data)} bytes)")

            # Calculate audio duration (assuming 16 kHz, 16-bit, mono)
            sample_rate = 16000
            bits_per_sample = 16
            channels = 1
            bytes_per_sample = bits_per_sample // 8
            total_samples = len(audio_data) // (bytes_per_sample * channels)
            duration_seconds = total_samples / sample_rate

            self.get_logger().info(
                f"Audio duration: {duration_seconds:.2f} s ({total_samples} samples)")

        except Exception as e:
            self.get_logger().error(f"Failed to save audio file: {str(e)}")


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

    try:
        node.get_logger().info(
            "Listening for noise-suppressed audio data, press Ctrl+C to exit...")
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Interrupt signal received, exiting...")
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

1. 运行程序：

   # 构建Python包
   colcon build --packages-select py_examples
   
   # 运行麦克风接收程序, 配合唤醒词触发VAD
   ros2 run py_examples mic_receiver
   

   按 Ctrl+C 安全退出程序，程序会自动处理音频流的开始和结束。

2. 查找录制的PCM文件：

   • 运行节点后会自动创建audio_recordings目录

   • 音频文件按stream_id分类存储（暂不区分, 一律使用stream_1）：

     • stream_1/: 内置麦克风音频

     • stream_2/: 外置麦克风音频

   • 文件命名格式：{stream_name}_{timestamp}.pcm

     • internal_mic_20250909_133649_738.pcm (内置麦克风)

     • external_mic_20250909_133650_123.pcm (外置麦克风)

3. 播放保存的PCM文件：

   # 播放内置麦克风录音
   aplay -r 16000 -f S16_LE -c 1 audio_recordings/stream_1/internal_mic_20250909_133649_738.pcm
   

   参数说明：

   • -r 16000 # 采样率16kHz

   • -f S16_LE # 16位小端格式

   • -c 1 # 单声道 你也可以用其他音频播放器（如Audacity）以原始PCM格式导入并播放。

4. 转换为WAV格式（可选）：

   # 使用ffmpeg转换为WAV格式
   ffmpeg -f s16le -ar 16000 -ac 1 -i external_mic_20250909_133649_738.pcm output.wav
   

输出示例：

正常启动和运行：

[INFO] [1779196050.590953283] [audio_subscriber]: Start subscribing to noise-suppressed audio data...
[INFO] [1779196050.592515383] [audio_subscriber]: Listening for noise-suppressed audio data, press Ctrl+C to exit...

检测到语音开始：

[INFO] [1779196055.100798381] [audio_subscriber]: Received audio data: stream_id=1, vad_state=1(Speech start), audio_size=2560 bytes
[INFO] [1779196055.101560663] [audio_subscriber]: [Built-in microphone] VAD state: Speech start audio: 2560 bytes
[INFO] [1779196055.102147334] [audio_subscriber]: 🎤 Speech start detected

语音处理过程：

[INFO] [1779196055.103814262] [audio_subscriber]: Received audio data: stream_id=1, vad_state=2(Speech in progress), audio_size=19200 bytes
[INFO] [1779196055.104458211] [audio_subscriber]: [Built-in microphone] VAD state: Speech in progress audio: 19200 bytes
[INFO] [1779196055.105151599] [audio_subscriber]: 🔄 Speech in progress...
[INFO] [1779196055.164856185] [audio_subscriber]: Received audio data: stream_id=1, vad_state=2(Speech in progress), audio_size=3200 bytes
[INFO] [1779196055.165511654] [audio_subscriber]: [Built-in microphone] VAD state: Speech in progress audio: 3200 bytes
[INFO] [1779196055.166184626] [audio_subscriber]: 🔄 Speech in progress...

语音结束和保存：

[INFO] [1779196055.867450360] [audio_subscriber]: Received audio data: stream_id=1, vad_state=3(Speech end), audio_size=2560 bytes
[INFO] [1779196055.868118086] [audio_subscriber]: [Built-in microphone] VAD state: Speech end audio: 2560 bytes
[INFO] [1779196055.868834482] [audio_subscriber]: ✅ Speech end
[INFO] [1779196055.870320425] [audio_subscriber]: Audio segment saved: audio_recordings/stream_1/internal_mic_20260519_210735_868.pcm (size: 49920 bytes)
[INFO] [1779196055.871051508] [audio_subscriber]: Audio duration: 1.56 s (24960 samples)

6.1.22 麦克风原始流式数据接收

该示例中用到了mic_raw_receiver，通过订阅/aima/hal/audio/capture话题来接收机器人的原始音频数据，识别麦克风/回采，自动按声道保存为PCM文件。

功能特点：

• 按时间戳和声道分类存储

• 自动保存声道音频数据为PCM文件（单声道，采样率16kHz，格式S16LE）

• 支持麦克风切换检测

#!/usr/bin/env python3
"""
Microphone raw data receiving example

This example subscribes to the `/aima/hal/audio/capture` topic to receive the robot's
raw audio data. It supports both the built-in microphone and the external
microphone audio streams, and automatically saves complete speech segments as PCM files.

Features:
- Automatically saves raw audio as PCM files
- Stores files categorized by timestamp and mic source

"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from aimdk_msgs.msg import AudioCapture
import os
import time
from datetime import datetime
from collections import defaultdict
from typing import Dict, List


class RawAudioSubscriber(Node):
    def __init__(self):
        super().__init__('raw_audio_subscriber')

        # Audio buffers, stored separately by channels
        self.audio_buffers: Dict[int, List[bytes]] = defaultdict(list)
        self.mic_channels = 0
        self.ref_channels = 0
        self.inited = 0
        self.dump_timestamp = 0

        # Create audio output directory
        now = datetime.now()
        timestamp = now.strftime("%Y%m%d_%H%M%S_%f")[:-3]  # to milliseconds

        self.audio_output_dir = os.path.join("audio_recordings", timestamp)
        os.makedirs(self.audio_output_dir, exist_ok=True)

        # QoS settings
        # Note: deep queue to avoid missing data
        qos = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=500,
            reliability=QoSReliabilityPolicy.BEST_EFFORT
        )

        # Create subscriber
        self.subscription = self.create_subscription(
            AudioCapture,
            '/aima/hal/audio/capture',
            self.audio_callback,
            qos
        )

        self.get_logger().info("Start subscribing to raw audio data...")

    def audio_callback(self, msg: AudioCapture):
        """Audio data callback"""
        if not self.inited:
            self.mic_channels = msg.mic_channels
            self.ref_channels = msg.ref_channels
            self.dump_timestamp = time.time()
            self.inited = 1
        elif self.mic_channels != msg.mic_channels or self.ref_channels != msg.ref_channels:
            # quit as MIC switched
            self.get_logger().error(
                f"MIC channels info changed (mic:{self.mic_channels} ref:{self.ref_channels}) -> (mic:{msg.mic_channels} ref:{msg.ref_channels})")
            if rclpy.ok():
                rclpy.shutdown()
            return
        audio_data = bytes(msg.data.data)

        self.handle_audio_data(audio_data)

    def handle_audio_data(self, audio_data: bytes):
        """Handle raw audio data"""

        # Split S16LE data into channels
        channels = self.ref_channels + self.mic_channels
        bytes_per_channel = 2
        unit_size = channels * bytes_per_channel
        base = 0
        for k in range(len(audio_data) // unit_size):
            for i in range(channels):
                tail = base + bytes_per_channel
                self.audio_buffers[i].append(audio_data[base:tail])
                base = tail

    def save_audio_segments(self):
        self.get_logger().info("🔄 Flushing cached audio data...")
        for i in range(self.mic_channels + self.ref_channels):
            self.save_audio_segment(i, i >= self.mic_channels)

    def save_audio_segment(self, channel: int, is_ref: bool):
        """Save audio segment"""
        if not self.audio_buffers[channel]:
            return

        # Merge all audio data
        audio_data = b''.join(self.audio_buffers[channel])
        self.audio_buffers[channel].clear()

        channel_type = 'mic'
        if is_ref:
            channel_type = 'ref'

        # Generate filename
        filepath = os.path.join(self.audio_output_dir,
                                f"channel_{channel}_{channel_type}.pcm")

        try:
            # Save PCM file
            with open(filepath, 'ab') as f:
                f.write(audio_data)

            self.get_logger().info(
                f"Audio segment saved: {filepath} appended: {len(audio_data)} bytes")

        except Exception as e:
            self.get_logger().error(f"Failed to save audio file: {str(e)}")

    def run(self):
        self.timer = self.create_timer(1.0, self.save_audio_segments)
        rclpy.spin(self)


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

    try:
        node = RawAudioSubscriber()
        node.get_logger().info("Listening to raw audio data, press Ctrl+C to exit...")
        node.run()
    except KeyboardInterrupt:
        rclpy.logging.get_logger('main').info(
            "Interrupt signal received, exiting...")

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

1. 运行程序：

   # 构建Python包
   colcon build --packages-select py_examples
   
   # 运行麦克风接收程序
   ros2 run py_examples mic_raw_receiver
   

2. 查找录制的PCM文件：

   • 运行节点后会自动创建audio_recordings目录

   • 音频文件按录制程序本次启动时间归集存储至如下格式的目录:

     • 20250909_133650_123

   • 文件命名格式：channel_{index}_{type}.pcm，如

     • channel_0_mic.pcm（麦克风）

     • channel_4_ref.pcm（回采）

3. 播放保存的PCM文件：

   • 转到计算机上播放：

     aplay -r 16000 -f S16_LE -c 1 audio_recordings/20250909_133650_123/channel_0_mic.pcm
     

   • 机器人内播放：

     # 使用原始流式音频播放例程
     ros2 run py_examples play_audio_stream --ros-args -p raw_audio_path:=$PWD/audio_recordings/20250909_133650_123/channel_0_mic.pcm
     # 或使用媒体文件播放例程, 注意提前将文件转存到交互板内, 并使用其路径替换下方pcm路径
     ros2 run py_examples play_audio please/use/path/on/PC3.pcm
     

6.1.23 获取当前麦克风

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import GetMicSourceRequest
from aimdk_msgs.msg import CommonRequest, CommonState


class GetMicSourceRequestClient(Node):
    def __init__(self):
        super().__init__('get_mic_source_request_client')
        self.client = self.create_client(
            GetMicSourceRequest, '/aimdk_5Fmsgs/srv/GetMicSourceRequest'
        )
        self.get_logger().info('✅ GetMicSourceRequest client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self):
        req = GetMicSourceRequest.Request()
        req.header = CommonRequest()

        self.get_logger().info(
            '📨 Sending request to get MIC source')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return

        if response.header.status.value == CommonState.SUCCESS:  # SUCCESS (1)
            self.get_logger().info('✅ MIC source get successfully.')
            self.get_logger().info(f'MIC id: {response.mic_source}')
        else:
            self.get_logger().error(
                f'❌ Failed to get MIC source: {response.header.message}'
            )


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        node = GetMicSourceRequestClient()
        node.send_request()
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.24 切换麦克风

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetMicSourceRequest
from aimdk_msgs.msg import CommonRequest, CommonState


class SetMicSourceRequestClient(Node):
    def __init__(self):
        super().__init__('set_mic_source_request_client')
        self.client = self.create_client(
            SetMicSourceRequest, '/aimdk_5Fmsgs/srv/SetMicSourceRequest'
        )
        self.get_logger().info('✅ SetMicSourceRequest client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, mic_source: int):
        req = SetMicSourceRequest.Request()
        req.header = CommonRequest()

        req.mic_source = mic_source

        self.get_logger().info(
            f'📨 Sending request to set mic source: {mic_source}')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return

        if response.header.status.value == CommonState.SUCCESS:  # SUCCESS (1)
            self.get_logger().info('✅ MIC source set successfully.')
        else:
            self.get_logger().error(
                f'❌ Failed to set MIC source: {response.header.message}'
            )


def main(args=None):
    mic_info = {
        0: ('int', 'internal MIC'),
        1: ('ext', 'external MIC'),
    }

    choices = {}
    for k, v in mic_info.items():
        choices[v[0]] = k

    rclpy.init(args=args)
    node = None
    try:
        # Prefer command-line argument, otherwise prompt for input
        if len(sys.argv) > 1:
            mic = sys.argv[1]
        else:
            print('{:<4} - {:<5} : {}'.format('abbr',
                  'mic_id', 'description'))
            for k, v in mic_info.items():
                print(f'{v[0]:<4} - {k:<5} : {v[1]}')
            mic = input('Enter abbr of MIC source:')

        mic_id = choices.get(mic)
        if mic_id is None:
            raise ValueError(f'Invalid abbr of MIC source: {mic}')

        node = SetMicSourceRequestClient()
        node.send_request(mic_id)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明

# 构建
colcon build --packages-select py_examples

# 交互式运行
ros2 run py_examples set_mic_source

# 命令行参数运行
ros2 run py_examples set_mic_source ext  # ext - 外置麦, int - 内置麦

6.1.25 原始流式音频播放

该示例中用到了play_audio_stream，先申请音频焦点，再将PCM文件以实时速率发布到/aima/hal/audio/playback话题进行播放；焦点被抢时自动退出。

#!/usr/bin/env python3
"""
Raw audio stream playback example

This example would get audio focus first and then publish raw audio data
at fixed rate to `/aima/hal/audio/playback` topic. And it would stop playing
when audio focus is lost.

"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
from aimdk_msgs.msg import AudioPlayback, AudioInfo, AudioData, FocusResponse, FocusRequester, CommonState
from aimdk_msgs.srv import RequestAudioFocus, AbandonAudioFocus
import os
import signal
import time
import sys

global_node = None


def signal_handler(sig, frame):
    global global_node
    if global_node is not None:
        if global_node.is_holding_focus():
            global_node.send_request(False)
        global_node.get_logger().info(
            f"Received signal {sig}, abandon audio focus and shutting down")
    if rclpy.ok():
        rclpy.shutdown()
    sys.exit(0)


class FakeMicDevice():
    def __init__(self, raw_audio_file):
        self.audio_data = []
        self.channels = 1
        self.sample_rate = 16000
        self.bytes_per_sample = 2  # S16LE
        self.audio_length = 0
        with open(raw_audio_file, 'rb') as f:
            self.audio_data = list(f.read())
            self.audio_length = len(self.audio_data)

        self.index = 0
        self.start_time = 0

    def get_channel_count(self):
        return self.channels

    def get_sample_rate(self):
        return self.sample_rate

    def get_cached_data(self):
        now = time.monotonic()
        if not self.start_time:
            # fake init: assume audio data started 1s ago
            self.start_time = now - 1

        # cached data range since last call
        tail = int((now - self.start_time) * self.sample_rate) * \
            self.channels * self.bytes_per_sample
        data = self.audio_data[self.index:tail]
        if tail < self.audio_length:
            self.index = tail
        else:
            # loopback
            self.index = 0
            self.start_time = now
        return data


class AudioStreamPlayer(Node):
    def __init__(self):
        super().__init__('audio_stream_player')

        self.declare_parameter('raw_audio_path', '')
        self.raw_audio_path = self.get_parameter('raw_audio_path').value

        self.mic_device = FakeMicDevice(self.raw_audio_path)

        self.pkg_name = 'audio_stream_player{}'.format(os.getpid())
        self.focus = False
        self.focus_force = True
        self.kAudioPriority = 8

        self.get_logger().info(f'local pkg name: {self.pkg_name}')
        # Create focus request/release clients
        self.request_client = self.create_client(
            RequestAudioFocus,
            '/aimdk_5Fmsgs/srv/RequestAudioFocus')
        self.release_client = self.create_client(
            AbandonAudioFocus,
            '/aimdk_5Fmsgs/srv/AbandonAudioFocus')

        # Wait for the service to become available
        while not self.request_client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')
        self.get_logger().info('🟢 Service available, ready to send request.')

        # Create focus event subscriber and raw audio publisher
        focus_qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10,
            durability=QoSDurabilityPolicy.VOLATILE
        )
        self.sub = self.create_subscription(
            FocusResponse,
            '/aima/hal/audio/focus_response',
            self.focus_event_callback, focus_qos
        )
        self.pub = self.create_publisher(
            AudioPlayback,
            '/aima/hal/audio/playback', 10
        )

    def send_request(self, focus: bool):
        if focus:
            self.send_focus_request()
        else:
            self.send_focus_release()

    def send_focus_request(self):
        req = RequestAudioFocus.Request()

        requester = FocusRequester()
        requester.pkg_name = self.pkg_name
        requester.priority = self.kAudioPriority

        req.focus_requester = requester

        self.get_logger().info('📨 Sending RequestAudioFocus request')
        for i in range(8):
            future = self.request_client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        if response.reponse.status.value == CommonState.SUCCESS:  # SUCCESS (1)
            focus = response.focus_response.focus_gain
            self.focus = focus
            self.get_logger().info(f'✅ RequestAudioFocus done: focus {focus}')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed in response of RequestAudioFocus: {response.reponse.message}'
            )
            return False

    def send_focus_release(self):
        req = AbandonAudioFocus.Request()

        requester = FocusRequester()
        requester.pkg_name = self.pkg_name
        requester.priority = self.kAudioPriority  # must match RequestAudioFocus

        req.focus_requester = requester

        self.get_logger().info('📨 Sending AbandonAudioFocus request')
        for i in range(8):
            future = self.release_client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        if response.reponse.status.value == CommonState.SUCCESS:  # SUCCESS (1)
            focus = response.focus_response.focus_gain
            # always focus False
            self.focus = focus
            self.get_logger().info(f'✅ AbandonAudioFocus done: focus {focus}')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed in response of AbandonAudioFocus: {response.reponse.message}'
            )
            return False

    def focus_event_callback(self, msg: FocusResponse):
        pkg_name = msg.pkg_name
        focus = msg.focus_gain
        if msg.pkg_name == self.pkg_name:
            self.get_logger().info(
                f'receive focus out event: focus state: {focus}')
            self.focus_force = msg.focus_gain

    def is_holding_focus(self):
        return self.focus and self.focus_force

    def spin_until_focus(self):
        while rclpy.ok():
            self.focus_force = True
            self.send_request(True)
            if self.is_holding_focus():
                return
            self.get_logger().info("need retry to get focus")
            t1 = t0 = time.monotonic()
            while rclpy.ok() and t1 - t0 < 1.0:
                rclpy.spin_once(self, timeout_sec=0.1)
                t1 = time.monotonic()
        return

    def run_once(self):
        """ flush raw audio data cached since last call """

        raw_audio_msg = AudioPlayback()

        raw_audio_msg.pkg_name = self.pkg_name
        raw_audio_msg.token_id = self.pkg_name
        # fill AudioInfo
        audio_info = AudioInfo()
        audio_info.channels = self.mic_device.get_channel_count()
        audio_info.sample_rate = self.mic_device.get_sample_rate()
        # fill raw audio data
        audio_data = AudioData()
        cached_data = self.mic_device.get_cached_data()
        audio_data.data = cached_data
        raw_audio_msg.info = audio_info
        raw_audio_msg.data = audio_data
        # publish
        self.pub.publish(raw_audio_msg)

    def run(self):
        self.get_logger().info('🟢 publishing audio data ...')
        while rclpy.ok() and self.is_holding_focus():
            self.run_once()
            rclpy.spin_once(self, timeout_sec=0.05)
            time.sleep(0.05)

        if not self.is_holding_focus():
            self.get_logger().info("focus out, exiting...")


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

    try:
        node = AudioStreamPlayer()

        global_node = node
        signal.signal(signal.SIGINT, signal_handler)
        signal.signal(signal.SIGTERM, signal_handler)

        node.spin_until_focus()
        node.run()

    except KeyboardInterrupt:
        rclpy.logging.get_logger('main').info(
            "Interrupt signal received, exiting...")
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明

# 注意替换/path/to/your/raw_audio_file.pcm为原始音频文件路径，单通道，采样率16kHz，格式S16LE
ros2 run py_examples play_audio_stream --ros-args -p raw_audio_path:=/path/to/your/raw_audio_file.pcm

6.1.26 表情控制

该示例中用到了play_emoji，通过该节点可实现表达指定的表情，用户可根据已有的表情列表来选择要变化的表情，具体表情列表可参考表情列表

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import PlayEmoji


class PlayEmojiClient(Node):
    def __init__(self):
        super().__init__('play_emoji_client')
        self.client = self.create_client(
            PlayEmoji, '/aimdk_5Fmsgs/srv/PlayEmoji')
        self.get_logger().info('✅ PlayEmoji client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, emoji: int, mode: int, priority: int):
        req = PlayEmoji.Request()

        req.emotion_id = int(emoji)
        req.mode = int(mode)
        req.priority = int(priority)

        self.get_logger().info(
            f'📨 Sending request to play emoji: id={emoji}, mode={mode}, priority={priority}')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        resp = future.result()
        if resp is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if resp.success:
            self.get_logger().info(
                f'✅ Emoji played successfully: {resp.message}')
            return True
        else:
            self.get_logger().error(f'❌ Failed to play emoji: {resp.message}')
            return False


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

    # Interactive input, same as the original C++ version
    try:
        emotion = int(
            input("Enter emoji ID: 1-blink, 60-bored, 70-abnormal, 80-sleeping, 90-happy ... 190-double angry, 200-adore: "))
        mode = int(input("Enter play mode (1: play once, 2: loop): "))
        if mode not in (1, 2):
            raise ValueError("invalid mode")
        priority = 10  # default priority

        node = PlayEmojiClient()
        node.send_request(emotion, mode, priority)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.27 地图构建

该示例中用到了slam，通过该节点给建图服务发送消息，实现建图功能

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy
from std_msgs.msg import String


class SlamCommandPublisher(Node):
    def __init__(self):
        super().__init__('slam_command_publisher')
        # Match SLAM subscriber QoS: TRANSIENT_LOCAL + RELIABLE
        qos = QoSProfile(
            depth=10,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            reliability=QoSReliabilityPolicy.RELIABLE,
        )
        self.publisher_ = self.create_publisher(
            String, '/integrated_command', qos)

    def publish_start_mapping(self):
        msg = String()
        msg.data = 'start_mapping'
        self.get_logger().info(f'Publishing: [{msg.data}]')
        self.publisher_.publish(msg)

    def publish_stop_mapping(self, map_name):
        msg = String()
        msg.data = f'stop_mapping:{map_name}'
        self.get_logger().info(f'Publishing: [{msg.data}]')
        self.publisher_.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    slam_command_publisher = SlamCommandPublisher()

    try:
        while rclpy.ok():
            input_value = input(
                "Enter 1 to start mapping, 2 to stop mapping: ")

            if input_value == '1':
                slam_command_publisher.publish_start_mapping()
            elif input_value == '2':
                map_name = input("Enter map name: ")
                slam_command_publisher.publish_stop_mapping(map_name)
            else:
                print("Invalid input. Please enter 1 or 2.")
    except KeyboardInterrupt:
        pass
    finally:
        slam_command_publisher.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples slam
Enter 1 to start mapping, 2 to stop mapping: 1
[INFO] [1772521830.713768092] [slam_command_publisher]: Publishing: [start_mapping]
Enter 1 to start mapping, 2 to stop mapping: 2
Enter map name: testmap
[INFO] [1772521837.760016698] [slam_command_publisher]: Publishing: [stop_mapping:testmap]

开始建图之后，推动机器，或者控制机器人移动，完成建图

6.1.28 使用地图进行重定位

该示例中用到地图构建步骤完成构建的地图进行定位

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy


class RelocalizationNode(Node):
    def __init__(self, map_id, init_x, init_y):
        super().__init__('relocalization_node')

        self.map_id = map_id
        self.init_x = init_x
        self.init_y = init_y

        # /integrated_command subscriber requires TRANSIENT_LOCAL durability
        command_qos = QoSProfile(
            depth=10,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
        )

        # Create publishers
        self.integrated_command_pub = self.create_publisher(
            String, '/integrated_command', command_qos)
        self.relocalization_pose_pub = self.create_publisher(
            Pose, '/relocalization_pose', 10)

        # Create subscriber with BEST_EFFORT QoS
        lidar_loc_qos = QoSProfile(
            depth=10,
            reliability=QoSReliabilityPolicy.BEST_EFFORT
        )

        self.odometry_sub = self.create_subscription(
            Odometry,
            '/slam/lidar_odom',
            self.odometry_callback,
            qos_profile=lidar_loc_qos
        )

        # Setup timer to publish messages in sequence
        self.success_received = False
        self.timeout_timer = None

        # Wait for subscribers before publishing
        self.publish_timer = self.create_timer(0.2, self.wait_and_publish)

    def wait_and_publish(self):
        if (self.integrated_command_pub.get_subscription_count() == 0 or
                self.relocalization_pose_pub.get_subscription_count() == 0):
            self.get_logger().info(
                f'Waiting for subscribers... '
                f'(command: {self.integrated_command_pub.get_subscription_count()}, '
                f'pose: {self.relocalization_pose_pub.get_subscription_count()})',
                throttle_duration_sec=2.0)
            return
        self.publish_timer.cancel()
        self.publish_sequence()

    def publish_sequence(self):
        # Publish integrated_command
        integrated_command_msg = String()
        integrated_command_msg.data = f'start_relocalization:{self.map_id}'
        self.integrated_command_pub.publish(integrated_command_msg)
        self.get_logger().info('Published integrated_command')

        # Schedule relocalization_pose publication after delay
        self.relocalization_pose_timer = self.create_timer(
            1.0,
            self.publish_relocalization_pose
        )

    def publish_relocalization_pose(self):
        # Cancel this timer immediately so it only fires once
        self.relocalization_pose_timer.cancel()

        relocalization_pose_msg = Pose()
        relocalization_pose_msg.position.x = self.init_x
        relocalization_pose_msg.position.y = self.init_y
        relocalization_pose_msg.position.z = 0.0
        relocalization_pose_msg.orientation.x = 0.0
        relocalization_pose_msg.orientation.y = 0.0
        relocalization_pose_msg.orientation.z = 0.0
        relocalization_pose_msg.orientation.w = 1.0

        self.relocalization_pose_pub.publish(relocalization_pose_msg)
        self.get_logger().info('Published relocalization_pose')

        # Start timeout timer (30 seconds)
        self.timeout_timer = self.create_timer(
            30.0,
            self.timeout_callback
        )

        self.get_logger().info('Waiting for robot pose data (timeout: 30s)...')

    def odometry_callback(self, msg):
        if not self.success_received:
            self.success_received = True
            if self.timeout_timer is not None:
                self.timeout_timer.cancel()
            self.get_logger().info('Received odometry data - Relocalization successful!')
            if rclpy.ok():
                rclpy.shutdown()

    def timeout_callback(self):
        if not self.success_received:
            self.get_logger().error('Timeout reached - Relocalization failed!')
            if rclpy.ok():
                rclpy.shutdown()


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

    # Inputs needed for relocalization. To find them, see:
    #   map_id        -> sqlite3 /agibot/data/var/MapManagerModule/map.db
    #                    "SELECT * FROM map;"
    #   init_x/init_y -> origin (u, v) in
    #                    /agibot/data/var/MapManagerModule/<map_id>/grid_map_info.txt
    #                    (line 2; pixel coordinates)
    try:
        map_id = int(input("Enter map_id: ").strip())
        init_x = float(input("Enter initial position.x (pixel u): ").strip())
        init_y = float(input("Enter initial position.y (pixel v): ").strip())
    except (ValueError, EOFError):
        print("Invalid input, aborted.")
        if rclpy.ok():
            rclpy.shutdown()
        return

    node = RelocalizationNode(map_id, init_x, init_y)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples relocate
Enter map_id: 1778133306741
Enter initial position.x (pixel u): 1041
Enter initial position.y (pixel v): 630
[INFO] [1772526167.342953490] [relocalization_node]: Published integrated_command
[INFO] [1772526168.346034723] [relocalization_node]: Published relocalization_pose
[INFO] [1772526168.346915278] [relocalization_node]: Waiting for robot pose data (timeout: 30s)...
[INFO] [1772526173.347455156] [relocalization_node]: Received odometry data - Relocalization successful!

需要将机器人移动到地图构建的起始点，然后运行重定位程序

参数获取方式：map_id 与起点像素坐标（position.x / position.y）的获取见 地图标识与坐标的获取。

6.1.29 获取已存储的地图

该示例中用到了get_map，按地图名称获取机器人上已存储的地图数据，包括地图路径、分辨率、宽高及地图ID等信息。

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor
from aimdk_msgs.srv import GetStoredMapByName
from std_msgs.msg import Header


class MapClient(Node):
    def __init__(self):
        super().__init__('get_map_client')
        self.client = self.create_client(
            GetStoredMapByName,
            '/aimdk_5Fmsgs/srv/GetStoredMapByName'
        )

    def send_request(self, map_name):
        while not self.client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for service.")
                return
            self.get_logger().info("Service not available, waiting again...")

        request = GetStoredMapByName.Request()

        request.header = Header()
        request.header.stamp = self.get_clock().now().to_msg()
        request.header.frame_id = ""

        request.map_name = map_name

        future = self.client.call_async(request)

        executor = SingleThreadedExecutor()
        executor.add_node(self)
        executor.spin_until_future_complete(future, timeout_sec=60.0)
        executor.remove_node(self)

        if future.result() is None:
            self.get_logger().error("Service call failed or timed out.")
            return

        result = future.result()

        if result.code == 0:
            self.get_logger().info("Service call succeeded")
            self.get_logger().info(f"Map path: {result.map_path}")
            self.get_logger().info(
                f"Map resolution: {result.map_info.resolution}")
            self.get_logger().info(f"Map width: {result.map_info.width}")
            self.get_logger().info(f"Map height: {result.map_info.height}")
            self.get_logger().info(f"Map id: {result.map_id}")
        else:
            self.get_logger().error(
                f"Service call failed with code: {result.code}")


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

    client = MapClient()
    try:
        # Map name (NOT map_id) is required. To list stored maps, run:
        #   sqlite3 /agibot/data/var/MapManagerModule/map.db "SELECT * FROM map;"
        map_name = input("Enter map name: ").strip()
        if not map_name:
            client.get_logger().error("Map name is empty, aborted.")
            return
        client.send_request(map_name)
    except KeyboardInterrupt:
        pass
    finally:
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples get_map
Enter map name: test_map
[INFO] [get_map_client]: Service call succeeded
[INFO] [get_map_client]: Map path: /path/to/test_map.png
[INFO] [get_map_client]: Map resolution: 0.05
[INFO] [get_map_client]: Map width: 200
[INFO] [get_map_client]: Map height: 200
[INFO] [get_map_client]: Map id: 1778133306741

参数获取方式：输入的是地图名称（map_name），不是 map_id；map_name 的获取见 地图标识与坐标的获取。

6.1.30 导航

该示例提供了交互式终端菜单，支持导航任务的开始、暂停、恢复和停止操作。

#!/usr/bin/env python3
"""
Navigation example: publishes navigation task requests (start/pause/resume/stop)
via an interactive terminal menu.
"""

import time

import numpy as np
import rclpy
from rclpy.node import Node

from aimdk_msgs.msg import MessageHeader, PncTaskRequest
from geometry_msgs.msg import PoseStamped

STARTUP_DELAY = 5.0

# task_request enum
TASK_REQUEST_START = 1
TASK_REQUEST_STOP = 2
TASK_REQUEST_PAUSE = 3
TASK_REQUEST_RESUME = 4


class NavigationExample(Node):

    def __init__(self):
        super().__init__('navigation_example')
        self.publisher = self.create_publisher(
            PncTaskRequest,
            '/aima/te/pnc_task_request',
            10,
        )
        self.task_id = int(time.time() * 1000)
        self.map_id = 0
        self.get_logger().info(
            f'Navigation example node started, task_id={self.task_id}')

    def _make_header(self) -> MessageHeader:
        header = MessageHeader()
        now = self.get_clock().now().to_msg()
        header.stamp = now
        header.meas_stamp = now
        header.frame_id = 'map'
        header.sequence = 0
        return header

    def send_start(self, map_id: int, x: float, y: float, z: float,
                   ox: float, oy: float, oz: float, ow: float,
                   radius: float, pnc_mode: int, max_speed: float):
        """Start navigation."""
        self.map_id = map_id
        msg = PncTaskRequest()
        msg.header = self._make_header()
        msg.task_type = 2
        msg.task_request = TASK_REQUEST_START
        msg.pnc_mode = pnc_mode
        msg.max_forward_speed = max_speed
        msg.task_id = self.task_id
        msg.map_id = map_id
        msg.target_pose_radius = radius
        msg.reserve_info = np.zeros(64, dtype=np.uint8)

        target = PoseStamped()
        target.header.stamp = self.get_clock().now().to_msg()
        target.header.frame_id = 'map'
        target.pose.position.x = x
        target.pose.position.y = y
        target.pose.position.z = z
        target.pose.orientation.x = ox
        target.pose.orientation.y = oy
        target.pose.orientation.z = oz
        target.pose.orientation.w = ow
        msg.target_pose = target

        self.publisher.publish(msg)
        self.get_logger().info(
            f'[Start] task_id={self.task_id}, map_id={map_id}, '
            f'target=({x}, {y}, {z}), radius={radius}'
        )

    def send_pause(self):
        """Pause navigation."""
        msg = PncTaskRequest()
        msg.header = self._make_header()
        msg.task_type = 2
        msg.task_request = TASK_REQUEST_PAUSE
        msg.pnc_mode = 0
        msg.max_forward_speed = 0.5
        msg.task_id = self.task_id
        msg.map_id = 0
        msg.target_pose_radius = 0.0
        msg.reserve_info = np.zeros(64, dtype=np.uint8)

        target = PoseStamped()
        target.pose.orientation.w = 1.0
        msg.target_pose = target

        self.publisher.publish(msg)
        self.get_logger().info(f'[Pause] task_id={self.task_id}')

    def send_resume(self):
        """Resume navigation."""
        msg = PncTaskRequest()
        msg.header = self._make_header()
        msg.task_type = 2
        msg.task_request = TASK_REQUEST_RESUME
        msg.pnc_mode = 0
        msg.max_forward_speed = 0.5
        msg.task_id = self.task_id
        msg.map_id = self.map_id
        msg.target_pose_radius = 0.0
        msg.reserve_info = np.zeros(64, dtype=np.uint8)

        target = PoseStamped()
        target.pose.orientation.w = 1.0
        msg.target_pose = target

        self.publisher.publish(msg)
        self.get_logger().info(
            f'[Resume] task_id={self.task_id}, map_id={self.map_id}')

    def send_stop(self):
        """Stop navigation."""
        msg = PncTaskRequest()
        msg.header = self._make_header()
        msg.task_type = 2
        msg.task_request = TASK_REQUEST_STOP
        msg.pnc_mode = 0
        msg.max_forward_speed = 0.5
        msg.task_id = self.task_id
        msg.map_id = self.map_id
        msg.target_pose_radius = 0.0
        msg.reserve_info = np.zeros(64, dtype=np.uint8)

        target = PoseStamped()
        target.pose.orientation.w = 1.0
        msg.target_pose = target

        self.publisher.publish(msg)
        self.get_logger().info(
            f'[Stop] task_id={self.task_id}, map_id={self.map_id}')


def _input_float(prompt: str, default: float) -> float:
    raw = input(f'{prompt} [{default}]: ').strip()
    if not raw:
        return default
    return float(raw)


def _input_int(prompt: str, default: int) -> int:
    raw = input(f'{prompt} [{default}]: ').strip()
    if not raw:
        return default
    return int(raw)


def print_menu(task_id: int):
    print('\n' + '=' * 40)
    print(f'  Navigation Control  (task_id={task_id})')
    print('=' * 40)
    print('  1. Start navigation')
    print('  2. Pause navigation')
    print('  3. Resume navigation')
    print('  4. Stop navigation')
    print('  q. Quit')
    print('=' * 40)


def main():
    rclpy.init()
    nav = NavigationExample()

    nav.get_logger().info(
        f'Waiting for node communication ready ({STARTUP_DELAY}s)...')
    time.sleep(STARTUP_DELAY)

    try:
        while True:
            print_menu(nav.task_id)
            choice = input('Select an option: ').strip()

            if choice == '1':
                print('\n--- Start Navigation Parameters ---')
                map_id = _input_int('Map ID (map_id)', 1773113429735)
                x = _input_float('Target x', 1.0)
                y = _input_float('Target y', 2.0)
                z = _input_float('Target z', 0.0)
                ox = _input_float('Orientation x', 0.0)
                oy = _input_float('Orientation y', 0.0)
                oz = _input_float('Orientation z', 0.0)
                ow = _input_float('Orientation w', 1.0)
                radius = _input_float('Target pose radius', 0.5)
                pnc_mode = _input_int('pnc_mode', 0)
                max_speed = _input_float('Max forward speed', 0.5)
                nav.send_start(map_id, x, y, z, ox, oy, oz,
                               ow, radius, pnc_mode, max_speed)

            elif choice == '2':
                nav.send_pause()

            elif choice == '3':
                nav.send_resume()

            elif choice == '4':
                nav.send_stop()

            elif choice.lower() == 'q':
                print('Exiting navigation control.')
                break

            else:
                print('Invalid input, please try again.')

    except KeyboardInterrupt:
        nav.get_logger().info('Interrupted by user (Ctrl+C)')
    finally:
        nav.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples navigation

输出示例:

[INFO] [navigation_example]: Navigation example node started, task_id=1709123456789
[INFO] [navigation_example]: Waiting for node communication ready (5.0s)...

========================================
  Navigation Control  (task_id=1709123456789)
========================================
  1. Start navigation
  2. Pause navigation
  3. Resume navigation
  4. Stop navigation
  q. Quit
========================================
Select an option: 1

--- Start Navigation Parameters ---
Map ID (map_id) [1773113429735]:
Target x [1.0]:
Target y [2.0]:
...
[INFO] [navigation_example]: [Start] task_id=1709123456789, map_id=1773113429735, target=(1.0, 2.0, 0.0), radius=0.5

开始导航时需输入地图ID、目标位姿、到点精度等参数，每项都有默认值，直接回车即可使用默认值。

参数获取方式：map_id 的获取见 地图标识与坐标的获取；target_x / target_y 为目标点米制坐标（map 坐标系），与重定位用的像素坐标不同，根据实际任务直接指定

6.1.31 播放灵创动作

该示例中用到了play_linkcraft，列出机器人上的灵创动作，并且选择一条进行播放

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from aimdk_msgs.srv import GetRobotResources, ExecuteActionResource


class RobotResourceClient(Node):
    def __init__(self):
        super().__init__('robot_resource_client')

        self.get_resource_client = self.create_client(
            GetRobotResources,
            '/aimdk_5Fmsgs/srv/GetRobotResources'
        )
        self.play_resource_client = self.create_client(
            ExecuteActionResource,
            '/aimdk_5Fmsgs/srv/ExecuteActionResource'
        )

        self.resources = []

    def get_robot_resources(self):
        while not self.get_resource_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for the service. Exiting.")
                return False
            self.get_logger().info("Service not available, waiting again...")

        request = GetRobotResources.Request()
        future = self.get_resource_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is None:
            self.get_logger().error("Failed to call GetRobotResources")
            return False

        response = future.result()
        self.resources = list(response.robot_resources)
        self.get_logger().info(f"Found {len(self.resources)} resources")
        return True

    def print_resource_menu(self):
        if not self.resources:
            print("(no resources)")
            return

        print("\n=== Available Robot Resources ===")
        for idx, resource in enumerate(self.resources):
            ver = resource.current_version
            print(f"[{idx}] {ver.name or '<no name>'}")
            print(f"     key:     {resource.resource_key}")
            print(f"     version: {ver.version}")
        print("=================================\n")

    def pick_resource(self):
        if not self.resources:
            return None

        while True:
            try:
                raw = input(
                    f"Select resource index [0-{len(self.resources) - 1}] (q to quit): ").strip()
            except EOFError:
                return None
            if raw.lower() in ("q", "quit", "exit"):
                return None
            if not raw.isdigit():
                print("Please enter a number.")
                continue
            idx = int(raw)
            if 0 <= idx < len(self.resources):
                return self.resources[idx]
            print(f"Index out of range (0-{len(self.resources) - 1}).")

    def play_resource(self, resource):
        while not self.play_resource_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for service.")
                return
            self.get_logger().info("Service not available, waiting...")

        request = ExecuteActionResource.Request()
        request.resource_key = resource.resource_key
        request.resource_version = resource.current_version.version

        if "onnx" in request.resource_key:
            request.meta = '{"resource_type": "BODY_MONTION"}'
        else:
            request.meta = '{"resource_type": "ARM_MONTION"}'

        self.get_logger().info("Sending request:")
        self.get_logger().info(f"  resource_key: {request.resource_key}")
        self.get_logger().info(
            f"  resource_version: {request.resource_version}")
        self.get_logger().info(f"  meta: {request.meta}")

        future = self.play_resource_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is not None:
            self.handle_response(future.result())
        else:
            self.get_logger().error("Failed to call ExecuteActionResource")

    def handle_response(self, response):
        self.get_logger().info("Service response received:")
        self.get_logger().info("Response Header:")
        self.get_logger().info(
            f"  stamp: {response.header.header.stamp.sec}.{response.header.header.stamp.nanosec:09}")
        self.get_logger().info(f"  code: {response.header.header.code}")
        self.get_logger().info("Status:")
        self.get_logger().info(f"  value: {response.header.status.value}")
        self.get_logger().info(f"  message: {response.header.message}")


def main(args=None):
    rclpy.init(args=args)
    client_node = None
    try:
        client_node = RobotResourceClient()
        if not client_node.get_robot_resources():
            return
        client_node.print_resource_menu()
        chosen = client_node.pick_resource()
        if chosen is None:
            client_node.get_logger().info("No resource selected, exiting.")
            return
        client_node.play_resource(chosen)
    except KeyboardInterrupt:
        pass
    finally:
        if client_node:
            client_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples play_linkcraft
[INFO] [1772526701.308025739] [robot_resource_client]: Found 1 resources

=== Available Robot Resources ===
[0] <no name>
     key:     linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
     version: 0.0.1
=================================

[INFO] [1772526701.308537895] [robot_resource_client]: Sending request:
[INFO] [1772526701.308692872] [robot_resource_client]:   resource_key: linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
[INFO] [1772526701.308853424] [robot_resource_client]:   resource_version: 0.0.1
[INFO] [1772526701.309014801] [robot_resource_client]:   meta: {"resource_type": "BODY_MONTION"}
[INFO] [1772526701.338307933] [robot_resource_client]: Service response received:
[INFO] [1772526701.338480169] [robot_resource_client]: Response Header:
[INFO] [1772526701.338647069] [robot_resource_client]:   stamp: 0.000000000
[INFO] [1772526701.338800125] [robot_resource_client]:   code: 0
[INFO] [1772526701.338951109] [robot_resource_client]: Status:
[INFO] [1772526701.339106459] [robot_resource_client]:   value: 1
[INFO] [1772526701.339239979] [robot_resource_client]:   message: 1/1 task(s) failed. Details: localhost: failed (status code: 400) - Failed with status code: 400

6.1.32 群控播放灵创动作

该示例中用到了 multi_play_linkcraft，在多台机器人之间进行时间同步，并群控播放灵创动作。

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from aimdk_msgs.msg import SlaveDevice
from aimdk_msgs.srv import GetRobotResources, ExecuteActionResource, GroupControlTimeSync


class RobotResourceClient(Node):
    def __init__(self):
        super().__init__('robot_resource_client')

        self.get_resource_client = self.create_client(
            GetRobotResources,
            '/aimdk_5Fmsgs/srv/GetRobotResources'
        )
        self.play_resource_client = self.create_client(
            ExecuteActionResource,
            '/aimdk_5Fmsgs/srv/ExecuteActionResource'
        )
        self.time_sync_client = self.create_client(
            GroupControlTimeSync,
            '/aimdk_5Fmsgs/srv/GroupControlTimeSync'
        )

        self.resources = []
        self.slave_ips = []

    def get_robot_resources(self):
        while not self.get_resource_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for the service. Exiting.")
                return False
            self.get_logger().info("Service not available, waiting again...")

        request = GetRobotResources.Request()
        future = self.get_resource_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is None:
            self.get_logger().error("Failed to call GetRobotResources")
            return False

        response = future.result()
        self.resources = list(response.robot_resources)
        self.get_logger().info(f"Found {len(self.resources)} resources")
        return True

    def print_resource_menu(self):
        if not self.resources:
            print("(no resources)")
            return

        print("\n=== Available Robot Resources ===")
        for idx, resource in enumerate(self.resources):
            ver = resource.current_version
            print(f"[{idx}] {ver.name or '<no name>'}")
            print(f"     key:     {resource.resource_key}")
            print(f"     version: {ver.version}")
        print("=================================\n")

    def pick_resource(self):
        if not self.resources:
            return None

        while True:
            try:
                raw = input(
                    f"Select resource index [0-{len(self.resources) - 1}] (q to quit): ").strip()
            except EOFError:
                return None
            if raw.lower() in ("q", "quit", "exit"):
                return None
            if not raw.isdigit():
                print("Please enter a number.")
                continue
            idx = int(raw)
            if 0 <= idx < len(self.resources):
                return self.resources[idx]
            print(f"Index out of range (0-{len(self.resources) - 1}).")

    def play_resource(self, resource):
        while not self.play_resource_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for service.")
                return
            self.get_logger().info("Service not available, waiting...")

        request = ExecuteActionResource.Request()
        request.resource_key = resource.resource_key
        request.resource_version = resource.current_version.version

        for ip in self.slave_ips:
            slave = SlaveDevice()
            slave.ip = ip
            request.slaves.append(slave)

        if "onnx" in request.resource_key:
            request.meta = '{"control_type": "GROUP_CONTROL","resource_type": "BODY_MONTION"}'
        else:
            request.meta = '{"control_type": "GROUP_CONTROL","resource_type": "ARM_MONTION"}'

        self.get_logger().info("Sending request:")
        self.get_logger().info(f"  resource_key: {request.resource_key}")
        self.get_logger().info(
            f"  resource_version: {request.resource_version}")
        self.get_logger().info(f"  meta: {request.meta}")
        for i, slave in enumerate(request.slaves):
            self.get_logger().info(f"  slaves[{i}] ip: {slave.ip}")

        future = self.play_resource_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is not None:
            self.handle_response(future.result())
        else:
            self.get_logger().error("Failed to call ExecuteActionResource")

    def sync_time(self, ips):
        self.slave_ips = ips

        while not self.time_sync_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for time sync service.")
                return False
            self.get_logger().info("TimeSync service not available, waiting again...")

        request = GroupControlTimeSync.Request()
        for ip in ips:
            slave = SlaveDevice()
            slave.ip = ip
            request.slaves.append(slave)

        self.get_logger().info("Sending GroupControlTimeSync request:")
        for slave in request.slaves:
            self.get_logger().info(f"  slave ip: {slave.ip}")

        future = self.time_sync_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is None:
            self.get_logger().error("Failed to call GroupControlTimeSync")
            return False

        response = future.result()
        self.get_logger().info("GroupControlTimeSync response:")
        self.get_logger().info(f"  error_code: {response.error_code}")
        self.get_logger().info(f"  error_msg: {response.error_msg}")
        return response.error_code == 0

    def handle_response(self, response):
        self.get_logger().info("Service response received:")
        self.get_logger().info("Response Header:")
        self.get_logger().info(
            f"  stamp: {response.header.header.stamp.sec}.{response.header.header.stamp.nanosec:09}")
        self.get_logger().info(f"  code: {response.header.header.code}")
        self.get_logger().info("Status:")
        self.get_logger().info(f"  value: {response.header.status.value}")
        self.get_logger().info(f"  message: {response.header.message}")


def main(args=None):
    rclpy.init(args=args)
    client_node = None
    try:
        client_node = RobotResourceClient()
        if not client_node.get_robot_resources():
            return

        # [USER_CONFIG] Replace the IPs below with your slave robot IPs.
        slave_ips = ["172.22.133.178", "172.22.135.45"]
        if not client_node.sync_time(slave_ips):
            client_node.get_logger().warn("Time sync failed or returned error.")

        client_node.print_resource_menu()
        chosen = client_node.pick_resource()
        if chosen is None:
            client_node.get_logger().info("No resource selected, exiting.")
            return
        client_node.play_resource(chosen)
    except KeyboardInterrupt:
        pass
    finally:
        if client_node:
            client_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples multi_play_linkcraft
[INFO] [1772526701.308025739] [robot_resource_client]: Found 1 resources
[INFO] [1772526701.308537895] [robot_resource_client]: Sending GroupControlTimeSync request:
[INFO] [1772526701.308692872] [robot_resource_client]:   slave ip: 172.22.133.178
[INFO] [1772526701.308853424] [robot_resource_client]:   slave ip: 172.22.135.45
[INFO] [1772526701.309014801] [robot_resource_client]: GroupControlTimeSync response:
[INFO] [1772526701.309114801] [robot_resource_client]:   error_code: 0
[INFO] [1772526701.309214801] [robot_resource_client]:   error_msg: ok
[INFO] [1772526701.308537895] [robot_resource_client]: Sending request:
[INFO] [1772526701.308692872] [robot_resource_client]:   resource_key: linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
[INFO] [1772526701.308853424] [robot_resource_client]:   resource_version: 0.0.1
[INFO] [1772526701.309014801] [robot_resource_client]:   meta: {"control_type": "GROUP_CONTROL","resource_type": "BODY_MONTION"}
[INFO] [1772526701.309114801] [robot_resource_client]:   slaves[0] ip: 172.22.133.178
[INFO] [1772526701.309214801] [robot_resource_client]:   slaves[1] ip: 172.22.135.45
[INFO] [1772526701.338307933] [robot_resource_client]: Service response received:
[INFO] [1772526701.338480169] [robot_resource_client]: Response Header:
[INFO] [1772526701.338647069] [robot_resource_client]:   stamp: 0.000000000
[INFO] [1772526701.338800125] [robot_resource_client]:   code: 0
[INFO] [1772526701.338951109] [robot_resource_client]: Status:
[INFO] [1772526701.339106459] [robot_resource_client]:   value: 1
[INFO] [1772526701.339239979] [robot_resource_client]:   message: 1/1 task(s) failed. Details: localhost: failed (status code: 400) - Failed with status code: 400

6.1.33 LED灯带控制

功能说明：演示如何控制机器人的LED灯带，支持多种显示模式和自定义颜色。

注意

使用本功能前，请在运控计算单元（PC1）上执行以下命令关闭 task_manager 模块，避免其与二开程序争抢灯带控制权：

aima em stop-app task_manager

核心代码：

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetPmuLed


class PlayLightsClient(Node):
    def __init__(self):
        super().__init__('play_lights_client')

        # create service client
        self.client = self.create_client(
            SetPmuLed, '/aimdk_5Fmsgs/srv/SetPmuLed')

        self.get_logger().info('✅ PlayLights client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, led_mode, r, g, b, priority=6, reset_priority=False):
        """Send LED control request"""
        # create request
        request = SetPmuLed.Request()
        request.led_strip_mode = led_mode
        request.r = r
        request.g = g
        request.b = b
        request.priority = priority
        request.reset_priority = reset_priority
        request.trace_id = 'play_lights'

        # send request
        # Note: LED strip is slow to response (up to ~5s)
        self.get_logger().info(
            f'📨 Sending request to control led strip: mode={led_mode}, RGB=({r}, {g}, {b}), priority={priority}, reset_priority={reset_priority}')
        for i in range(4):
            request.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(request)
            rclpy.spin_until_future_complete(self, future, timeout_sec=5)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if response.status_code == 0:
            self.get_logger().info('✅ LED strip command sent successfully.')
            return True
        else:
            # 0x1024: priority insufficient (priority < current threshold);
            # use a higher priority or set reset_priority=True to reset the
            # threshold to 0.
            reason = ('priority insufficient'
                      if response.status_code == 0x1024
                      else 'failure')
            self.get_logger().error(
                f'❌ LED strip command failed with status: '
                f'0x{response.status_code:X} ({reason})')
            return False


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

    try:
        # get command line args
        if len(sys.argv) > 6:
            # use CLI args
            led_mode = int(sys.argv[1])
            if led_mode not in (0, 1, 2, 3):
                raise ValueError("invalid mode")
            r = int(sys.argv[2])
            if r < 0 or r > 255:
                raise ValueError("invalid R value")
            g = int(sys.argv[3])
            if g < 0 or g > 255:
                raise ValueError("invalid G value")
            b = int(sys.argv[4])
            if b < 0 or b > 255:
                raise ValueError("invalid B value")
            priority = int(sys.argv[5])
            if priority < 0:
                raise ValueError("invalid priority value")
            reset_priority = sys.argv[6].lower() in ('true', '1', 'yes')
        else:
            # interactive input
            print("=== LED strip control example ===")
            print("Select LED strip mode:")
            print("0 - Steady on")
            print("1 - Breathing (4s cycle, sine brightness)")
            print("2 - Blinking (1s cycle, 0.5s on, 0.5s off)")
            print("3 - Flowing (2s cycle, light up from left to right)")

            led_mode = int(input("Enter mode (0-3): "))
            if led_mode not in (0, 1, 2, 3):
                raise ValueError("invalid mode")

            print("\nSet RGB color values (0-255):")
            r = int(input("Red (R): "))
            if r < 0 or r > 255:
                raise ValueError("invalid R value")
            g = int(input("Green (G): "))
            if g < 0 or g > 255:
                raise ValueError("invalid G value")
            b = int(input("Blue (B): "))
            if b < 0 or b > 255:
                raise ValueError("invalid B value")

            print("\nSet priority (higher value = higher priority, default=6):")
            priority = int(input("Priority: "))
            if priority < 0:
                raise ValueError("invalid priority value")

            reset_priority = input(
                "Reset priority after command? (y/n, default=n): ").strip().lower() in ('y', 'yes', 'true', '1')

        node = PlayLightsClient()
        node.send_request(led_mode, r, g, b, priority, reset_priority)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

# 构建
colcon build --packages-select py_examples

# 交互式运行
ros2 run py_examples play_lights

# 命令行参数运行: <模式> <R> <G> <B> <优先级> <重置优先级(true/false)>
ros2 run py_examples play_lights 1 255 0 0 6 false  # 模式1，红色，优先级6

输出示例：

=== LED strip control example ===
Select LED strip mode:
0 - Steady on
1 - Breathing (4s cycle, sine brightness)
2 - Blinking (1s cycle, 0.5s on, 0.5s off)
3 - Flowing (2s cycle, light up from left to right)
Enter mode (0-3): 1

Set RGB color values (0-255):
Red (R): 255
Green (G): 0
Blue (B): 0

Set priority (higher value = higher priority, default=6):
Priority: 6
Reset priority after command? (y/n, default=n): n

[INFO] [play_lights_client]: 📨 Sending request to control led strip: mode=1, RGB=(255, 0, 0), priority=6, reset_priority=False
[INFO] [play_lights_client]: ✅ LED strip command sent successfully.

技术特点：

• 支持4种LED显示模式

• RGB颜色自定义

• 优先级控制（值越大优先级越高）

• 优先级重置功能

• 异步服务调用

• 命令行参数支持

• 输入参数验证

• 友好的用户交互界面

6.1.34 GNSS定位数据订阅

该示例中用到了echo_gnss_data，通过订阅GNSS话题获取定位数据，输出经纬度、海拔和协方差信息。

功能特点：

• 订阅NavSatFix定位数据（经纬度、海拔）

• 实时显示定位状态和协方差

话题说明：

• /aima/hal/sensor/gnss: 定位数据 (sensor_msgs/NavSatFix)，由系统自动发布

#!/usr/bin/env python3
import time
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
from rclpy.time import Time
from sensor_msgs.msg import NavSatFix, NavSatStatus


STATUS_STR = {
    NavSatStatus.STATUS_NO_FIX: 'NO_FIX',
    NavSatStatus.STATUS_FIX: 'FIX',
}


class GnssPrinter(Node):
    NAVSAT_TOPIC = '/aima/hal/sensor/gnss'

    def __init__(self):
        super().__init__('gnss_printer')

        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            depth=10,
        )

        self._last_recv = None
        self.sub = self.create_subscription(
            NavSatFix, self.NAVSAT_TOPIC, self.cb, qos
        )
        self.get_logger().info(f'Subscribing NavSatFix: {self.NAVSAT_TOPIC}')

    def cb(self, msg: NavSatFix):
        now = time.perf_counter()
        dt_ms = 0.0 if self._last_recv is None else (
            now - self._last_recv) * 1000.0
        self._last_recv = now

        t_sec = Time.from_msg(msg.header.stamp).nanoseconds / 1e9
        status_str = STATUS_STR.get(msg.status.status, 'UNKNOWN')
        cov = msg.position_covariance

        self.get_logger().info(
            "\n========== NavSatFix ==========\n"
            f"stamp={t_sec:.6f}s\n"
            f"frame={msg.header.frame_id}\n"
            f"recv_dt={dt_ms:.3f}ms\n"
            f"latitude:  {msg.latitude:.8f} deg\n"
            f"longitude: {msg.longitude:.8f} deg\n"
            f"altitude:  {msg.altitude:.3f} m\n"
            f"status: {status_str}\n"
            f"service: {msg.status.service}\n"
            f"position_covariance: [{cov[0]:.4f}, {cov[1]:.4f}, {cov[2]:.4f}, "
            f"{cov[3]:.4f}, {cov[4]:.4f}, {cov[5]:.4f}, "
            f"{cov[6]:.4f}, {cov[7]:.4f}, {cov[8]:.4f}]\n"
            f"position_covariance_type: {msg.position_covariance_type}"
        )


def main():
    rclpy.init()
    node = GnssPrinter()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

# 构建
colcon build --packages-select py_examples

# 订阅GNSS数据
ros2 run py_examples echo_gnss_data

输出示例：

[INFO] [gnss_printer]: Subscribing NavSatFix: /aima/hal/sensor/gnss

========== NavSatFix ==========
stamp=1234567890.123456s
frame=gnss
recv_dt=1000.000ms
latitude:  31.84537000 deg
longitude: 117.19882000 deg
altitude:  30.000 m
status: FIX
service: 1
position_covariance: [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000]
position_covariance_type: 2

注意事项：

• 定位状态说明：NO_FIX(无定位)、FIX(单点定位)

• 室内环境下 status 通常为 NO_FIX，需到室外开阔处等待定位

6.1.35 上肢控制

备注

运行前需先切换到对应模式，详见 上肢控制。

#!/usr/bin/env python3
"""
Upper body control example.

Usage:
  ros2 run py_examples upper_body_control <mode>

  mode:
    head    — head center pose,          requires HEAD_ONLY mode
    claw    — claw half-open,            requires UPPERBODY_REMOTE_SPLIT mode
    joint   — dexterous hand half-open,  requires UPPERBODY_REMOTE_SPLIT mode
    gesture — dexterous hand gesture 1,  requires UPPERBODY_REMOTE_SPLIT mode

Switch to the desired MC mode before running, e.g. for head:
  ros2 run py_examples set_mc_action SD     # → STAND_DEFAULT
  ros2 run py_examples set_mc_action HO     # → HEAD_ONLY
or for claw / joint / gesture:
  ros2 run py_examples set_mc_action URS    # → UPPERBODY_REMOTE_SPLIT
"""

import sys
import rclpy
from rclpy.node import Node
from aimdk_msgs.msg import UpperBodyCommandArray, MessageHeader

# hand_sub_mode, head_pos, arm_pos, hand_pos
MODES = {
    # head_yaw=0, head_pitch=0 (center)
    'head':    (0, [0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                []),
    # left_open=0.5, right_open=0.5
    'claw':    (UpperBodyCommandArray.HAND_CLAW_OPEN_CLOSE,  # claw (1)
                [0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.5, 0.5]),
    # left 10 joints + right 10 joints, in rad, all half-open
    # (each joint at half its URDF range limit). Right is the mirror of left.
    # Joint order: [thumb_roll, thumb_abad, thumb_mcp, index_abad, index_pip,
    #               middle_pip, ring_abad, ring_pip, pinky_abad, pinky_pip]
    'joint':   (UpperBodyCommandArray.HAND_DEXTEROUS_JOINT,  # dexterous joint (2)
                [0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [-0.6, 0.8, -0.4, 0.1, 0.7, 0.7, -0.1, 0.7, -0.1, 0.7,
                 0.6, -0.8, 0.4, -0.1, 0.7, 0.7, 0.1, 0.7, 0.1, 0.7]),
    # left_gesture=1, left_open=1.0, right_gesture=1, right_open=1.0
    'gesture': (UpperBodyCommandArray.HAND_DEXTEROUS_GESTURE,  # dexterous gesture (3)
                [0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [1.0, 1.0, 1.0, 1.0]),
}


class UpperBodyControlNode(Node):
    def __init__(self, mode: str):
        super().__init__('upper_body_control')
        self.hand_sub_mode, self.head_pos, self.arm_pos, self.hand_pos = MODES[mode]
        self.pub = self.create_publisher(
            UpperBodyCommandArray, '/mc/upper_body_command', 10)
        self.timer = self.create_timer(0.02, self.publish)  # 50 Hz
        self._seq = 0
        self.get_logger().info(
            f'mode={mode}  hand_sub_mode={self.hand_sub_mode}')

    def publish(self):
        msg = UpperBodyCommandArray()
        now = self.get_clock().now()
        msg.header = MessageHeader()
        msg.header.stamp.sec = now.nanoseconds // 1_000_000_000
        msg.header.stamp.nanosec = now.nanoseconds % 1_000_000_000
        msg.header.frame_id = 'mc_upper_body'
        msg.header.sequence = self._seq
        self._seq += 1
        msg.source = 'upper_body_example'
        msg.hand_sub_mode = self.hand_sub_mode
        msg.head_pos = self.head_pos
        msg.arm_pos = self.arm_pos
        msg.hand_pos = self.hand_pos
        self.pub.publish(msg)


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

    argv = sys.argv[1:]
    if not argv or argv[0] not in MODES:
        print(f'Usage: upper_body_control <{"│".join(MODES)}>')
        rclpy.shutdown()
        return

    node = UpperBodyControlNode(argv[0])
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

colcon build --packages-select py_examples

# 头部控制（HEAD_ONLY 模式）
ros2 run py_examples upper_body_control head

# 夹爪控制（claw，UPPERBODY_REMOTE_SPLIT 模式）
ros2 run py_examples upper_body_control claw

# 灵巧手关节（UPPERBODY_REMOTE_SPLIT 模式）
ros2 run py_examples upper_body_control joint

# 灵巧手手势（UPPERBODY_REMOTE_SPLIT 模式）
ros2 run py_examples upper_body_control gesture

6.1.36 订阅 HDS 诊断信息 / 查询历史事件

订阅 /aima/hds/diag_code_list、/aima/hds/alert_code_list、/aima/hds/post_proc/signal 三个话题实时获取诊断码、告警码与后处理信号；同时支持调用 GetHdsEventHistory 服务查询历史事件。完整的诊断码清单见 诊断码参考表。

#!/usr/bin/env python3
"""
HDS diagnostics tool - two modes:

  echo       Subscribe to diag / alert / post_proc topics in real time.
  history    Query historical alert / diagnostic events via service.

See docs/cn/example/Python.md section "Subscribe HDS diagnostics /
query history events" for full usage and field descriptions.
"""

import argparse
import sys
import time

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy

import aimdk_msgs.msg as aimdk_msg
import aimdk_msgs.srv as aimdk_srv


DIAG_TYPE_NAMES = {
    aimdk_msg.DiagnosticType.NORMAL: "NORMAL",
    aimdk_msg.DiagnosticType.TRIGGER_APPEAR: "APPEAR",
    aimdk_msg.DiagnosticType.TRIGGER_DISAPPEAR: "DISAPPEAR",
}

STATE_NAMES = {
    aimdk_msg.HdsEventQuery.STATE_ALL: "ALL",
    aimdk_msg.HdsEventQuery.STATE_ACTIVE: "ACTIVE",
    aimdk_msg.HdsEventQuery.STATE_RECOVERED: "RECOVERED",
}


# ---------- echo mode ----------

class HdsDiagnosticsEcho(Node):
    """Subscribe to three HDS topics and print incoming messages."""

    def __init__(self):
        super().__init__('hds_diagnostics_echo')

        # HDS publishers use BEST_EFFORT; match it to receive messages.
        qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
        )

        self.create_subscription(
            aimdk_msg.DiagnosticInfoArray,
            '/aima/hds/diag_code_list',
            self.on_diagnostics, qos)

        self.create_subscription(
            aimdk_msg.AlertCodeArray,
            '/aima/hds/alert_code_list',
            self.on_alerts, qos)

        self.create_subscription(
            aimdk_msg.PostProcSigArray,
            '/aima/hds/post_proc/signal',
            self.on_post_proc, qos)

        self.get_logger().info(
            'Subscribed: diag_code_list / alert_code_list / post_proc/signal')

    def on_diagnostics(self, msg: aimdk_msg.DiagnosticInfoArray):
        if not msg.diagnostics:
            return
        lines = [f'[Diagnostics] count={len(msg.diagnostics)}']
        for d in msg.diagnostics:
            type_name = DIAG_TYPE_NAMES.get(
                d.type.value, f'UNKNOWN({d.type.value})')
            lines.append(
                f'  diag_code=0x{d.diag_code:010X}  type={type_name}  '
                f'module_id={d.module_id}  ts={d.timestamp}  info="{d.info}"')
        self.get_logger().info('\n'.join(lines))

    def on_alerts(self, msg: aimdk_msg.AlertCodeArray):
        if not msg.list:
            return
        lines = [f'[Alerts] count={len(msg.list)}']
        for a in msg.list:
            lines.append(
                f'  code="{a.code}"  ts={a.timestamp}  '
                f'first_ts={a.first_timestamp}')
        self.get_logger().info('\n'.join(lines))

    def on_post_proc(self, msg: aimdk_msg.PostProcSigArray):
        if not msg.post_proc_sig:
            return
        lines = [f'[PostProc] count={len(msg.post_proc_sig)}']
        for p in msg.post_proc_sig:
            s = p.sig_set
            lines.append(
                f'  diag_code=0x{p.diag_code:010X}  ts={p.timestamp}  '
                f'robot={s.robot_sig} motion={s.motion_sig} '
                f'sound={s.sound_sig} led={s.led_sig} facial={s.facial_sig} '
                f'funcdisable=0x{s.funcdisable_sig:016X}')
        self.get_logger().info('\n'.join(lines))


def run_echo(_args) -> int:
    node = HdsDiagnosticsEcho()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
    return 0


# ---------- history mode ----------

class HdsHistoryClient(Node):
    """Call the GetHdsEventHistory service to query historical events."""

    SERVICE_NAME = '/aimdk_5Fmsgs/srv/GetHdsEventHistory'

    def __init__(self):
        super().__init__('hds_history_client')
        self.client = self.create_client(
            aimdk_srv.GetHdsEventHistory, self.SERVICE_NAME)

    def call(self, query: aimdk_msg.HdsEventQuery, timeout_sec: float = 10.0):
        if not self.client.wait_for_service(timeout_sec=5.0):
            self.get_logger().error(
                f'Service {self.SERVICE_NAME} not available')
            return None

        request = aimdk_srv.GetHdsEventHistory.Request()
        now = self.get_clock().now()
        request.header.stamp.sec = now.nanoseconds // 1_000_000_000
        request.header.stamp.nanosec = now.nanoseconds % 1_000_000_000
        request.query = query

        future = self.client.call_async(request)
        start = time.time()
        while rclpy.ok() and time.time() - start < timeout_sec:
            rclpy.spin_once(self, timeout_sec=0.1)
            if future.done():
                return future.result()

        self.get_logger().error('Service call timeout')
        future.cancel()
        return None


def build_query(args) -> aimdk_msg.HdsEventQuery:
    q = aimdk_msg.HdsEventQuery()
    q.start_time = args.start_ns
    q.end_time = args.end_ns
    q.diag_codes = args.diag_codes
    q.alert_codes = args.alert_codes
    q.alert_locations = []
    q.state = args.state
    q.order_desc = args.desc
    q.offset = args.offset
    q.limit = args.limit
    return q


def print_records(response) -> None:
    print(f'total_count = {response.total_count}, '
          f'returned = {len(response.records)}, '
          f'has_more = {response.has_more}')
    if not response.records:
        return
    print('-' * 80)
    for r in response.records:
        recovered = 'YES' if r.recovered else 'NO'
        duration_s = r.duration_ns / 1e9
        print(f'id={r.id}  diag=0x{r.diag_code:010X}  '
              f'alert="{r.alert_code}"')
        print(f'  first_ts={r.first_timestamp}  '
              f'last_ts={r.last_timestamp}')
        print(f'  recovered={recovered}  '
              f'recover_ts={r.recover_timestamp}  '
              f'duration={duration_s:.3f}s')
        if r.alert_location:
            print(f'  location="{r.alert_location}"')
        print('-' * 80)


def run_history(args) -> int:
    node = HdsHistoryClient()
    try:
        query = build_query(args)
        node.get_logger().info(
            f'Querying history: state={STATE_NAMES.get(query.state)}, '
            f'offset={query.offset}, limit={query.limit}, '
            f'desc={query.order_desc}')

        response = node.call(query)
        if response is None:
            return 1
        print_records(response)
        return 0
    finally:
        node.destroy_node()


# ---------- entry point ----------

def parse_args():
    parser = argparse.ArgumentParser(
        description='HDS diagnostics tool - echo topics or query history')
    sub = parser.add_subparsers(dest='mode', required=True)

    sub.add_parser(
        'echo', help='subscribe diag/alert/post_proc topics and print live')

    h = sub.add_parser('history', help='query GetHdsEventHistory service')
    h.add_argument('--start-ns', type=int, default=0,
                   help='start timestamp (ns), 0 means unlimited')
    h.add_argument('--end-ns', type=int, default=0,
                   help='end timestamp (ns), 0 means unlimited')
    h.add_argument('--diag-codes', type=lambda s: int(s, 0), nargs='*',
                   default=[], help='filter diag codes (e.g. 0x6200000001)')
    h.add_argument('--alert-codes', nargs='*', default=[],
                   help='filter alert code strings')
    h.add_argument('--state', type=int, default=aimdk_msg.HdsEventQuery.STATE_ALL,
                   choices=[0, 1, 2],
                   help='0=ALL, 1=ACTIVE, 2=RECOVERED')
    h.add_argument('--offset', type=int, default=0)
    h.add_argument('--limit', type=int, default=20)
    h.add_argument('--desc', action='store_true',
                   help='order by time descending')

    return parser.parse_args()


def main() -> int:
    args = parse_args()
    rclpy.init()
    try:
        if args.mode == 'echo':
            return run_echo(args)
        if args.mode == 'history':
            return run_history(args)
        return 1
    finally:
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    sys.exit(main())

使用说明

实时订阅 (echo)：

# 直接订阅三个话题，实时打印诊断码、告警码、后处理信号
ros2 run py_examples hds_diagnostics echo

话题	数据类型
/aima/hds/diag_code_list	aimdk_msgs/msg/DiagnosticInfoArray
/aima/hds/alert_code_list	aimdk_msgs/msg/AlertCodeArray
/aima/hds/post_proc/signal	aimdk_msgs/msg/PostProcSigArray

查询历史事件 (history)，所有过滤条件均对应 HdsEventQuery 字段，使用示例：

# 查最近 20 条 (默认 limit=20，正序)
ros2 run py_examples hds_diagnostics history

# 按时间范围查询 (对应 start_time / end_time)
ros2 run py_examples hds_diagnostics history \
  --start-ns 1716000000000000000 \
  --end-ns   1716100000000000000

# 只看活跃中的告警 (对应 state = STATE_ACTIVE)
ros2 run py_examples hds_diagnostics history --state 1

# 按诊断码过滤 (对应 diag_codes，支持十六进制)
ros2 run py_examples hds_diagnostics history --diag-codes 0x6200000001 0x6200000002

# 按告警码过滤 (对应 alert_codes)
ros2 run py_examples hds_diagnostics history --alert-codes MOTOR_OVERHEAT COMM_LOST

# 分页 + 倒序 (对应 limit / offset / order_desc，最新的先显示)
ros2 run py_examples hds_diagnostics history --limit 50 --offset 0 --desc

# 组合使用：只看已恢复的，每次取 10 条，最新的先显示
ros2 run py_examples hds_diagnostics history --state 2 --limit 10 --desc

返回字段对应 HdsEventRecord。

echo 模式输出示例

[INFO] [hds_diagnostics_echo]: Subscribed: diag_code_list / alert_code_list / post_proc/signal
[INFO] [hds_diagnostics_echo]: [Diagnostics] count=1
  diag_code=0x6200000007  type=APPEAR  module_id=98  ts=1770125762202825297  info="获取不到机器人定位"
[INFO] [hds_diagnostics_echo]: [Alerts] count=1
  code="LOCALIZATION_LOST"  ts=1770125762202825297  first_ts=1770125762202825297
[INFO] [hds_diagnostics_echo]: [PostProc] count=1
  diag_code=0x6200000007  ts=1770125762202825297  robot=1 motion=2 sound=1 led=3 facial=0 funcdisable=0x0000000000000004

history 模式输出示例

[INFO] [hds_history_client]: Querying history: state=ALL, offset=0, limit=20, desc=True
total_count = 5, returned = 5, has_more = False
--------------------------------------------------------------------------------
id=42  diag=0x6200000007  alert="LOCALIZATION_LOST"
  first_ts=1770125762202825297  last_ts=1770125765310145000
  recovered=YES  recover_ts=1770125770000000000  duration=7.797s
--------------------------------------------------------------------------------

接口参考

• 话题：/aima/hds/diag_code_list、/aima/hds/alert_code_list、/aima/hds/post_proc/signal

• 服务：/aimdk_5Fmsgs/srv/GetHdsEventHistory

• 消息：aimdk_msgs/msg/DiagnosticInfoArray、aimdk_msgs/msg/AlertCodeArray、aimdk_msgs/msg/PostProcSigArray