6.1 Python接口使用示例

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

构建 & 运行说明

• 进入到 aimdk 目录，执行以下命令

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

📝 代码说明

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

小心

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

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) -> bool:
        """Send migration request synchronously"""
        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

        self.get_logger().info(
            f'Sending migration request to {self.target_state}...')

        try:
            # Use call() for synchronous call (requires ROS2 Foxy or newer)
            # For older versions, use call_async() with spin_until_future_complete
            future = self.migrate_client.call_async(request)

            # Spin until future completes
            start_time = time.time()
            while rclpy.ok() and time.time() - start_time < 10.0:
                rclpy.spin_once(self, timeout_sec=0.1)
                if future.done():
                    response = future.result()
                    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().error('Timeout waiting for migration response')
            return False

        except Exception as e:
            self.get_logger().error(f'Migration service call failed: {e}')
            return False

    def get_current_state_sync(self):
        """Get current system state synchronously"""
        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

        try:
            future = self.get_state_client.call_async(request)

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

            self.get_logger().error('Timeout getting system state')
            return None

        except Exception as e:
            self.get_logger().error(f'GetSystemState service call failed: {e}')
            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
            current_state_lower = response.cur_state.lower()
            target_state_lower = self.target_state.lower()

            state_match = (current_state_lower == target_state_lower)
            status_match = (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()
        rclpy.shutdown()

    return result


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

使用说明

ros2 run py_examples migrate_system_state Ready

输出示例

[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...
[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=False), Status=2 (match=False)
[INFO] [1770125665.733183333] [sync_system_state_migrator]: Check 2: State="Business" (match=False), Status=2 (match=False)
[INFO] [1770125666.736488851] [sync_system_state_migrator]: Check 3: State="Ready" (match=True), Status=1 (match=True)
[INFO] [1770125666.736982424] [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()

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, McAction, 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()
        # 解决source域缺失的问题，添加了默认的'node'作为source
        req.source = 'node'
        #
        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

        if response.response.status.value == CommonState.SUCCESS:
            self.get_logger().info('✅ Robot mode set successfully.')
        else:
            self.get_logger().error(
                f'❌ Failed to set robot mode: {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)'),
    }

    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
from aimdk_msgs.msg import McPresetMotion, McControlArea, RequestHeader, CommonState


class SetMcPresetMotionClient(Node):
    def __init__(self):
        super().__init__('preset_motion_client')
        self.client = self.create_client(
            SetMcPresetMotion, '/aimdk_5Fmsgs/srv/SetMcPresetMotion')
        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 send_request(self, area_id: int, motion_id: int) -> bool:
        request = SetMcPresetMotion.Request()
        request.header = RequestHeader()

        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
        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()
        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
import time


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=2)  # gripper mode
        msg.right_hand_type = HandType(value=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()
        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, MessageHeader
import time


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(
            0.8,
            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'
        msg.left_hand_type.value = 1      # NIMBLE_HANDS
        msg.right_hand_type.value = 1     # NIMBLE_HANDS

        # left hand
        msg.left_hands = [self.build_hand_cmd('') for _ in range(10)]
        msg.left_hands[0].name = 'left_thumb'
        for i in range(1, 10):
            msg.left_hands[i].name = 'left_index'

        # right hand
        msg.right_hands = [self.build_hand_cmd('') for _ in range(10)]
        msg.right_hands[0].name = 'right_thumb'
        for i in range(1, 10):
            msg.right_hands[i].name = 'right_pinky'

        if self.target_finger < 10:
            msg.right_hands[self.target_finger].position = 0.8
        else:
            target_finger_ = self.target_finger - 10
            target_position = 0.8
            if target_finger_ < 3:
                # The three thumb motors on the left hand need their signs inverted to mirror the right hand's motion
                target_position = -target_position
            msg.left_hands[target_finger_].position = target_position

        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()
        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
import time


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
        message.left_hand_type.value = 1  # NIMBLE_HANDS
        message.right_hand_type.value = 1  # NIMBLE_HANDS

        # Create left hand command array
        from aimdk_msgs.msg import HandCommand
        message.left_hands = []

        # Set left thumb
        left_thumb = HandCommand()
        left_thumb.name = "left_thumb"
        left_thumb.position = 0.0
        left_thumb.velocity = 0.1
        left_thumb.acceleration = 0.0
        left_thumb.deceleration = 0.0
        left_thumb.effort = 0.0
        message.left_hands.append(left_thumb)

        # Set other left fingers
        for i in range(1, 10):
            finger = HandCommand()
            finger.name = "left_index"
            finger.position = 0.0
            finger.velocity = 0.1
            finger.acceleration = 0.0
            finger.deceleration = 0.0
            finger.effort = 0.0
            message.left_hands.append(finger)

        # Create right hand command array
        message.right_hands = []

        # Set right thumb
        right_thumb = HandCommand()
        right_thumb.name = "right_thumb"
        right_thumb.position = 0.0
        right_thumb.velocity = 0.1
        right_thumb.acceleration = 0.0
        right_thumb.deceleration = 0.0
        right_thumb.effort = 0.0
        message.right_hands.append(right_thumb)

        # Set other right fingers (pinky)
        for i in range(1, 10):
            finger = HandCommand()
            finger.name = "right_pinky"
            finger.position = 0.0
            finger.velocity = 0.1
            finger.acceleration = 0.0
            finger.deceleration = 0.0
            finger.effort = 0.0
            message.right_hands.append(finger)

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

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


def main(args=None):
    rclpy.init(args=args)
    node = HandStateSubscriber()
    rclpy.spin(node)
    node.destroy_node()
    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


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. 1001 = register)
        req.action = McInputAction()
        req.action.value = 1001

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

        # Send request and wait for response
        self.get_logger().info(
            f'📨 Sending input source request: action_id={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(
                f'✅ 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] [get_current_input_source_client]: 当前输入源: node
[INFO] [get_current_input_source_client]: 优先级: 40
[INFO] [get_current_input_source_client]: 超时时间: 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, 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

        self.max_forward_speed = 1.0
        self.min_forward_speed = 0.2

        self.max_lateral_speed = 1.0
        self.min_lateral_speed = 0.2

        self.max_angular_speed = 1.0
        self.min_angular_speed = 0.1

        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 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...")

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

        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 to register input source... [{i}]")

        if future.done():
            try:
                response = future.result()
                state = response.response.state.value
                self.get_logger().info(
                    f"Input source set successfully: state={state}, task_id={response.response.task_id}")
                return True
            except Exception as e:
                self.get_logger().error(f"Service call exception: {str(e)}")
                return False
        else:
            self.get_logger().error("Service call failed or timed out")
            return False

    def publish_velocity(self):
        msg = McLocomotionVelocity()
        msg.header = MessageHeader()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.source = "node"
        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")
    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")
        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}")
        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")

    rclpy.spin(node)
    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. 添加新的控制逻辑：

   • 添加新的控制回调函数

   • 自由组合关节运动

3. 调整控制频率：

   • 修改control_timer_的周期（当前为2ms）

#!/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
from std_msgs.msg import Header
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 QoS: best-effort reliability, keep last 10 messages
subscriber_qos = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
    durability=DurabilityPolicy.VOLATILE
)

# Publisher QoS: reliable transport, keep last 10 messages
publisher_qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    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,
            subscriber_qos
        )
        self.pub = self.create_publisher(
            JointCommandArray,
            pub_topic,
            publisher_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, MessageHeader
from aimdk_msgs.srv import SetMcInputSource
import curses
import time


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 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...")

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

        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 to register input source... [{i}]")

        if future.done():
            try:
                resp = future.result()
                state = resp.response.state.value
                self.get_logger().info(
                    f"Input source set successfully: state={state}, task_id={resp.response.task_id}")
                return True
            except Exception as e:
                self.get_logger().error(f"Service exception: {str(e)}")
                return False
        else:
            self.get_logger().error("Service call failed or timed out")
            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"
        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)
    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图视频录制功能(TBD, 请参考C++示例)

• 可配置的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 time


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.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').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 == "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 is different with other cameras. The best_effort + volatile QoS is enough for 10Hz 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 is different with other cameras. The best_effort + volatile QoS is enough for 10Hz 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):
        """Video dump is only supported for RGB images"""
        # You can add video recording functionality here
        # Simplified in the Python version, only logs instead
        if self.should_print(master=False):
            self.get_logger().info(f"📝 Video dump not implemented in Python version")


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()
        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.avi
   

双目相机数据订阅

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

功能特点：

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

• 实时FPS统计和数据显示

• 支持RGB图视频录制功能(TBD, 请参考C++示例)

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

支持的数据类型：

• 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 time


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.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').value

        # 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):
        """Video dump is only supported for RGB images"""
        # You can add video recording functionality here
        # Simplified in the Python version, only logs instead
        if self.should_print(master=False):
            self.get_logger().info(f"📝 Video dump not implemented in Python version")


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()
        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.avi
   

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

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

功能特点：

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

• 实时FPS统计和数据显示

• 支持RGB图视频录制功能及把手遮挡区域mask处理(TBD, 请参考C++示例)

• 可配置的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
"""

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 time
import os
import cv2


class HeadRearCameraTopicEcho(Node):
    def __init__(self):
        super().__init__('head_rear_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('with_mask', False)

        self.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').value
        self.with_mask = self.get_parameter('with_mask').value
        self.mask_image = None

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

        if self.with_mask and self.dump_video_path:
            mask_path = os.path.join(os.path.dirname(
                __file__), 'data', 'rgb_head_rear_mask.png')
            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")

        # Create different subscribers based on topic_type
        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"
                self.get_logger().info(
                    f"📝 Will dump received images {mask_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"
            # 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 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 (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 (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 (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):
        """Video dump is only supported for RGB images"""
        # You can add video recording functionality here
        # Simplified in the Python version, only logs instead
        # Note: Refer to cpp implementation, get cv images by cv_bridge first,
        # then you can use 'image[self.mask_image == 0] = 0' to mask them and
        # finally use VideoWriter to save them as video
        if self.should_print(master=False):
            self.get_logger().info(f"📝 Video dump not implemented in Python version")


def main(args=None):
    rclpy.init(args=args)
    try:
        node = HeadRearCameraTopicEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        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.avi
   

应用场景：

• 人脸识别和追踪

• 目标检测和识别

• 视觉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",
            TouchState.SLIDE: "SLIDE",
            TouchState.PAT_ONCE: "PAT_ONCE",
            TouchState.PAT_TWICE: "PAT_TWICE",
            TouchState.PAT_TRIPLE: "PAT_TRIPLE"
        }

        # 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, f"INVALID({msg.event_type})")

        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()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

ros2 run py_examples echo_head_touch_sensor

输出示例：

[INFO] [1769420383.315173538] [touch_state_subscriber]: Timestamp: 1769420394.129927670, Event: IDLE (1)
[INFO] [1769420383.324978563] [touch_state_subscriber]: Timestamp: 1769420394.139941215, Event: IDLE (1)
[INFO] [1769420383.335265681] [touch_state_subscriber]: Timestamp: 1769420394.149990634, Event: TOUCH (2)
[INFO] [1769420383.344826732] [touch_state_subscriber]: Timestamp: 1769420394.159926892, 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
import time


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 = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

            # Format fields info
            fields_str = " ".join(
                [f"{f.name}({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:.6f}\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():1.1f}"
            )

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

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

            self.get_logger().info(
                f"🟢 LIDAR IMU received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\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()
        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] [lidar_chest_echo]: ✅ Subscribing LIDAR PointCloud2: /aima/hal/sensor/lidar_chest_front/lidar_pointcloud
[INFO] [lidar_chest_echo]: 🟢 LIDAR PointCloud2 received
  • frame_id:        lidar_chest_front
  • stamp (sec):     1234567890.123456
  • width x height:  1 x 36000
  • point_step:      16
  • row_step:        16
  • fields:          x(7) y(7) z(7) intensity(7)
  • is_bigendian:    False
  • is_dense:        True
  • data size:       576000
  • recv FPS (1s):   10.0

6.1.17 播放视频

该示例中用到了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, '/face_ui_proxy/play_video')
        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
        video_path = "/agibot/data/home/agi/zhiyuan.mp4"
        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.18 音频文件播放

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

功能特点：

• 支持多种音频格式播放（WAV（PCM编码）、RAW PCM）

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

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

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

技术实现：

• 使用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


class PlayAudioClient(Node):
    def __init__(self):
        super().__init__('PlayAudioClient')
        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, media_path):
        req = PlayAudioFile.Request()

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

        self.get_logger().info(
            f'📨 Sending request to play media: {media_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

            # 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 == 1:
            self.get_logger().info('✅ Request to play media file recorded successfully.')
            return True
        else:
            self.get_logger().error('❌ Failed to record play-media request.')
            return False


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

    default_media = '/agibot/data/var/interaction/tts_cache/normal/demo.wav'
    try:
        if len(sys.argv) > 1:
            media_path = sys.argv[1]
        else:
            media_path = input(
                f'Enter media file path to play (default: {default_media}): ').strip()
            if not media_path:
                media_path = default_media

        node = PlayAudioClient()
        node.send_request(media_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/interaction/tts_cache/normal/demo.wav

输出示例：

[INFO] [PlayAudioClient]: ✅ Request to play media file recorded successfully.

注意事项：

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

• 支持的文件格式：WAV（PCM编码）、RAW PCM

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

• 程序具有完善的异常处理机制

6.1.19 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
        req.tts_req.priority_level.value = 6

        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('❌ Failed to send TTS.')
            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] [play_tts_client_min]: ✅ 播报请求成功

注意事项

• 确保TTS服务正常运行

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

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

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

接口参考

• 服务：/aimdk_5Fmsgs/srv/PlayTts

• 消息：aimdk_msgs/srv/PlayTts

6.1.20 麦克风数据接收

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

功能特点：

• 支持多音频流同时接收（内置麦克风 stream_id=1，外置麦克风 stream_id=2）

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

• 自动保存完整语音片段为PCM格式文件

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

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

• 智能缓冲区管理，避免内存泄漏

• 完善的错误处理和异常管理

• 详细的调试日志输出

VAD状态说明：

• 0: 无语音

• 1: 语音开始

• 2: 语音处理中

• 3: 语音结束

技术实现：

• 支持PCM格式音频文件保存（16kHz, 16位, 单声道）

• 提供详细的日志输出和状态监控

• 支持实时音频流处理和文件保存

应用场景：

• 语音识别和语音处理

• 音频数据采集和分析

• 实时语音监控

• 音频质量检测

• 多麦克风阵列数据处理

#!/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:
- 0: No speech
- 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
import time
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 = {
            0: "No speech",
            1: "Speech start",
            2: "Speech in progress",
            3: "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 == 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
        elif vad_state == 2:
            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 == 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

        # No speech
        elif vad_state == 0:
            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 to 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()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明：

1. 运行程序：

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

2. 目录结构：

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

   • 音频文件按stream_id分类存储：

     • stream_1/: 内置麦克风音频

     • stream_2/: 外置麦克风音频

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

   • internal_mic_20250909_133649_738.pcm (内置麦克风)

   • external_mic_20250909_133650_123.pcm (外置麦克风)

4. 音频格式：16kHz, 16位, 单声道PCM

5. 播放保存的PCM文件：

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

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

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

7. 程序控制：

   • 按 Ctrl+C 安全退出程序

   • 程序会自动处理音频流的开始和结束

   • 支持同时处理多个音频流

输出示例：

正常启动和运行：

[INFO] 开始订阅降噪音频数据...
[INFO] 收到音频数据: stream_id=1, vad_state=0(无语音), audio_size=0 bytes
[INFO] [内置麦克风] VAD状态: 无语音 音频数据: 0 bytes
[INFO] 收到音频数据: stream_id=2, vad_state=0(无语音), audio_size=0 bytes
[INFO] [外置麦克风] VAD状态: 无语音 音频数据: 0 bytes

检测到语音开始：

[INFO] 收到音频数据: stream_id=2, vad_state=1(语音开始), audio_size=320 bytes
[INFO] [外置麦克风] VAD状态: 语音开始 音频数据: 320 bytes
[INFO] 🎤 检测到语音开始

语音处理过程：

[INFO] 收到音频数据: stream_id=2, vad_state=2(语音处理中), audio_size=320 bytes
[INFO] [外置麦克风] VAD状态: 语音处理中 音频数据: 320 bytes
[INFO] 🔄 语音处理中...
[INFO] 收到音频数据: stream_id=2, vad_state=2(语音处理中), audio_size=320 bytes
[INFO] [外置麦克风] VAD状态: 语音处理中 音频数据: 320 bytes
[INFO] 🔄 语音处理中...

语音结束和保存：

[INFO] 收到音频数据: stream_id=2, vad_state=3(语音结束), audio_size=320 bytes
[INFO] [外置麦克风] VAD状态: 语音结束 音频数据: 320 bytes
[INFO] ✅ 语音结束
[INFO] 音频段已保存: audio_recordings/stream_2/external_mic_20250909_133649_738.pcm (大小: 960 bytes)
[INFO] 音频时长: 0.06 秒 (480 样本)

多音频流同时处理：

[INFO] 收到音频数据: stream_id=1, vad_state=1(语音开始), audio_size=320 bytes
[INFO] [内置麦克风] VAD状态: 语音开始 音频数据: 320 bytes
[INFO] 🎤 检测到语音开始
[INFO] 收到音频数据: stream_id=2, vad_state=1(语音开始), audio_size=320 bytes
[INFO] [外置麦克风] VAD状态: 语音开始 音频数据: 320 bytes
[INFO] 🎤 检测到语音开始

程序退出：

^C[INFO] 接收到中断信号，正在退出...
[INFO] 程序已安全退出

注意事项：

• 程序支持同时处理多个音频流（内置和外置麦克风）

• 每个音频流都有独立的缓冲区和录音状态

• 音频文件会自动按时间戳和音频流分类保存

• 程序具有完善的错误处理机制，确保稳定运行

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

#!/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(
                "MIC channels info changed (mic:{self.mic_channels} ref:{self.ref_channels}) -> (mic:{msg.mic_channels} ref:{msg.ref_channels})")
            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("🔄 Flashing 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} append: {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. 目录结构：

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

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

     • 20250909_133650_123

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

   • channel_0_mic.pcm (麦克风)

   • channel_4_ref.pcm (回采)

4. 音频格式：16kHz, 16位, 单声道PCM

5. 播放保存的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.22 获取当前麦克风

#!/usr/bin/env python3

import sys
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(
            f'📨 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:
            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.23 切换麦克风

#!/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:
            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.24 原始流式音频播放

#!/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")
    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.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 = 6

        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:
            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

        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:
            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 foucs 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_untill_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_untill_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为原始音频文件的路径, 单通道1, 采样率16kHz, 格式S16LE 
ros2 run py_examples play_audio_stream --ros-args -p raw_audio_path:=/path/to/your/raw_audio_file.pcm

6.1.25 表情控制

该示例中用到了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, '/face_ui_proxy/play_emoji')
        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.26 地图构建

该示例中用到了slam，通过该节点给建图服务发送消息，实现建图功能

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


class SlamCommandPublisher(Node):
    def __init__(self):
        super().__init__('slam_command_publisher')
        self.publisher_ = self.create_publisher(
            String, '/integrated_command', 10)

    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()
        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.27 使用地图进行重定位

该示例中用到地图构建步骤完成构建的地图进行定位

#!/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
import time


class RelocalizationNode(Node):
    def __init__(self):
        super().__init__('relocalization_node')

        # Create publishers
        self.integrated_command_pub = self.create_publisher(
            String, '/integrated_command', 10)
        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_loc',
            self.odometry_callback,
            qos_profile=lidar_loc_qos
        )

        # Setup timer to publish messages in sequence
        self.success_received = False
        self.timeout_timer = None

        # Start the sequence
        self.publish_sequence()

    def publish_sequence(self):
        # Publish integrated_command
        integrated_command_msg = String()
        integrated_command_msg.data = 'start_relocalization:1758012592981'
        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 = 0.0
        relocalization_pose_msg.position.y = 0.0
        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 (5 seconds)
        self.timeout_timer = self.create_timer(
            5.0,
            self.timeout_callback
        )

        self.get_logger().info('Waiting for robot pose data (timeout: 5s)...')

    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!')
            rclpy.shutdown()

    def timeout_callback(self):
        if not self.success_received:
            self.get_logger().error('Timeout reached - Relocalization failed!')
            rclpy.shutdown()


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


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples relocate 
[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: 5s)...
[INFO] [1772526173.347455156] [relocalization_node]: Received odometry data - Relocalization successful!

需要将机器人移动到地图构建的起始点，然后运行重定位程序

6.1.28 获取已存储的地图

该示例中用到了get_map，按地图名称获取机器人上已存储的地图数据，包括地图路径、分辨率、宽高及地图ID等信息。

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
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)

        rclpy.spin_until_future_complete(self, future)

        if future.result() is None:
            self.get_logger().error("Service call failed.")
            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()
    client.send_request("test_map")

    rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples get_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: 1

示例默认请求名称为 test_map 的地图，可在代码中修改 send_request 的参数以请求其他地图。

6.1.29 播放灵创动作

该示例中用到了play_linkcraft，列出机器人上的灵创动作，并且选择一条进行播放

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from aimdk_msgs.srv import GetRobotResources, ExecuteActionResource
import time


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'
        )

    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
            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 not None:
            response = future.result()
            self.get_logger().info("Service call succeeded")

            self.get_logger().info(
                f"Robot Resources ({len(response.robot_resources)}):")
            for resource in response.robot_resources:
                self.get_logger().info(
                    f"  Resource Key: {resource.resource_key}")
                self.get_logger().info("  Current Version:")
                self.get_logger().info(
                    f"    version: {resource.current_version.version}")
                self.get_logger().info(
                    f"    name: {resource.current_version.name}")
                self.get_logger().info(
                    f"    files ({len(resource.current_version.files)}):")
                for file in resource.current_version.files:
                    self.get_logger().info(f"      {file}")
        else:
            self.get_logger().error("Failed to call service")

    def play_robot_resources(self):
        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 = "linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA"
        request.resource_version = "0.0.1"

        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:
            response = future.result()
            self.handle_response(response)
        else:
            self.get_logger().error("Failed to call service")

    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 = RobotResourceClient()
    client_node.get_robot_resources()
    client_node.play_robot_resources()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

使用说明:

ros2 run py_examples play_linkcraft
[INFO] [1772526701.308025739] [robot_resource_client]: Service call succeeded
[INFO] [1772526701.308253238] [robot_resource_client]: Robot Resources (0):
[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.30 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:
            self.get_logger().error(
                f'❌ LED strip command failed with status: {response.status_code}')
            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() == 'y'

        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颜色自定义

• 优先级控制（值越大优先级越高）

• 优先级重置功能

• 同步服务调用

• 命令行参数支持

• 输入参数验证

• 友好的用户交互界面